PCL之——点云配准算法ICP_pcl icp

一、ICP介绍：

1.ICP用途

ICP原名IterativeClosestPoint（迭代最近点算法），主要用于点云配准，找到两部分点云之间的刚体变换关系。可用于SLAM、位姿估计等

2.ICP原理

刚体变换不理解先看：
https://blog.csdn.net/kksc1099054857/article/details/80280964

ICP算法的基本原理是： 分别在带匹配的目标点云P和源点云Q中，按照一定的约束条件，找到最邻近点（pi，qi），然后计算出最优匹配参数R和t，使得误差函数最小。误差函数为E（R，t）为：

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

ICP算法的基本步骤是：
（1）在目标点云P中取点集pi∈P；

（2）找出源点云Q中的对应点集qi∈Q，使得||qi-pi||=min；

（3）计算旋转矩阵R和平移矩阵t，使得误差函数最小；

（4）对pi使用上一步求得的旋转矩阵R和平移矩阵t进行旋转和平移变换，的到新的

对应点集pi’={pi’=Rpi+t,pi∈P}；

（5）计算pi’与对应点集qi的平均距离；

（6）如果d小于某一给定的阈值或者大于预设的最大迭代次数，则停止迭代计算。
否则返回第2步，直到满足收敛条件为止。
ICP算法参数：
（1）优化的目标函数：点到点的距离，点到线的距离，点到面的距离
（2）点云采样方法：
（3）迭代停止条件：mse小于某个值，两次mse差值小于某个值，迭代次数
（4）对应点关系查找：KD树数据结构

3.PCL的ICP实现

PCL doc中Module registration
http://docs.pointclouds.org/1.8.1/group__registration.html
差不多五种实现：
（1）pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >：
广义迭代最近点算法
（2）pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar >
经典迭代最近点算法
（3）pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >：
带法向的迭代最近点算法
（4）pcl::IterativeClosestPointNonLinear< PointSource, PointTarget, Scalar >
非线性优化的迭代最近点算法
（5）pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >
角度空间的迭代最近点算法

二、ICP使用：

1.ICP配准步骤

但是一般教程里都没有关键点提取和描述这一步，都是直接匹配的

2.官方例程——采坑

官方例程：采用的猴子模型，我这里用来做位姿估计，使用的我自己的模型
http://pointclouds.org/documentation/tutorials/interactive_icp.php#interactive-icp

ICP输入：源点云和目标点云，初始位姿（可以没有，默认为单位阵）。

坑：
在ICP默认代码中，是源点云配目标点云，求取的变换矩阵是源点云——>目标点云的变换矩阵，

官方的源点云是cloud_icp，目标点云是cloud_in，ICP找的是cloud_icp->cloud_in的变换；然而，输入的点云只有cloud_in；cloud_icp是通过已知的变换得到的，如以下代码；
即：cloud_icp=transformation_matrix *cloud_in（矩阵左乘）；
按照道理，ICP的输出应该是transformation_matrix的逆，
但是在ICP中却采用将变换矩阵连续右乘，很明显输出的结果不是transformation_matrix的逆

// Defining a rotation matrix and translation vector
  Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity ();

  // A rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
  double theta = M_PI / 8;  // The angle of rotation in radians
  transformation_matrix (0, 0) = std::cos (theta);
  transformation_matrix (0, 1) = -sin (theta);
  transformation_matrix (1, 0) = sin (theta);
  transformation_matrix (1, 1) = std::cos (theta);

  // A translation on Z axis (0.4 meters)
  transformation_matrix (2, 3) = 0.4;

  // Display in terminal the transformation matrix
  std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;
  print4x4Matrix (transformation_matrix);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

transformation_matrix重新赋值

 if (icp.hasConverged ())
  {
    std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl;
    std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;
    transformation_matrix = icp.getFinalTransformation ().cast<double>();
    print4x4Matrix (transformation_matrix);
  }
1
2
3
4
5
6
7

transformation_matrix连续左乘

while (!viewer.wasStopped ())
  {
    viewer.spinOnce ();

    // The user pressed "space" :
    if (next_iteration)
    {
      // The Iterative Closest Point algorithm
      time.tic ();
      icp.align (*cloud_icp);
      std::cout << "Applied 1 ICP iteration in " << time.toc () << " ms" << std::endl;

      if (icp.hasConverged ())
      {
        printf ("\033[11A");  // Go up 11 lines in terminal output.
        printf ("\nICP has converged, score is %+.0e\n", icp.getFitnessScore ());
        std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_in" << std::endl;
        transformation_matrix *= icp.getFinalTransformation ().cast<double>();  // WARNING /!\ This is not accurate! For "educational" purpose only!
        print4x4Matrix (transformation_matrix);  // Print the transformation between original pose and current pose

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

我在输出每次ICP后还剩余的误差时，一直不对劲，直到把左乘改为右乘。

3.修改ICP参数

（1）经典ICP基础参数

设置迭代停止条件：最大迭代次数、mse变化大小，

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_model(new pcl::PointCloud<pcl::PointXYZ>);

pcl::IterativeClosestPoint<PointT, PointT> icp;
icp.setMaximumIterations (iterations)
icp.setInputSource (cloud_view);
icp.setInputTarget (cloud_model);
icp.align(*cloud_view);
icp.getFinalTransformation()
icp.getFitnessScore()
1
2
3
4
5
6
7
8
9
10
11
12

（2）非线性优化ICP

只需修改第一句定义即可

pcl::IterativeClosestPointNonlinear<PointT, PointT> icp;
1

发现非线性优化非常耗时（本例子中约6倍耗时）

（3）修改距离指标为点到面

• 点到面距离

作者论文：Low K L. Linear least-squares optimization for point-to-plane icp surface registration[J]. Chapel Hill, University of North Carolina, 2004, 4(10): 1-3

优化指标为：

• 点云库中如何使用

首先要计算点云的法向，然后把法线信息和xyz信息放在一起，然后使用icp.setTransformationEstimation(point_to_plane)设置距离指标；
经过对比，发现对于本实验目标有较好的效果，几次迭代就收敛到1mm以下

		//法向量计算
		pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
		pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
		//建立kdtree来进行近邻点集搜索
		pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
		//为kdtree添加点云数据
		tree->setInputCloud(init_trans_cloud);

		n.setInputCloud(init_trans_cloud);
		n.setSearchMethod(tree);
		//点云法向计算时，需要搜索的近邻点大小
		n.setKSearch(20);
		//开始进行法向计算
		n.compute(*normals);
		//* normals should not contain the point normals + surface curvatures
		//将点云数据与法向信息拼接
		pcl::PointCloud<pcl::PointNormal>::Ptr init_trans_cloud_normals(new pcl::PointCloud<pcl::PointNormal>);
		pcl::concatenateFields(*init_trans_cloud, *normals, *init_trans_cloud_normals);

		pcl::PointCloud<pcl::Normal>::Ptr normals1(new pcl::PointCloud<pcl::Normal>);
		tree->setInputCloud(cloud_model);
		n.setInputCloud(cloud_model);
		n.setSearchMethod(tree);
		n.compute(*normals1);
		pcl::PointCloud<pcl::PointNormal>::Ptr cloud_model_normals(new pcl::PointCloud<pcl::PointNormal>);
		pcl::concatenateFields(*cloud_model, *normals1, *cloud_model_normals);

		//点到面的距离函数
		pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
		typedef pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal> PointToPlane;
		boost::shared_ptr<PointToPlane> point_to_plane(new PointToPlane);
		icp.setTransformationEstimation(point_to_plane);
		icp.setInputSource (init_trans_cloud_normals);
		icp.setInputTarget (cloud_model_normals);
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

结果：

（4）修改距离指标为对称版本点到面距离指标

PCL1.10版本最新发布了一种新的指标： A symmetric objective function of point to plane，感谢开源
作者论文：Rusinkiewicz S. A symmetric objective function for ICP[J]. ACM Transactions on Graphics (TOG), 2019, 38(4): 1-7.

• 简单介绍

发布了两个版本的目标函数，旋转法线版本和简化版本

作者表示小范围内收敛效果要比点到面好

• 使用

集成的比较好，只需要在点到面的基础上修改

pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
typedef pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal> PointToPlane;
typedef pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal> SymmPointToPlane;
boost::shared_ptr<SymmPointToPlane> point_to_plane(new SymmPointToPlane);
//boost::shared_ptr<PointToPlane> point_to_plane(new PointToPlane);
icp.setTransformationEstimation(point_to_plane);   
1
2
3
4
5
6

（5）icp.getFitnessScore()方法无法在配准前计算mse

每次配准虽然可以获得欧拉适应度得分（即mse），但是如果想看初始情况的mse；即在icp.align(*cloud_view);前使用则会报错，原因是有些初始化工作需要icp.align中的一些代码来完成，主要是kdtree的东西

参考
http://www.pcl-users.org/ICP-GICP-getFitnessScore-td4039822.html
PCL原始的getFitnessScore()方法：

template <typename PointSource, typename PointTarget, typename Scalar> inline double
pcl::Registration<PointSource, PointTarget, Scalar>::getFitnessScore (double max_range)
{

  double fitness_score = 0.0;

  // Transform the input dataset using the final transformation
  PointCloudSource input_transformed;
  transformPointCloud (*input_, input_transformed, final_transformation_);

  std::vector<int> nn_indices (1);
  std::vector<float> nn_dists (1);

  // For each point in the source dataset
  int nr = 0;
  for (size_t i = 0; i < input_transformed.points.size (); ++i)
  {
    // Find its nearest neighbor in the target
    tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
    
    // Deal with occlusions (incomplete targets)
    if (nn_dists[0] <= max_range)
    {
      // Add to the fitness score
      fitness_score += nn_dists[0];
      nr++;
    }
  }

  if (nr > 0)
    return (fitness_score / nr);
  else
    return (std::numeric_limits<double>::max ());

}
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

修改的计算mse的函数：

double getmse(PointCloud::Ptr modelPointCloud, PointCloud::Ptr queryPointCloud) {
	pcl::KdTreeFLANN <PointT, flann::L2_Simple<float> > modelTree;
	modelTree.setInputCloud(modelPointCloud->makeShared());
	std::vector<int> nn_indices(1);
	std::vector<float> nn_dists(1);
	double fitness_score = 0.0;
	int count = 0;
	//For each point in the source PointCloud
	for (int i = 0; i < queryPointCloud->points.size(); i++) {
		//Find nearest neighbor in the target (query)
		modelTree.nearestKSearch(queryPointCloud->points[i], 1, nn_indices, nn_dists);
		count++;
		fitness_score += nn_dists[0];
	}
	if (count > 0)
		return (fitness_score / count);
	else
		return (std::numeric_limits<double >::max());
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

• 注意：如果采用不同的目标函数来衡量最近点之间的距离，如点到点，点到面等，获得的距离衡量指标是不同的，就不能简单的用上面的程序来计算mse了，这个是最简单的点对点距离的。