ICP算法和NDT算法

1、ICP算法、ICP算法概述Algorithm 1 Summary of ICP algorithm. readingref eren.ee initial transformatiojidata filterst datEi filterst move reading associate points filter outliersRequire: KVRequire:上。Require:气同峋，datafiltcr(AP)BQy - datafiltcrfQ) TinitrepeatMi matchf, Q)Wi J outlier (Al j弋丁 - aigmin (error(T (5)

2、 , Q。)until convergenceEnsure:=(O i-T、转换矩阵T-0T 1其中，R是旋转矩阵，t是转换向量。三、滤波器(Data Filters)滤波器的目的是为了增加差异性，减少处理时间，去噪。四、匹配度(match function)匹配度，是测量两个点集的关联度的，又称之为association solver。比如，用欧式距离来 测量两个点集的关联度，定义如下：M = matclP, Q) - | (丽血)：Vpn e 已 qm arg min(d(?ri1 史)with dp. q) = |p-q|3.五、离群滤波器(outlier filters)离群滤波器主要

3、是降低匹配带来的误差。yMUQJZu-SUE图ICP算法流程图表ICP算法模块的实现方法Data filteringFixStepSampling,MaxDensity,MaxPointCount, MaxQuantileOnAxis,MinDist , ObservationDirection , OrientNormals, RandomSampling,RemoveNaN ,SamplingSurfaceNormal,Shadow , SimpleSensorNoise , SurfaceNormalData associationKDTree , KDTreeVarDistOutlie

4、r filteringMaxDist , MedianDist , MinDist , SurfaceNormal , TrimmedDist , VarTrimmedDistError minimizationPointToPlane , PointToPointTransformation checkingBound , Counter , DifferentialRandom SamplingFigure: Modules compnsing the default QP chainReference ReadingMatcherOutlier FiltersMinimizerKD Tr

5、奕MatcherPoint to PlaneTransformationCheckersData FiltersRandom SamplirgCounterDifferentialTrarrsfornnationTrimmedDistanceNDT算法一、NDT算法概述ICP算法存在两个重要问题：一是点与点的匹配没有考虑到点与周围平面的联系;二是算 法中用到的最邻近搜索计算量比较大。二、NDT算法具体步骤将空间(reference scan)划分成各个格子cell将点云投票到各个格子计算格子的正态分布PDF参数k=l = 洁T立嵌网天-回T，底3）wheir V&= ftl are the p

6、ositions of rhe reference scan points contained in rhe cell,将第二幅scan的每个点按转移矩阵T的变换第二幅scan的点落于reference的哪个格子，计算响应的概率分布函数求所有点的最优值，目标函数为IW - JJp（T0 靠） 4=1PDF可以当做表面的近似表达，协方差矩阵的特征向量和特征值可以表达表面信息（朝 向、平整度）格子内少于3个点，经常会协方差矩阵不存在逆矩阵，所以只计算点数大于5的cell， 涉及到下采样方法。、2D-NDT 算法Fig. L An example of the NDT: The original l

7、aser scan and the resulting probability density.Collect all 2D-Points 妊 contained in this box.Calculate the mean q_ *Calculate the covariance matrixS =-q)(xi -q)匕The probability of measuring a sample at 2D-point x con tai ned in this cell is now modeled by the normal distribution E):I ( (x-q)F-i(x_q

8、)p(x)exp(),(1)2D-NDT算法是在2维平面上描述连续，可微的概率密度。具体步骤如下：Build the NDT of the first scan.Initialize the estimate for the parameters (by zero or by using odometry data).For each sample of the second scan: Map the reconstructed 2D point into the coordinate frame of the first scan according to lhe parameters.D

9、etermine the corresponding normal distributions for each mapped point.The score for the parameters is detennined by I evaluating the distribution for each mapped point and summing the result-Calculate a new parameter estimate by trying to optimize the score. This is done by performing one step of Ne

10、wtotfs Algorithm,Goto 3 until a convergence criterion is met.参考文献：P Besl and H McKay, 1992. A method for registration of 3-D shapes. Pattern Analysis and Machine Intelligence, IEEE Transactions on, 14, pp. 239 - 256.A Review of Point Cloud Registration Algorithms for Mobile Robotics.Comparing ICP variants on real-world data sets.Peter Biber and Wolfgang StraRer. The normal distributions transform: A new approach