对点云匹配算法ICP 、PL-ICP 、NICP 和IMLS-ICP 的理解
点云匹配算法是为了匹配两帧点云数据，从⽽得到传感器（激光雷达或摄像头）前后的位姿差，即⾥程数据。匹配算法已经从最初的ICP⽅法发展出了多种改进的算法。他们分别从配准点的寻，误差⽅程等等⽅⾯进⾏了优化。下⾯分别介绍： ICP
ICP的基本思想是：
给定两个点云集合
其中 和表⽰点云坐标，和表⽰点云的数量。
在实际⼯程中不可能知道两个点云的点是如何配对的。只能通过迭代求解的⽅法⼀步步缩⼩误差，最终得到使误差⽅程最⼩的旋转矩阵R和平移矩阵t。
算法流程：
1）寻对应点
通常使⽤编码盘的⾥程计数据得到位姿差，即当前机器⼈在上次机器⼈坐标系中的位姿。将此R和t作为ICP算法的first guess，帮助算法寻点云对应点。这⾥需注意，如果激光传感器没有安装在机器⼈坐标系中⼼，则存在⾥程计得到的位姿到激光传感器位姿的坐标转换关系。如下图中
所⽰：
2）根据对应点计算R和t。
这⼀步就是根据好的对应点构建误差⽅程。普通的ICP 是使⽤点到点的距离作为误差的。误差⽅程如下：
然后求解出R和t。具体的求解推导如下：
X =x ,x ,⋯,x {12N x }
P =p ,p ,⋯,p {12N p }
x i p i N x N p E (R ,t )=x −Rp −t N p 1∑i =1N p ∥i i ∥2
l E (R ,t )=x −Rp −t N p 1∑i =1N p ∥i i ∥2
其中
⽽项累加次后会等于0。
最终得到的等式中只有项与有关。
可以先令等于0，则只需保证项最⼩即可。求出后带⼊求解出t。所以误差⽅程可以简化成下式：因为项与矩阵没有关系，所以可以不考虑。由于为正交矩阵，则，进⽽。可以看到该项与也没有什么关系，
同样不考虑它。最后就只剩下⼀项与相关了。
要求最⼩即最⼤化下⾯的式⼦：
其中利⽤定理，假设矩阵A为正定对称矩阵，则对于任意的正交矩阵B，都有
对进⾏SVD分解令，则为正交矩阵。
得到正定对称矩阵。
则：
E (R ,t )E (R ,t )===x −Rp −t N p 1i =1∑∥i i ∥
2=x −Rp −t −u +Ru +u −Ru N p 1i =1∑N p ∥i i x p x p ∥
2x −u −R p −u +u −Ru −t N p 1i =1∑N p ∥i x (i p )(x p )∥
2x −u −R p −u +u −Ru −t N p 1i =1∑N p ∥i x (i p )∥2∥x p ∥
2+2x −u −R p −u u −Ru −t (i x (i p ))T
(x p )=x −u −R p −u +u −Ru −t N p 1i =1∑N p ∥i x (i p )∥2∥x p ∥2u =x x u =N p 1∑i =1N p i p p N p 1∑i =1N p
i x −u −R p −u (i x (i p ))N p x −u −R p −u ∥i x (i p )∥2
R u −Ru −t ∥x p ∥2x −u −R p −u ∥i x (i p )∥2R u −Ru −t =∥x p ∥20min E (R ,t )=
x −u −R p −u N p 1∑i =1N p ∥i x (i p )∥2=x −Rp =N p 1∑i =1N p ∥i
′i ′∥2x x +N p 1∑i =1N p i T i ′p R Rp −i T T i ′2x Rp i T i ′=−2x Rp ∑i =1N p i ′T i
′x x i T i ′R R R R =T I p R Rp =i T T i ′p p i T i ′R R max x Rp =∑i =1N p i ′T i
′Trace Rx p =∑i =1N p (i ′i T )Trace(RH )H =x p ∑i =1N p i ′i ′T
Trace(A )≥Trace(BA )
H H =U ΛV T
X =V U T X XH =V U U ΛV =T T V ΛV T
B是任意的正交矩阵，X也是⼀个正交矩阵，因此BX可以取遍所有的正交矩阵。所以BX也包括了需要求解的旋转矩阵T，因此
当R=X时，等式成⽴。因此：
则3）将计算得到的R和t⽤于下⼀次迭代计算，直到误差值⼩于设定的阈值。
这⾥指出ICP的⼀个明显缺陷：
两帧激光点云数据中的点不可能表⽰的是空间中相同的位置。所以⽤点到点的距离作为误差⽅程势必会引⼊随机误差。
参考论⽂：Least-Squares Fitting of Two 3-D Point Sets
PL-ICP
PL-ICP相对于PP-ICP最⼤的区别是其改进了误差⽅程。PP-ICP是点对点的距离作为误差⽽PL-ICP是采⽤点到其最近两个点连线的距离。
下图展⽰了误差⽅程的差异。
从上⽅的(a)中可以看出，激光点是对实际环境中曲⾯的离散采样。最好的误差尺度是激光点到实际曲⾯的距离。⽽PL-ICP采⽤分段线性的⽅法对实际曲⾯进⾏近似，⽤激光点到最近两点连线的距离来模拟实际激光点到曲⾯的距离。可以看出PL-ICP的误差⽅程更贴近实际情况。
Tr ace(XH )≥Tr ace(BXH )
Trace(RH )≤Trace(XH )
R =X =V U T
t =u −x Ru p
算法流程：
先贴⼀张论⽂⾥的算法步骤
1）给定⼀个初始的转换矩阵，将当前激光帧的数据转换到参考帧坐标系下。初始的转换矩阵⼀般通过⾥程计来获得。后⾯迭代计算所需的由上⼀次算法迭代计算得到。
2）为当前激光帧中的每⼀个点，到其最近的两个点j1和j2。
3）去除误差过⼤的点。
4）构建最⼩化误差⽅程。
5）求解出位姿转换矩阵。然后将其⽤于下次迭代计算。
NICP
NICP⽅法与ICP⽅法的主要流程和思想是⼀致的。但是它在Trim outlier和误差项⾥考虑了更多的因素。这也是它效果更好的原因。它充分利⽤实际曲⾯的特征来对错误点进⾏了滤除，主要使⽤的特征为法向量和曲率。在误差项⾥除了考虑了点到对应点切⾯的距离，还考虑了对应点法向量的⾓度差。⽬前NICP⽅法开源的代码主要是针对3D点云的，其调⽤了Eigen库和OpenCV库。源码中显⽰的部分调⽤了QT5。既然NICP⽅法考虑了法向量和曲率，那么就涉及到了如何求解点的法向量和曲率。下⾯简述论⽂中的⽅法：
1）⾼斯拟合。到点周围半径范围内的所有点。求解均值和协⽅差。q 0q 0q k q k +1p i R V i μi s Σi s =p V ∣i ∣1p ∈V j i ∑i
=p −μp −μV ∣i ∣1p ∈V j i ∑(i i )T (i i )
2）对协⽅差矩阵进⾏SVD分解，得到按从⼩到⼤顺序的特征值。论⽂中曲率的定义为：
法向量的定义：最⼩特征值对应的特征向量。
Trim outlier中的改进：
1）如果没有well define的法向量，则拒绝。即选择⽐较结构化的点，如果对应点周围过于杂乱就丢弃该点。
2）两点间的距离⼤于阈值，则拒绝。
3）两点的曲率之差距⼤于阈值，则拒绝。
4）两点的法向量⾓度之差⼤于阈值，则拒绝。
误差项中的改进：1）点到对应点切⾯的距离作为其中的⼀个误差项。
2）法向量的夹⾓作为另⼀个误差项。考虑该项误差增加了旋转矩阵的求解精度。
最后得到的误差定义为：λ1:3Σ=i s R R
⎝⎛λ1000λ2000
λ3⎠⎞T σ=i λ+λ12
λ1p −T ⊕p >∥∥i c j r ∥∥ϵd
log σ−log σ>∣∣i c j r ∣∣ϵσ
n ⋅i c T ⊕n <j r
ϵn
R
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议