ICP配准算法

本篇将介绍配准算法，将介绍ICP(point to point)、ICP(point to plane)和NDT算法。其中ICP有两种，point to point表示通过构建点与点的对应关系完成代价损失的最优化，point to plane表示通过构建点到面的对应关系完成代价损失的最优化。

问题定义

配准算法就是通过迭代完成两个点集的旋转矩阵$R$和平移矩阵$T$的迭代求解。数学定义如下:
给定源点集 P , P = { p 1 , p 2 , . . . , p n } , p i ∈ R m P,P=\{p_1,p_2,...,p_n\},p_i\in R_m P,P={p1​,p2​,...,pn​},pi​∈Rm​,和目标点击 Q = { q 1 , q 2 , . . . , q n } , q i ∈ R m Q=\{q_1,q_2,...,q_n\},q_i\in R_m Q={q1​,q2​,...,qn​},qi​∈Rm​。两个点集之间存在着旋转$R$和平移$t$的转化关系，构建如下损失函数(点与点在某个尺度上的距离)，$$E(R,t)=\frac{1}{n}\sum _{i=1}^n{\Vert q_i-R{p}_i-t\Vert }^2$$最优化损失函数$$\begin{array}{rl}R,t& ={\arg }_{R,t}\min E(R,t)\\ & ={\arg }_{R,t}\min \frac{1}{n}\sum _{i=1}^n{\Vert q_i-R{p}_i-t\Vert }^2\\ & ={\Vert Q-\left(RP+t{1}^T\right)\Vert }_F^2,\text{ s.t. }R{R}^T=I_m\end{array}$$其中，$$\begin{gathered} P=[p_1,...,p_n]\in R_{m\times n},p_i\in R_m \\ Q=[q_1,...,q_n]\in R_{m\times n},q_i\in R_m \\ 1=[1,1,...,1{]}^T\in R_n \end{gathered}$$得，也是Procrustes Transformation的求解过程 μ p = 1 n ∑ i = 1 n p i , μ 1 = 1 n ∑ i = 1 n q i P ′ = { p i − μ p } , Q ′ = { q i − μ q } O ′ P ′ T = U Σ V T R = U V T , t = μ q − R μ q \mu_p = {1 \over n} \sum_{i=1}^np_i, \mu_1 = {1 \over n} \sum_{i=1}^nq_i \\ P^{'} =\{ p_i - \mu_p \}, Q^{'}=\{q_i- \mu_q \} \\ O^{'}P^{'T} = U \Sigma V^T \\ R = U V^T, t = \mu_q-R\mu_q μp​=n1​i=1∑n​pi​,μ1​=n1​i=1∑n​qi​P′={pi​−μp​},Q′={qi​−μq​}O′P′T=UΣVTR=UVT,t=μq​−Rμq​其中$R,t$即为集合P与Q之间的旋转平移，不过需要注意的是，这是迭代解，求出的只是当前集合状态的最优解。并不是最终解。

ICP(point to point)算法思想步骤分解

以ICP算法为例子，配准算法分为以下步骤:

1. 给定集合P和Q，从中提取部分点(记为集合$subP，subQ$)进行求解，可以使用随机采样，体素降采样，NSS采样和特征提取(比如ISS提取特征点)
2. 选定匹配的若干点对，对于选出集合$subP$中的每个点$p_i$,在集合$subQ$中找到邻居点。邻居点的尺度可以使用欧式空间最邻近思想，也可以是特征描述子空间的距离(对应ISS算法提取的特征点的特征描述子)，也可以选用Normal shooting思想和Projection思想。要求双方互为最近邻居，并且为了算法的精度，可以仅仅使用点对距离超过某值的点对，或者选用相对距离前百分之k的点对。
   前两个都容易理解，后两个的直观表达如下:
   
   
3. 构建point to point类型的优化函数进行迭代，完成当前迭代的旋转平移矩阵求解。
   $$\begin{array}{rl}R,t& ={\arg }_{R,t}\min E(R,t)\\ & ={\arg }_{R,t}\min \frac{1}{n}\sum _{i=1}^n{\Vert q_i-R{p}_i-t\Vert }^2\\ & ={\Vert Q-\left(RP+t{1}^T\right)\Vert }_F^2,\text{ s.t. }R{R}^T=I_m\end{array}$$4. 判断旋转平移矩阵的更新量是否已经满足阈值，如果是满足就终止，输出最后的结果。如果没有，就迭代到最大次数。

point to point和point to plane的区别

• 算法思想的不同
  point to point和point to plane类型的ICP算法之间的优化函数构建思想不同。

  • 在点对点配准中，算法通过最小化两个点云之间的点与点之间的距离来实现配准。这意味着算法试图将一个点云中的每个点与另一个点云中的最近邻点对齐，然后通过优化来最小化它们之间的距离。
  • 点对点配准适用于两个点云表面之间存在较小的形变，并且点云中的噪声较少的情况。
  • 点对点匹配只是寻找了匹配点，点邻域内的信息没有使用，没有考虑上下文，并且最近邻查找的时候时间消耗大。
  • 在点对面配准中，不仅考虑了点的位置，还考虑了点的法线（即表面的朝向），从而更好地描述了表面的几何特征。
  • 点对面配准试图最小化一个点到另一个点云表面的法线方向上的投影之间的距离，而不仅仅是点之间的距离。这意味着算法不仅考虑了点的位置，还考虑了点云表面的曲率和法线方向，因此能够更好地处理曲面之间的配准。
  • 点对面配准通常比点对点配准更稳健，因为它们对点云中的噪声和表面曲率变化更具有鲁棒性

• 优化函数求解不同
  point to point的优化函数形式为:
  $$\begin{array}{rl}R,t& ={\arg }_{R,t}\min E(R,t)\\ & ={\arg }_{R,t}\min \frac{1}{n}\sum _{i=1}^n{\Vert q_i-R{p}_i-t\Vert }^2\end{array}$$point to plane的优化函数形式为:$$\begin{array}{rl}R,t& ={\arg }_{R,t}\min E(R,t)\\ & ={\arg }_{R,t}\min \frac{1}{n}\sum _{i=1}^n{\Vert (R{p}_i+t-q_i{)}^T{n}_i\Vert }^2\end{array}$$其中，$n_i$为点$q_i$的法向量。
  对于point to plane的优化函数，采用最小二乘法的方式进行求解，计算过程如下，
  
  
  

ICP配准算法的标准流程

• 粗配置
  粗配准使用RANSAC+ISS+FPFH完成

  • 使用ISS算法完成点集P和Q的特征点提取，并计算FPFH特征描述子ru
  • 在特征描述空间寻找两个点集中关键点在对方集合中的配对点，构成配对点集集合。
  • 采用RANSAC算法，从配对点集中，选择3个点对。采用Procrustes Transformation的求解思路完成3个点对的$R，t$求解
  • 根据得到的$R,t$，完成源点云P的几何位置变换。然后计算配对点之间的几何距离，设置距离阈值，得到内点个数。
  • 如果内点个数满足阈值要求，就终止。如果不满足，就一直迭代到最大次数，选择所有迭代中内点比例最高的一次迭代的$R,t$结果，作为ICP配准的初始解

• 精配置
  精配准的时候，可以不用完成点云的降采样，选用全部点进行计算。尺度为欧式几何空间，在欧式空间中，寻找双方互为最近邻的配对点，保留TOP k进行P进行计算。

NDT

NDT算法有点复杂，NDT相对ICP而言，可以更好的利用周围邻域信息，有更好的鲁棒性，而且效率而比较高。具体思路等之后整理