大家好，成明志来为大家解答对点云匹配算法ICP、PL-ICP、NICP和IMLS-ICP的理解的相关ICP许可证知识。对点云匹配算法ICP、PL-ICP、NICP和IMLS-ICP的理解很多人还不知道，现在让我们一起来看看吧！
点云匹配算法是为了匹配两帧点云数据，从而得到传感器（激光雷达或摄像头）前后的位姿差，即里程数据。匹配算法已经从最初的ICP方法发展出了多种改进的算法。他们分别从配准点的寻找，误差方程等等方面进行了优化。下面分别介绍：
ICP的基本思想是：
给定两个点云集合
X=\left\{x_{1}, x_{2}, \cdots, x_{N_{x}}\right\}
P=\left\{p_{1}, p_{2}, \cdots, p_{N_{p}}\right\}
其中 x_{i}和p_{i}表示点云坐标，N_{x}和N_{p}表示点云的数量。
求解旋转矩阵R和平移向量t使得下式结果最小。
E(R, t)=\frac{1}{N_{p}} \sum_{i=1}^{N_{p}}\left\|x_{i}-R p_{i}-t\right\|^{2}
在实际工程中不可能知道两个点云的点是如何配对的。只能通过迭代求解的方法一步步缩小误差，最终得到使误差方程最小的旋转矩阵R和平移矩阵t。
算法流程：
1）寻找对应点
通常使用编码盘的里程计数据得到位姿差，即当前机器人在上次机器人坐标系中的位姿。将此R和t作为ICP算法的first guess，帮助算法寻找点云对应点。
这里需注意，如果激光传感器没有安装在机器人坐标系中心，则存在里程计得到的位姿到激光传感器位姿的坐标转换关系。如下图中l所示：
从上方的(a)中可以看出，激光点是对实际环境中曲面的离散采样。最好的误差尺度是激光点到实际曲面的距离。
而PL-ICP采用分段线性的方法对实际曲面进行近似，用激光点到最近两点连线的距离来模拟实际激光点到曲面的距离。可以看出PL-ICP的误差方程更贴近实际情况。
算法流程：
先贴一张论文里的算法步骤
3）PL-ICP的求解精度高于ICP，特别是在结构化环境中。
4）PL-ICP对初始值更敏感。不单独使用。其容易陷入局部循环，如下图(a)所示。通常用里程计得到一个初始转换矩阵q_{0}给到PL-ICP算法。
csm源码包分析：
ros的csm包实现了ICP和PL-ICP算法。
源码包和论文下载地址如下：
https://censi.science/software/csm/
csm包的作者给出了一个该功能包的操作说明文件（csm_manual.pdf）。里面详细描述了各项配置参数的含义。
其中sm/app文件夹中的sm0.c sm1.c sm2.c sm3.c 相当于是几个使用示例。
主要的算法实现是在csm/icp文件夹中的几个文件里。论文中的所有算法步骤完整的体现在了icp_loop.c文件中的icp_loop函数里。
开源代码:
http://github.com/AndreaCensi/csm
参考论文：An ICP variant using a point-to-line metric
NICP方法与ICP方法的主要流程和思想是一致的。但是它在Trim outlier和误差项里考虑了更多的因素。这也是它效果更好的原因。它充分利用实际曲面的特征来对错误点进行了滤除，主要使用的特征为法向量和曲率。
在误差项里除了考虑了点到对应点切面的距离，还考虑了对应点法向量的角度差。目前NICP方法开源的代码主要是针对3D点云的，其调用了Eigen库和OpenCV库。源码中显示的部分调用了QT5。既然NICP方法考虑了法向量和曲率，那么就涉及到了如何求解点的法向量和曲率。
下面简述论文中的方法：
1）高斯拟合。找到点p_i周围半径R范围内的所有点V_i。求解均值和协方差。
\begin{aligned} \mu_{i}^{s} &=\frac{1}{\left|\mathcal{V}_{i}\right|} \sum_{\mathbf{p}_{j} \in \mathcal{V}_{i}} \mathbf{p}_{i} \\ \boldsymbol{\Sigma}_{i}^{s} &=\frac{1}{\left|\mathcal{V}_{i}\right|} \sum_{\mathbf{p}_{j} \in \mathcal{V}_{i}}\left(\mathbf{p}_{i}-\mu_{i}\right)^{T}\left(\mathbf{p}_{i}-\mu_{i}\right) \end{aligned}
2）对协方差矩阵进行SVD分解，得到按从小到大顺序的特征值\lambda_{1: 3}。
\mathbf{\Sigma}_{i}^{\mathrm{s}}=\mathbf{R}\left(\begin{array}{ccc}{\lambda_{1}} & {0} & {0} \\ {0} & {\lambda_{2}} & {0} \\ {0} & {0} & {\lambda_{3}}\end{array}\right) \mathbf{R}^{T}
论文中曲率的定义为：
\sigma_{i}=\frac{\lambda_{1}}{\lambda_{1}+\lambda_{2}}
法向量的定义：最小特征值对应的特征向量。
Trim outlier中的改进：
1）如果没有well define的法向量，则拒绝。即选择比较结构化的点，如果对应点周围过于杂乱就丢弃该点。
2）两点间的距离大于阈值，则拒绝。
\left\|\mathbf{p}_{i}^{\mathrm{c}}-\mathbf{T} \oplus \mathbf{p}_{j}^{\mathrm{r}}\right\|>\epsilon_{d}
3）两点的曲率之差距大于阈值，则拒绝。
\left|\log \sigma_{i}^{\mathrm{c}}-\log \sigma_{j}^{\mathrm{r}}\right|>\epsilon_{\sigma}
4）两点的法向量角度之差大于阈值，则拒绝。
\mathbf{n}_{i}^{\mathrm{c}} \cdot \mathbf{T} \oplus \mathbf{n}_{j}^{\mathrm{r}}<\epsilon_{n}
误差项中的改进：
1）点到对应点切面的距离作为其中的一个误差项。
2）法向量的夹角作为另一个误差项。考虑该项误差增加了旋转矩阵$R$的求解精度。
最后得到的误差定义为：
\mathbf{e}_{i j}(\mathbf{T})=\left(\tilde{\mathbf{p}}_{i}^{\mathrm{c}}-\mathbf{T} \oplus \tilde{\mathbf{p}}_{j}^{\mathrm{r}}\right)
其中
T \oplus \widetilde{\mathrm{p}}_{i}=\left(\begin{array}{c}{R p_{i}+t} \\ {R n_{i}}\end{array}\right)
即同时考虑了欧式距离和法向量的夹角。
目标函数的求解：
1）目标函数的定义为：
\sum_{c} \mathbf{e}_{i j}(\mathbf{T})^{T} \tilde{\Omega}_{i j} \mathbf{e}_{i j}(\mathbf{T})
2）非线性最小二乘问题，通过LM方法进行求解。
\begin{aligned}(\mathbf{H}+\lambda \mathbf{I}) \Delta \mathbf{T} &=\mathbf{b} \\ H &=\sum_{J_{i} J_{i}}^{T} \\ J_{i} &=\frac{\partial e_{i j}(T)}{\partial T} \quad \mathbf{T} \leftarrow \Delta \mathbf{T} \oplus \mathbf{T} \end{aligned}
对NICP的总结：
1）由于在寻找点匹配的过程中，考虑了环境 曲面的法向量和曲率，因此可以提前排除 一些明显是错误的匹配。这样就减少了计算量并且提高了计算结果的精度。
2）在误差定义中，除了考虑欧式距离之外，还考虑了法向量之间的夹角，因此具有更加准确的求解角度。
3）用LM方法进行迭代求解目标误差方程，迭代收敛即可得到两帧激光数据之间的相对位姿。
源码地址:
https://github.com/yorsh87/nicp
参考网址：
http://jacoposerafin.com/nicp/
IMLS-SLAM是一个仅依赖点云数据的低漂移（low-drift）SLAM算法。其依赖于一个scan-to-model的匹配框架。这里的model可以认为是对点云进行的局部曲面建模。
基本思想：
1）选择具有代表性的激光点来进行匹配，既能减少计算量同时又能减少激光点分布不均匀导致的计算结果出现偏移。
2）点云中隐藏着真实的曲面，最好的做法是能从参考帧点云中把曲面重建出来。
3）曲面重建的越准确，对真实世界描述越准确，匹配的精度就越高。
具体的改进措施：
1）scan egomotion
egomotion的定义如下：
进行激光数据的运动畸变去除。在雷达扫描一圈的过程中，车子是在运动的。这会造成一帧激光的时间内，每束激光测量时车子实际的位置是不同的。
论文里假设两次连续的激光扫描间隔，车子的egomotion是相似的。所以采用上一次的相对位移来计算当前的实际的egomotion。
2）去除动态障碍物
在匹配scan和model之前，我们要去除scan中所有移动物体。
我们采用去除所有可能会动的小物体的方法：首先，删去地面点云，聚类，并抛弃所有（bounding box的）长小于14m，宽小于14m，高小于4m的物体。剩下的结构足够我们匹配。最后添上去掉的地面点云。
3）特征点的选取
选取思路：
4）曲面重建
上图中的Height=(x-p_i)\cdot n_i，表示点到曲面的距离。
我们认为激光点云是分布在真实曲面的附近，并可以用高斯分布描述。如下图
所以可以用W_i(x)表示点x到点云p_i距离的权重。当点x到点云p_i距离很远时，权重会接近0。该算法会选取点x附近的一部分点使用上面的公式重建曲面。公式中的{\sum_{p_{j} \in P_{k}} W_{j}(x)}为所有这些权重的累加。
公式的实现代码
double mh2 = m_h * m_h;
for(int i = 0; i < nearPoints.size(); ++i)
{
Eigen::Vector2d delta_p = x - nearPoints[i];
double weight = std::exp(-delta_p.squaredNorm()/mh2);
projSum += weight * delta_p.dot(nearNormals[i]);
weightSum += weight;
}
height = projSum / (weightSum + 0.000001);//加一个很小的数避免除05)匹配求解
总结：
IMLS-ICP使用高斯拟合和最小二乘重建出一个隐含的曲面。找到空间点在隐含曲面的投影点。使用点到该曲面上投影点间的距离构建误差方程。
参考网址：
https://blog.csdn.net/shuangyu/article/details/88978235
IMLS-SLAM: scan-to-model matching based on 3D data,Jean-Emmanuel Deschaud
相关论文放到我的github里了：
https://github.com/shoufei403/icplearning.git
一个好的初始值可以减少icp迭代的次数，提高效率和效果。初始值的设定可以从下面三个角度来考虑：
1.、假设机器人静止，设置帧间位移为0。
2、假设机器人匀速运动，利用上一时刻机器人的速度和时间，计算当前时刻机器人位移间隔，将此作为初值传入icp
3、利用传感器如里程计、IMU等，计算前后两时刻的初始位移。实际使用时要注意坐标系的转换。因为icp方法实在激光坐标系下进行的。
精度上：
1）使用其他传感器或方法（如里程计、IMU）提供初值（coarse to fine）。
2）Ransac框架，去除outlier。
效率上：
1）选取合适的采样点。IMLS-ICP的论文里就提到了一些去除误差点的方法。
2）使用合适的数据结构，提高程序的运行效率。
我是首飞，一个帮大家填坑的机器人开发攻城狮。
另外在公众号《首飞》内回复“机器人”获取精心推荐的C/C++，Python，Docker，Qt，ROS1/2等机器人行业常用技术资料。
本文到此结束，希望对大家有所帮助。
特别声明：本文来自网络或网友投稿，部分图片和文字来源于网络收集整理，仅供学习交流，版权归原作者所有，转载的目的在于传递更多信息及用于网络分享，并不代表本站赞同其观点和对其真实性负责，也不构成任何其他建议。如果您发现网站上有侵犯您的知识产权的作品，请与我们取得联系，我们会及时修改或删除；邮箱：1091218940@qq.com
本页标题：对点云匹配算法ICP、PL-ICP、NICP和IMLS-ICP的理解
留言通道
发送成功之后，我们会尽快回复您！
Online Consulting
