ICP算法和NDT算法

ICP 算法 一、ICP 算法概述 二、转换矩阵 T 其中，R 是旋转矩阵，t 是转换向量。 三、滤波器(Data Filters) 滤波器的目的是为了增加差异性，减少处理时间，去噪。 四、匹配度（match function） 匹配度,是测量两个点集的关联度的，又称之为 association solver。比如，用欧式距离来 测量两个点集的关联度，定义如下： 五、离群滤波器(outlier filters) 离群滤波器主要是降低匹配带来的误差。 表 ICP 算法模块的实现方法 Data filtering FixStepSampling , MaxDensity , MaxPointCount , MaxQuantileOnAxis , MinDist , ObservationDirection , OrientNormals , RandomSampling , RemoveNaN , SamplingSurfaceNormal , Shadow , SimpleSensorNoise , SurfaceNormal Data association KDTree , KDTreeVarDist Outlier filtering MaxDist , MedianDist , MinDist , SurfaceNormal , TrimmedDist , VarTrimmedDist Error minimization PointToPlane , PointToPoint Transation checking Bound , Counter , Differential 图 ICP 算法流程图 NDT 算法 一、NDT 算法概述 ICP 算法存在两个重要问题： 一是点与点的匹配没有考虑到点与周围平面的联系;二是算 法中用到的最邻近搜索计算量比较大。 二、NDT 算法具体步骤 将空间（reference scan）划分成各个格子 cell 将点云投票到各个格子 计算格子的正态分布 PDF 参数 将第二幅 scan 的每个点按转移矩阵 T 的变换 第二幅 scan 的点落于 reference 的哪个格子，计算响应的概率分布函数 求所有点的最优值，目标函数为 PDF 可以当做表面的近似表达，协方差矩阵的特征向量和特征值可以表达表面信息（朝 向、平整度） 格子内少于 3 个点，经常会协方差矩阵不存在逆矩阵，所以只计算点数大于 5 的 cell， 涉及到下采样方法。 二、2D-NDT 算法 2D-NDT 算法是在 2 维平面上描述连续，可微的概率密度。具体步骤如下： 参考文献： [1]P Besl and H McKay, 1992. A for registration of 3-D shapes. Pattern Analysis and Machine Intelligence, IEEE Transactions on, 14, pp. 239 - 256. [2]A Review of Point Cloud Registration Algorithms for Mobile Robotics. [3]Comparing ICP variants on real-world data sets. [4]Peter Biber and Wolfgang Straßer. The normal distributions trans: A new approach to laser scan matching. In Proceedings of the IEEE International Conference on Intelligent Robots and Systems (IROS), pages 2743? 2748, Las Vegas, USA, October 2003.