配准是点云处理中的一个基础问题，众多学者此问题进行了广泛而深入的研究，也出现了一系列优秀成熟的算法，在三维建模、自动驾驶等领域发挥着重要的作用。

本文主要介绍粗配准NDT (Normal Distribution Transform) 与精配准ICP (Iterative Closest Point)两种经典算法。

NDT (Normal Distribution Transform)点云配准

在去年曾经对NDT原理进行了简单总结，大家可以参考链接：点云配准NDT (P2D)算法详解。

ICP (Iterative Closest Point)点云配准

参考：
基于SVD求解 3D-3D 点对匹配
该如何学习三维点云配准的相关知识？
ICP是经典的精配准算法，其以“点”为配准基元，不断迭代求得最优的位姿，最终使得目标函数最小。

点云集合

假设存在两个点云集合，
目标点云 (target cloud )： $P=\{p_1, p_2, p_3,..., p_n\}$
在这里插入图片描述
源点云 (source cloud)： $Q=\{q_1, q_2, q_3,..., q_n\}$

两者叠加显示：

可以看到上图中的两帧点云之间存在一个偏差，这个偏差需要一个位姿变换 $(R, t)$ 进行转换。

构造目标函数

ICP算法则希望得到一个最优的位姿变换 $R^*, t^*)$ 使得下式目标函数最小：
$f(R^*, t^*)=MIN(\frac{1}{N_p}{\textstyle \sum_{i=1}^{N_p}\left | p_{i}^{t}-(R^*q_{i}^{s}+t^*) \right |^2 } )$
其中 $N_p$ 为配对点云个数， $p_{i}^{t}$ 与 $q_{i}^{s}$ 是目标点云与源点云中的一对配对点。

寻找对应点对

起始步骤中，我们只有一系列无序的三维点，并没有对应的 $p_{i}^{t}$ 与 $q_{i}^{s}$ 点对。ICP中定义了“最近邻点”的定义。

使用一个初始位姿 $R^0, t^0)$ 对源点云 $Q$ 进行变换，得到一个变换后的点云 $Q^{'}$ 。
对变换后的点云 $Q^{'}$ 中的点 $q_{i}^{'}$ 在目标点云中查找最近邻点 $p_{j}$ ，如果两点之间的距离小于预先设置的阈值，则认为点 $q_{i}^{'}$ 与点 $p_{j}$ 为对应的点对。

优化位姿变换 $(R, t)$

找到所有合理的最近邻点对之后，则每一个点对都可以构建一个函数（每个点对类似于一个观测），而待求变量 $(R, t)$ 只有6个自由度。所以我们有了 $N_p$ 个观测，6个待求变量。使用最小二乘进行优化求解。

这样我们就更新了初始位姿 $R^0, t^0)$ 为新的 $R^1, t^1)$

迭代优化

得到新的位姿变换 $R^1, t^1)$ 之后，我们再次回到寻找对应点对步骤中，重新转换源点云 $Q$ 为 $Q^2$ ，查找 $Q^2$ 与 $P$ 新的点对。
接着，进行新的 “优化位姿变换 $(R, t)$ ” 步骤，重复得到新的优化位姿 $R^2, t^2)$

直到达到迭代停止条件，如：1）达到最大迭代次数，或2）位姿变化量小于阈值，或3）最近邻点对不再变化等则终止迭代。

调用PCL库

参考链接：Interactive Iterative Closest Point

  1#include <iostream>
  2#include <string>
  3
  4#include <pcl/io/ply_io.h>
  5#include <pcl/point_types.h>
  6#include <pcl/registration/icp.h>
  7#include <pcl/visualization/pcl_visualizer.h>
  8#include <pcl/console/time.h>   // TicToc
  9
 10typedef pcl::PointXYZ PointT;
 11typedef pcl::PointCloud<PointT> PointCloudT;
 12
 13bool next_iteration = false;
 14
 15void
 16print4x4Matrix (const Eigen::Matrix4d & matrix)
 17{
 18  printf ("Rotation matrix :\n");
 19  printf ("    | %6.3f %6.3f %6.3f | \n", matrix (0, 0), matrix (0, 1), matrix (0, 2));
 20  printf ("R = | %6.3f %6.3f %6.3f | \n", matrix (1, 0), matrix (1, 1), matrix (1, 2));
 21  printf ("    | %6.3f %6.3f %6.3f | \n", matrix (2, 0), matrix (2, 1), matrix (2, 2));
 22  printf ("Translation vector :\n");
 23  printf ("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix (0, 3), matrix (1, 3), matrix (2, 3));
 24}
 25
 26void
 27keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event,
 28                       void*)
 29{
 30  if (event.getKeySym () == "space" && event.keyDown ())
 31    next_iteration = true;
 32}
 33
 34int
 35main (int argc,
 36      char* argv[])
 37{
 38  // The point clouds we will be using
 39  PointCloudT::Ptr cloud_in (new PointCloudT);  // Original point cloud
 40  PointCloudT::Ptr cloud_tr (new PointCloudT);  // Transformed point cloud
 41  PointCloudT::Ptr cloud_icp (new PointCloudT);  // ICP output point cloud
 42
 43  // Checking program arguments
 44  if (argc < 2)
 45  {
 46    printf ("Usage :\n");
 47    printf ("\t\t%s file.ply number_of_ICP_iterations\n", argv[0]);
 48    PCL_ERROR ("Provide one ply file.\n");
 49    return (-1);
 50  }
 51
 52  int iterations = 1;  // Default number of ICP iterations
 53  if (argc > 2)
 54  {
 55    // If the user passed the number of iteration as an argument
 56    iterations = atoi (argv[2]);
 57    if (iterations < 1)
 58    {
 59      PCL_ERROR ("Number of initial iterations must be >= 1\n");
 60      return (-1);
 61    }
 62  }
 63
 64  pcl::console::TicToc time;
 65  time.tic ();
 66  if (pcl::io::loadPLYFile (argv[1], *cloud_in) < 0)
 67  {
 68    PCL_ERROR ("Error loading cloud %s.\n", argv[1]);
 69    return (-1);
 70  }
 71  std::cout << "\nLoaded file " << argv[1] << " (" << cloud_in->size () << " points) in " << time.toc () << " ms\n" << std::endl;
 72
 73  // Defining a rotation matrix and translation vector
 74  Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity ();
 75
 76  // A rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
 77  double theta = M_PI / 8;  // The angle of rotation in radians
 78  transformation_matrix (0, 0) = std::cos (theta);
 79  transformation_matrix (0, 1) = -sin (theta);
 80  transformation_matrix (1, 0) = sin (theta);
 81  transformation_matrix (1, 1) = std::cos (theta);
 82
 83  // A translation on Z axis (0.4 meters)
 84  transformation_matrix (2, 3) = 0.4;
 85
 86  // Display in terminal the transformation matrix
 87  std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;
 88  print4x4Matrix (transformation_matrix);
 89
 90  // Executing the transformation
 91  pcl::transformPointCloud (*cloud_in, *cloud_icp, transformation_matrix);
 92  *cloud_tr = *cloud_icp;  // We backup cloud_icp into cloud_tr for later use
 93
 94  // The Iterative Closest Point algorithm
 95  time.tic ();
 96  pcl::IterativeClosestPoint<PointT, PointT> icp;
 97  icp.setMaximumIterations (iterations);
 98  icp.setInputSource (cloud_icp);
 99  icp.setInputTarget (cloud_in);
100  icp.align (*cloud_icp);
101  icp.setMaximumIterations (1);  // We set this variable to 1 for the next time we will call .align () function
102  std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc () << " ms" << std::endl;
103
104  if (icp.hasConverged ())
105  {
106    std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl;
107    std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;
108    transformation_matrix = icp.getFinalTransformation ().cast<double>();
109    print4x4Matrix (transformation_matrix);
110  }
111  else
112  {
113    PCL_ERROR ("\nICP has not converged.\n");
114    return (-1);
115  }
116
117  // Visualization
118  pcl::visualization::PCLVisualizer viewer ("ICP demo");
119  // Create two vertically separated viewports
120  int v1 (0);
121  int v2 (1);
122  viewer.createViewPort (0.0, 0.0, 0.5, 1.0, v1);
123  viewer.createViewPort (0.5, 0.0, 1.0, 1.0, v2);
124
125  // The color we will be using
126  float bckgr_gray_level = 0.0;  // Black
127  float txt_gray_lvl = 1.0 - bckgr_gray_level;
128
129  // Original point cloud is white
130  pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h (cloud_in, (int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl,
131                                                                             (int) 255 * txt_gray_lvl);
132  viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v1", v1);
133  viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v2", v2);
134
135  // Transformed point cloud is green
136  pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h (cloud_tr, 20, 180, 20);
137  viewer.addPointCloud (cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);
138
139  // ICP aligned point cloud is red
140  pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h (cloud_icp, 180, 20, 20);
141  viewer.addPointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);
142
143  // Adding text descriptions in each viewport
144  viewer.addText ("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);
145  viewer.addText ("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);
146
147  std::stringstream ss;
148  ss << iterations;
149  std::string iterations_cnt = "ICP iterations = " + ss.str ();
150  viewer.addText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);
151
152  // Set background color
153  viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
154  viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);
155
156  // Set camera position and orientation
157  viewer.setCameraPosition (-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
158  viewer.setSize (1280, 1024);  // Visualiser window size
159
160  // Register keyboard callback :
161  viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL);
162
163  // Display the visualiser
164  while (!viewer.wasStopped ())
165  {
166    viewer.spinOnce ();
167
168    // The user pressed "space" :
169    if (next_iteration)
170    {
171      // The Iterative Closest Point algorithm
172      time.tic ();
173      icp.align (*cloud_icp);
174      std::cout << "Applied 1 ICP iteration in " << time.toc () << " ms" << std::endl;
175
176      if (icp.hasConverged ())
177      {
178        printf ("\033[11A");  // Go up 11 lines in terminal output.
179        printf ("\nICP has converged, score is %+.0e\n", icp.getFitnessScore ());
180        std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_in" << std::endl;
181        transformation_matrix *= icp.getFinalTransformation ().cast<double>();  // WARNING /!\ This is not accurate! For "educational" purpose only!
182        print4x4Matrix (transformation_matrix);  // Print the transformation between original pose and current pose
183
184        ss.str ("");
185        ss << iterations;
186        std::string iterations_cnt = "ICP iterations = " + ss.str ();
187        viewer.updateText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt");
188        viewer.updatePointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2");
189      }
190      else
191      {
192        PCL_ERROR ("\nICP has not converged.\n");
193        return (-1);
194      }
195    }
196    next_iteration = false;
197  }
198  return (0);
199}