本博文参考：https://blog.csdn.net/wokaowokaowokao12345/article/details/73741957

介绍

给定两个来自不同坐标系的三维数据点集，找到两个点集空间的变换关系，使得两个点集能统一到同一坐标系统中，即配准过程。配准的目标是在全局坐标框架中找到单独获取的视图的相对位置和方向，使得它们之间的相交区域完全重叠。对于从不同视图（views）获取的每一组点云数据，需要一个能够将它们对齐在一起的单一点云模型，从而可以应用后续处理步骤，如分割和对象重构。这篇博文就基于PCL中的ICP算法，实现不同视图点云数据的配准，并延伸相关知识。

配准模型

两个(pairwise)点云数据集的配准，即在其中一个数据上进行旋转和平移，使得他们之间的相交区域完全重合。这里输出结果是一个（4x4）刚性（rigid）变换矩阵。

两个数据集的计算步骤：

识别最能代表两个数据集中的场景的兴趣点（interest points）（即关键点 keypoints）
在每个关键点处，计算特征描述符;
从特征描述符集合以及它们在两个数据集中的x,y,z位置，基于特征和位置之间的相似性来估计对应关系;
假设数据被认为包含噪声的，并不是所有的对应关系都是有效的，所以舍弃对配准过程产生负面影响的那些负影响对应关系;
从剩下的一组好的对应关系中，估计一个变换行为。

关键点

点云中的特征点是一些兴趣点，如目标的转点、角点等具有明显特征（feature）的点或点集。
特征描述基于发现的关键点，提取特征组合信息并生成向量来进行比较。一致性估计 correspondence estimate给定两组数据获取相应的特征描述向量，找到相应的特征以在数据中找到重叠的部分。
舍弃糟糕的一致性估计

不是所有的一致性估计都是正确的，错误的一致性将对变换产生负影响，因此需要舍弃。
变换 transformation

步骤：

根据一致性评价误差度量（error metric）
估计变换矩阵
优化点结构

迭代最近点 Iterative Closest Point （ICP）

ICP算法本质上是基于最小二乘法的最优配准方法。该算法重复进行选择对应关系点对，计算最优刚体变换这一过程，直到满足正确配准的收敛精度要求。ICP是一个广泛使用的配准算法，主要目的就是找到旋转和平移参数，将两个不同坐标系下的点云，以其中一个点云坐标系为全局坐标系，另一个点云经过旋转和平移后两组点云重合部分完全重叠。ICP算法最早由Chen and Medioni，and Besl and McKay提出。

算法的输入：参考点云和目标点云，停止迭代的标准。
算法的输出：旋转和平移矩阵，即转换矩阵。

基本过程

对于目标点云中的每个点，匹配参考点云（或选定集合）中的最近点。求得使上述对应点对计算均方根（root mean square，RMS）最小的刚体变换，求得平移参数和旋转参数。使用获得的转换矩阵来转换目标点云。迭代（重新关联点），直到满足终止迭代的条件（迭代次数或误差小于阈值）。这里的误差最小，可以是相邻两次均方根差的绝对值小于某一限差。

可能存在的问题

算法收敛于局部最小误差。
噪声或异常数据可能导致算法无法收敛或错误。
在进行ICP算法第一步要确定一个迭代初值，选取的初值将对最后配准结果产生重要的影响，如果初值选择不合适，算法可能就会限入局部最优，使得迭代不能收敛到正确的配准结果。

源代码

#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>   // TicToctypedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;bool next_iteration = false;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));
}void
keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event,void* nothing)
{if (event.getKeySym () == "space" && event.keyDown ())next_iteration = true;
}int
main ()
{// The point clouds we will be usingPointCloudT::Ptr cloud_in (new PointCloudT);  // Original point cloudPointCloudT::Ptr cloud_tr (new PointCloudT);  // Transformed point cloudPointCloudT::Ptr cloud_icp (new PointCloudT);  // ICP output point cloudint iterations = 1;  // Default number of ICP iterationspcl::console::TicToc time;time.tic ();if (pcl::io::loadPLYFile ("fish-2.ply", *cloud_in) < 0){PCL_ERROR ("Error loading cloud %s.\n");return (-1);}std::cout << "\nLoaded file " << "fish-2.ply" << " (" << cloud_in->size () << " points) in " << time.toc () << " ms\n" << std::endl;// Defining a rotation matrix and translation vectorEigen::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) = cos (theta);transformation_matrix (0, 1) = -sin (theta);transformation_matrix (1, 0) = sin (theta);transformation_matrix (1, 1) = cos (theta);// A translation on Z axis (0.4 meters)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);// Executing the transformationpcl::transformPointCloud (*cloud_in, *cloud_icp, transformation_matrix);*cloud_tr = *cloud_icp;  // We backup cloud_icp into cloud_tr for later use// The Iterative Closest Point algorithmtime.tic ();pcl::IterativeClosestPoint<PointT, PointT> icp;icp.setMaximumIterations (iterations);icp.setInputSource (cloud_icp);icp.setInputTarget (cloud_in);icp.align (*cloud_icp);icp.setMaximumIterations (1);  // We set this variable to 1 for the next time we will call .align () functionstd::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc () << " ms" << std::endl;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);}else{PCL_ERROR ("\nICP has not converged.\n");return (-1);}// Visualizationpcl::visualization::PCLVisualizer viewer ("ICP demo");// Create two vertically separated viewportsint v1 (0);int v2 (1);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 usingfloat bckgr_gray_level = 0.0;  // Blackfloat txt_gray_lvl = 1.0 - bckgr_gray_level;// Original point cloud is whitepcl::visualization::PointCloudColorHandlerCustom<PointT> 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<PointT> 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<PointT> 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 viewportviewer.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);// 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 visualiserwhile (!viewer.wasStopped ()){viewer.spinOnce ();// 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);}}next_iteration = false;}return (0);
}

实验效果

实验数据

https://download.csdn.net/download/qq_29462849/10859837

PCL中的点云ICP配准（附源代码和数据）相关推荐

PCL中3D点云特征描述与提取（三）
PCL中3D点云特征描述与提取(三) 1 如何从一个深度图像中提取NARF特征 2 RoPs特征 2.1 理论基础 2.1.1 生物视觉认知学启示 2.1.2 局部参考坐标框架构建 2.1.3 RoP ...
【公测中】阿里云发布国内首个大数据双活容灾服务，满足高要求大数据灾备场景...
在6月上旬举行的云栖大会上海峰会上,阿里云发布了国内首个大数据集群双活容灾产品-混合云容灾服务下的混合云大数据容灾服务(HDR for Big Data, 简称 HDR-BD),并已经在7月份开始邀测 ...
【公测中】阿里云发布国内首个大数据双活容灾服务，满足高要求大数据灾备场景
在6月上旬举行的云栖大会上海峰会上,阿里云发布了国内首个大数据集群双活容灾产品-混合云容灾服务下的混合云大数据容灾服务(HDR for Big Data, 简称 HDR-BD),并已经在7月份开始邀测 ...
open3d完成点云ICP配准
环境:win10,python3.6+open3d==0.8.0 完成点云配准之后的效果图演示这里直接放上完整的代码 import open3d as o3d import numpy as np# ...
android wsdl封装,在Android中调用C#写的WebService(附源代码)
由于项目中要使用Android调用C#写的WebService,于是便有了这篇文章.在学习的过程中,发现在C#中直接调用WebService方便得多,直接添加一个引用,便可以直接使用将WebServi ...
传统的点云分割方法及PCL中的分割模块
参考:https://www.cnblogs.com/li-yao7758258/p/10908980.html 三维点云分割是将同属性的点云物体分割出来,以便于单独对该点云物体处理,但是由于点云数据 ...
PCL点云处理之点面ICP配准（附代码，参数设置，实验结果）（六十七）
PCL点云处理之点面ICP配准(附代码,参数设置,实验结果)(六十七) 前言一.点面ICP是什么? 二.使用步骤 1.代码效果总结前言学习点云配准,我辈义不容辞一.点面ICP是什么? 将之 ...
斯坦福的著名小兔子模型的点云数据_传统方法的点云分割以及PCL中分割模块
之前在微信公众号中更新了以下几个章节 1,如何学习PCL以及一些基础的知识 2,PCL中IO口以及common模块的介绍 3, PCL中常用的两种数据结构KDtree以及Octree树的介绍有兴趣 ...
MeshLab中进行点云配准
MeshLab是一个开源.可移植和可扩展的三维几何处理系统,主要用于交互处理和非结构化编辑三维三角形网格.它支持多种文件格式: import:PLY, STL, OFF, OBJ, 3DS, COLL ...
PCL提取3D点云模型特征（3.0 FPFH快速点特征直方图）附完整代码
一.概述上一篇博客解释了PFH是什么以及如何利用PFH来提取点云的特征,那么讲了PFH(PCL提取3D点云模型特征(2.0 PFH点特征直方图 )附完整代码)之后肯定是要接着说FPFH的.本来想着把 ...

PCL中的点云ICP配准（附源代码和数据）

介绍