前端匹配：ICP, PL-ICP, N-ICP, IMLS-ICP (to be continued)

前端匹配：ICP, PL-ICP, N-ICP, IMLS-ICP

PL-ICP

PL-ICP的目标函数实际上表示点到曲面距离，即点到直线距离。
ICP目标函数：$\underset{q}{\min }=\sum _i||p_i\oplus q-\prod (S^{res},p_i\oplus q)|{|}^2$
     $q=(t,\theta )$
     $p\oplus (t,\theta )\triangleq R(\theta )+t$

$S^{ref}$表示参考激光帧生成的曲面

$\prod (S^{ref},p_i)$表示激光点$p_i$在曲面上的投影

PL-ICP目标函数： $\underset{q_{k+1}}{\min }=\sum _i(n_i^T[p_i\oplus q_{k+1}-\prod (S^{res},p_i\oplus q_k)]{)}^2$

N-ICP

替换ICP方法中的对应点匹配(point correspondences)方法。
充分利用实际曲面的特征来对错误的点匹配进行滤除，主要的特征为法向量和曲率。
误差项除了考虑对应点的欧式距离之外，同时还考虑对应点法向量的角度差。
 $p_i$表示激光点的坐标
 $n_i$表示对应的法向量
 ${\sigma }_i$表示对应的曲率
 $\widetilde{p_i}=(p_i,n_i{)}^T$表示拓展的点
 $T$表示欧式变换的矩阵
 $\oplus$表示对于$\widetilde{p_i}$的操作符
 $T\oplus \widetilde{p_i}=\left({}_{R{n}_i}^{R{p}_i+t}\right)$

误差函数的定义：
  $e_{ij}(T)=(\widetilde{p_i}-T\oplus \widetilde{p_j^r})$
目标函数的定义：
  
to be continued!