重新理解Point-to-Point的ICP

重新理解Point-to-Point ICP

说了很多次ICP,其实只知道个大概,ICP是做什么的,也用PCL做过一点ICP,但是在一次组会中发现自己对ICP的算法过程理解还不是很到位,所以重新阅读&重新理解,推荐的一篇博客

ICP算法的基本原理

分别在带匹配的目标点云$P$和源点云$Q$中,按照一定的约束条件,找到最邻近点$(p_i, q_i)$,然后计算出最优匹配参数$R$和$t$,使得误差函数最小。误差函数为$E(R, t)$为:

其中$n$为最邻近点对的个数,$p_i$为目标点云$P$中的一点,$q_i$ 为源点云$Q$中与$p_i$对应的最近点,$R$为旋转矩阵,$t$为平移向量。

ICP算法步骤

(1)在源点云$P$中取点集$p_{i} ∈ P $;

(2)找出目标点云$Q$中的对应点集$q_i ∈ Q$,使得$||q_i - p_i ||= min$;

(3)计算旋转矩阵$R$和平移矩阵$t$,使得误差函数最小;

(4)对使用上一步求得的$R$和$t$进行旋转和平移变换,得到新的对应点集

(5)计算与对应点集的平均距离

(6)如果$d$小于某一给定的阈值或者大于预设的最大迭代次数,则停止迭代计算,否则返回第2步;

  • 最近点对查找:

    对应点的计算是整个配准过程中耗费时间最长的步骤,查找最近点,PCL中利用了K-d tree来提高查找速度,最普通的可以是遍历整个点云找距离最近的点;

    对应点对 - ①基于特征的最近点;②基于距离的最近点

  • 如何计算(3)中的Rt矩阵

    可以参考另一篇文章Link)

    为了找到最佳旋转,我们首先平移两个点云集,使两个点云的质心都在原点,假设是进行质心平移后,代表的点的集合,是协方差矩阵。

  • Converges(是否收敛?)

    • Errors decrease monotonically(偏差单调减少)
    • Converges to local minimum(陷入局部最优解)
    • Good initial guess ⟹⟹ Converges to global minimum(全局最优解)
  • 收敛条件

    • 前一个变换矩阵和当前变换矩阵的差异小于阈值时,认为已经收敛
    • 均方误差和(Mean Square Error, $\sigma = \sqrt{\frac{\sum ^n \varepsilon _i ^2}{n}}$)小于阈值

ICP算法关键点:

(1)原始点集的采集:均匀采样、随机采样和法矢采样

(2)确定对应点集:点到点、点到投影、点到面

(3)计算变化矩阵:四元数法、SVD奇异值分解法

使用PCL中封装ICP的代码:

  • PCL中的ICP算法是基于SVD(Singular Value Decomposition)实现的
  • 使用pcl的ICP之前要set几个参数,如最大迭代次数,阈值,两次矩阵之间的差值,对应点对之间的最大距离
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75

#include <iostream>
#include <pcl/io/ply_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/transforms.h>

int main(int argc, char** argv)
{

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_trans(new pcl::PointCloud<pcl::PointXYZ>());

pcl::io::loadPLYFile("b000.ply", *cloud_target); //读入目标点云
pcl::io::loadPLYFile("b045.ply", *cloud_source); //源点云

//去除源点云和目标点云中的无效点
std::vector<int> index;
pcl::removeNaNFromPointCloud(*cloud_source, *cloud_source, index);
pcl::removeNaNFromPointCloud(*cloud_target, *cloud_target, index);

//ICP
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud_source);
icp.setInputTarget(cloud_target);

//参数设置,部分可缺省
icp.setMaxCorrespondenceDistance(1); //对应点间的最大距离(单位为m)
icp.setMaximumIterations(50); //最大迭代次数
icp.setTransformationEpsilon(1e-6); //两次变化矩阵之间的差值
icp.setEuclideanFitnessEpsilon(0.001); //设置收敛条件是均方误差和小于阈值, 停止迭代
icp.align(*cloud_source_trans); //变换后的源点云

if (icp.hasConverged())
{
//如果满足输入条件,说明收敛,输出收敛分数
std::cout << "Converged. score =" << icp.getFitnessScore() << std::endl;

//输出变换矩阵
Eigen::Matrix4f transformation = icp.getFinalTransformation();
std::cout << transformation << std::endl;
}
else
std::cout << "Not converged." << std::endl;


//点云可视化
boost::shared_ptr< pcl::visualization::PCLVisualizer > viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(0, 0, 0); //背景色可设置

//显示源点云
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(cloud_source, 0, 255, 0);
viewer->addPointCloud<pcl::PointXYZ>(cloud_source, source_color, "source");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "source");

//显示目标点云
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(cloud_target, 255, 0, 255);
viewer->addPointCloud<pcl::PointXYZ>(cloud_target, target_color, "target");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target");

//显示变换后的源点云
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_trans_color(cloud_source_trans, 255, 255, 255);
viewer->addPointCloud<pcl::PointXYZ>(cloud_source_trans, source_trans_color, "source trans");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "source trans");

//保存变换结果
pcl::io::savePLYFile("ICP_test_trans.ply", *cloud_source_trans, false);
viewer->spin();

system("pause");
return(0);
}

ICP之变种:

ICP有很多变种,有point-to-point的,也有point-to-plane的,一般后者的计算速度快一些,是基于法向量的,需要输入数据有较好的法向量,具体使用时建议根据自己的需要及可用的输入数据选择具体方法。

pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget > Class Template Reference
pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar > Class Template Reference
pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar > Class Template Reference
pcl::IterativeClosestPointNonLinear< PointSource, PointTarget, Scalar > Class Template Reference
pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar > Class Template Reference
pcl::registration::IncrementalICP< PointT, Scalar > Class Template Reference

*References

文章作者:Jiadai Sun

最后更新:2019年09月25日 22:09:07

原始链接:https://sunjiadai.xyz/2019/09/14/重新理解Point-to-Point的ICP/

版权声明:本博客所有文章除特别声明外,均采用 BY-NC-SA 3.0 许可协议,转载请注明出处!


-----------本文结束-----------
0%