點云配準

1 點云概述

定義：點云是某個坐標系下的點的數據集。點包含了豐富的信息，包括三維坐標X，Y，Z、顏色、分類值、強度值、時間等等。

來源：

• 三維激光掃描儀采集，RGB-D相機采集
• 二維影像三維重建
• 三維模型計算生成

作用：三維重建，…

2 點云配準

點云配準旨在將多個點云正確配準到同一個坐標系下，形成更完整的點云。點云配準要應對點云非結構化、不均勻和噪聲等干擾，要以更短的時間消耗達到更高的精度，時間消耗和精度往往是矛盾的，但在一定程度上優化是有可能的。點云配準廣泛應用于3維重建、參數評估、定位和姿態估計等領域,在自動駕駛、機器人和增強現實等新興應用上也有點云配準技術的參與。現有方法歸納為非學習方法和基于學習的方法進行分析。非學習方法分為經典方法和基于特征的方法；基于學習的方法分為結合了非學習方法的部分學習方法和直接的端到端學習方法^[1]。

點云配準可分為兩步，先粗后精：

• 粗配準（Coarse Global Registeration）：基于局部幾何特征
• 精配準（Fine Local Registeration）：需要初始位姿（initial alignment）

相關算法：

ICP(iterative closest point)是一種最經典的點云配準算法(Besl和Mckay,1992)，通過迭代對應點搜尋和最小化點對整體距離以估計變換矩陣。因為是非凸的，所以容易陷入局部極小值。當點云配準需要較大的旋轉平移時，ICP往往無法得到正確的結果。

NDT(normal distributions transform)是另一種經典的精配準方法，通過最大化源點在目標點體素化后計算出的正態分布的概率密度上的得分進行配準。

ICP和NDT都屬于非學習的配準方法，隨著深度學習技術和計算能力的提高，逐步出現了基于學習的點云配準方法，其具有速度上的優勢，特別是在粗配準方面有很大優勢。

[1]李建微,占家旺.三維點云配準方法研究進展[J].中國圖象圖形學報,2022,27(02):349-367.

ICP（Iterative Closest Point，迭代最近鄰點）

point-to-point ICP

算法流程：

得到源點云與目標點云之間的對應關系；

通過最小二乘法構建目標函數，迭代最小化點對整體距離。最終得到變換矩陣。

優缺點：

• 精度高，無需提取特征點；

• 使用前需完成粗配準，否則易陷入局部最優；

• 只適用于剛性配準；

  剛性配準主要解決的是簡單的圖像整體移動（如平移、旋轉等）問題；非剛性配準主要解決的是圖像的柔性變換問題，它容許變換過程中任意兩個像素點之間對應位置關系發生變動。

• 不適用于部分重疊點云的配準。

算法原理

假設點云$\{Q\}$為目標點云（參考點云），$\{P\}$為源點云（待配準的點云）， $p_i(i\in 1,2,...N)$是$\{P\}$中的一個點，$q_i$是$\{Q\}$中與$p_i$距離最近的點，組成點對$(q_i,p_i)$
我們需要計算從$\{P\}$到$\{Q\}$的$RT$變換矩陣，即旋轉矩陣$R$和平移矩陣$T$。如果變換參數是準確的，那么點云$\{P\}$中的每一個點$p_i$，經過變換后應該與點云$\{Q\}$中的點$q_i$完全重合，定義誤差函數如下：

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-pcgTLv8u-1658737706113)(http://zhihu.com/equation?tex=||\textbf{x}||2+%3D\sqrt{\sum{i%3D1}^Nx_i2})]

Euclid范數（歐幾里得范數，常用計算向量長度），即向量元素絕對值的平方和再開方。

其中n為兩點云之間的點對個數，$p_i$為源點云中的一點，$q_i$為目標點云中與$p_i$對應的最近鄰點，R、t分別為旋轉矩陣和平移矩陣。

因此，ICP問題可以描述為，尋找使得$E(R,t)$最小時的R和t的值。

ICP的求解分為兩種方式：利用線性代數進行求解（SVD），利用非線性優化進行求解。

SVD求解流程

• 計算兩組匹配點的質心：

  [外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-AZrlDHel-1658737706115)(https://s2.loli.net/2022/07/25/sIuxCtpgeXMS7Bv.png)]

• 得到去質心的點集：
  $${\mathbf{x}}_i:={\mathbf{p}}_i?\overline{\mathbf{p}}，{\mathbf{y}}_i:={\mathbf{q}}_i?\overline{\mathbf{q}},i=1,\mathrm{2...}n$$

• 計算3x3矩陣 H：
  $$H=X{Y}^T$$

  X和Y分別為去質心的源點云和目標點云矩陣，大小均為3xn。

• 對H進行SVD分解$H=U\Sigma V^T$，得到最優旋轉矩陣：
  $$R^{?}=V{U}^T$$

  關于SVD分解的細節參考：奇異值分解（SVD） - 知乎 (zhihu.com)

• 計算最優平移向量：
  $$\mathbf{t}=\overline{\mathbf{q}}?R\overline{\mathbf{p}}$$

ICP的優缺點及改進

上圖原文鏈接：三維點云配準 – ICP 算法原理及推導 - 知乎 (zhihu.com)

PCL庫中ICP接口使用

參考文章鏈接：PCL|ICP|Interactive Iterative Closest Point|代碼實踐 - 知乎 (zhihu.com)

PCL 庫中 ICP 的接口及其變種：

• 點到點：pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar >
• 點到面：pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >
• 面到面：pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >

其中，IterativeClosestPoint 模板類是 ICP 算法的一個基本實現，其優化求解方法基于 Singular Value Decomposition (SVD)，算法迭代結束條件包括：

• 最大迭代次數：Number of iterations has reached the maximum user imposed number of iterations (via setMaximumIterations)
• 兩次變換矩陣之間的差值：The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than an user imposed value (via setTransformationEpsilon)
• 均方誤差：The sum of Euclidean squared errors is smaller than a user defined threshold (via setEuclideanFitnessEpsilon)

基本用法：

IterativeClosestPoint<PointXYZ, PointXYZ> icp;// Set the input source and target icp.setInputCloud (cloud_source); icp.setInputTarget (cloud_target);// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored) icp.setMaxCorrespondenceDistance (0.05); // Set the maximum number of iterations (criterion 1) icp.setMaximumIterations (50); // Set the transformation epsilon (criterion 2) icp.setTransformationEpsilon (1e-8); // Set the euclidean distance difference epsilon (criterion 3) icp.setEuclideanFitnessEpsilon (1);// Perform the alignment icp.align (cloud_source_registered); // Obtain the transformation that aligned cloud_source to cloud_source_registered Eigen::Matrix4f transformation = icp.getFinalTransformation ();

官方demo：

#include <iostream> #include <string> #include <pcl/io/ply_io.h> #include <pcl/point_types.h> #include <pcl/registration/icp.h> #include <pcl/visualization/pcl_visualizer.h> #include <pcl/console/time.h> // TicToc/*** The bool will help us know when the user asks for the next iteration of ICP*/ bool next_iteration = false;/*** This function takes the reference of a 4x4 matrix and prints the rigid transformation (剛體變換)in an human readable (可讀的) way.* %6.3f 是指：要輸出的浮點數總位數（包括小數點）大于6位的話，按全寬輸出，小于 6 位時，小數點后輸出3位小數，右對齊，左邊不足的位用空格填充*/ void print4x4Matrix(const Eigen::Matrix4d &matrix) {printf("Rotation matrix :\n");printf(" | %6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));printf("R = | %6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));printf(" | %6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));printf("Translation vector :\n");printf("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix(0, 3), matrix(1, 3), matrix(2, 3)); }/*** This function is the callback for the viewer.* This function will be called whenever a key is pressed when the viewer window is on top.* If “space” is hit; set the bool(next_iteration) to true.*/ void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void *) {if (event.getKeySym() == "space" && event.keyDown())next_iteration = true; }int main(int argc, char *argv[]) {// The point clouds we will be using. The 3 point clouds we will use to store the data.pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>); // Original point cloudpcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tr(new pcl::PointCloud<pcl::PointXYZ>); // Transformed point cloudpcl::PointCloud<pcl::PointXYZ>::Ptr cloud_icp(new pcl::PointCloud<pcl::PointXYZ>); // ICP output point cloud// Checking program argumentsif (argc < 2) {printf("Usage : ");printf("%s file.ply number_of_ICP_iterations\n", argv[0]);PCL_ERROR ("Provide one ply file.\n");return (-1);}int iterations = 1; // Default number of ICP iterationsif (argc > 2) {// If the user passed the number of iteration as an argumentiterations = atoi(argv[2]);if (iterations < 1) {PCL_ERROR ("Number of initial iterations must be >= 1\n");return (-1);}}pcl::console::TicToc time;time.tic();if (pcl::io::loadPLYFile(argv[1], *cloud_in) < 0) {PCL_ERROR ("Error loading cloud %s.\n", argv[1]);return (-1);}std::cout << "\nLoaded file " << argv[1] << " (" << cloud_in->size() << " points) in " << time.toc() << " ms\n" << std::endl;// Defining a rotation matrix and translation vector.// We check the arguments of the program, set the number of initial ICP iterations and try to load the PLY file.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 radianstransformation_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) Z 方向平移 0.4 米transformation_matrix(2, 3) = 0.4;// Display in terminal the transformation matrixstd::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;print4x4Matrix(transformation_matrix);cout << "transformation_matrix\n" << transformation_matrix << endl;// Executing the transformationpcl::transformPointCloud(*cloud_in, *cloud_icp, transformation_matrix);// We backup cloud_icp into cloud_tr for later use*cloud_tr = *cloud_icp;/*** The Icp(Iterative Closest Point algorithm)* We transform the original point cloud using a rigid matrix transformation.* cloud_in contains the original point cloud.* cloud_tr and cloud_icp contains the translated/rotated point cloud.* cloud_tr is a backup we will use for display (green point cloud).* This is the creation of the ICP object. We set the parameters of the ICP algorithm.* setMaximumIterations(iterations) sets the number of initial iterations to do (1 is the default value).* We then transform the point cloud into cloud_icp.* After the first alignment we set ICP max iterations to 1 for all the next times this ICP object will be used (when the user presses “space”).*/time.tic();pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;icp.setMaximumIterations(iterations);icp.setInputSource(cloud_icp);icp.setInputTarget(cloud_in);// 輸出配準后點云icp.align(*cloud_icp);// We set this variable to 1 for the next time we will call .align () functionicp.setMaximumIterations(1);std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc() << " ms" << std::endl;/*** Check if the ICP algorithm converged(收斂 ); otherwise exit the program.* In case of success we store the transformation matrix in a 4x4 matrix and then print the rigid matrix transformation.* The reason why we store this matrix is explained later.*/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);cout << "transformation_matrix\n" << transformation_matrix << endl;} else {PCL_ERROR ("\nICP has not converged.\n");return (-1);}/*** For the visualization we create two viewports in the visualizer vertically separated(垂直分離的可視化器 ).* bckgr_gray_level and txt_gray_lvl are variables to easily switch* from white background & black text/point cloud to black background & white text/point cloud.*/pcl::visualization::PCLVisualizer viewer("ICP demo");// Create two vertically separated viewportsint v1(0);int v2(1);// 零點在左下角，x軸向右，ｙ軸向上。viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);// The color we will be using -> 背景顏色float bckgr_gray_level = 0.0; // Blackfloat txt_gray_lvl = 1.0 - bckgr_gray_level;/*** We add the original point cloud in the 2 viewports and display it the same color as txt_gray_lvl.* We add the point cloud we transformed using the matrix in the left viewport in green and the point cloud aligned with ICP in red (right viewport).*/// Original point cloud is white　-> 原始點云是白色的，旋轉45°的綠色的，紅色的是icp每一步運行運行的結果。// 這里的txt_gray_lvl，是上面定義的顯示界面的文字的顏色，這個顏色和背景顏色相加的和是“１“pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_in_color_h(cloud_in,(int) 255 * txt_gray_lvl,(int) 255 * txt_gray_lvl,(int) 255 * txt_gray_lvl);viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v1", v1);viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v2", v2);// Transformed point cloud is greenpcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_tr_color_h(cloud_tr, 20, 180, 20);viewer.addPointCloud(cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);// ICP aligned point cloud is redpcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_icp_color_h(cloud_icp, 180, 20, 20);viewer.addPointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);/*** Adding text descriptions in each viewport* We add descriptions for the point clouds in each viewport so the user knows what is what.* The string stream ss is needed to transform the integer iterations into a string.*/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);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);std::stringstream ss;ss << iterations;std::string iterations_cnt = "ICP iterations = " + ss.str();viewer.addText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);/*** We set the two viewports background color according to bckgr_gray_level.* To get the camera parameters I simply pressed “C” in the viewer.* Then I copied the parameters into this function to save the camera position / orientation / focal point.* The function registerKeyboardCallback allows us to call a function whenever the users pressed a keyboard key when viewer windows is on top.*/// Set background colorviewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);// Set camera position and orientationviewer.setCameraPosition(-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);viewer.setSize(1280, 1024); // Visualiser window size// Register keyboard callback :viewer.registerKeyboardCallback(&keyboardEventOccurred, (void *) NULL);// Display the visualiser// This is the normal behaviour if no key is pressed. The viewer waits to exit.while (!viewer.wasStopped()) {viewer.spinOnce();/*** If the user press any key of the keyboard, the function keyboardEventOccurred is called;* this function checks if the key is “space” or not. If yes the global bool next_iteration is set to true,* allowing the viewer loop to enter the next part of the code: the ICP object is called to align the meshes.* Remember we already configured this object input/output clouds and we set max iterations to 1 in lines 90-93.** As before we check if ICP as converged, if not we exit the program. printf(“033[11A”);* is a little trick to go up 11 lines in the terminal to write over the last matrix displayed.* In short it allows to replace text instead of writing new lines; making the output more readable.* We increment iterations to update the text value in the visualizer.* Now we want to display the rigid transformation from the original transformed point cloud to the current alignment made by ICP.* The function getFinalTransformation() returns the rigid matrix transformation done during the iterations (here: 1 iteration).* This means that if you have already done 10 iterations this function returns the matrix to transform the point cloud from the iteration 10 to 11.* This is not what we want.* If we multiply the last matrix with the new one the result is the transformation matrix from the start to the current iteration.* This is basically how it works** While this is mathematically true, you will easily notice that this is not true in this program due to roundings.* This is why I introduced the initial ICP iteration parameters.* Try to launch the program with 20 initial iterations and save the matrix in a text file.* Launch the same program with 1 initial iteration and press space till you go to 20 iterations.* You will a notice a slight difference.* The matrix with 20 initial iterations is much more accurate than the one multiplied 19 times.*/// The user pressed "space" :if (next_iteration) {// The Iterative Closest Point algorithmtime.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 posess.str("");ss << iterations;std::string iterations_cnt = "ICP iterations = " + ss.str();viewer.updateText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl,"iterations_cnt");viewer.updatePointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2");} else {PCL_ERROR ("\nICP has not converged.\n");return (-1);}}// We set the bool to false and the rest is the ending of the program.next_iteration = false;}return (0); }

使用teapot.ply（41472 points）迭代100次結果：

圖中白色點云為目標點云，綠色為配準前源點云，紅色為配準后點云。

使用bunny.ply（1889 points）迭代100次結果：

總結

以上是生活随笔為你收集整理的点云配准（PCL+ICP）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。