你好赵伟

这个屌丝很懒，什么也没留下！

热门标签

点云配准（1）ICP 点对点配准以及原理分析

作者：你好赵伟 | 2024-04-04 17:41:48

踩

点云配准

概述

三维点云配准 -- ICP 算法原理及推导 - 知乎

基于PCL的ICP及其变种算法实现_pcl icp-CSDN博客

ICP算法对待拼接的2片点云，首先根据一定的准则确立对应点集P与Q，其中对应点对的个数为n。然后通过最小二乘法迭代计算最优的坐标变换，即旋转矩阵R和平移矢量t，使得误差函数最小。ICP算法计算简便直观且可使拼接具有较好的精度，但是算法的运行速度以及向全局最优的收敛性却在很大程度上依赖于给定的初始变换估计以及在迭代过程中对应关系的确立。各种粗拼接技术可为ICP算法提供较好的初始位置，所以迭代过程中确立正确对应点集以避免迭代陷入局部极值成为各种改进算法的关键，决定了算法的收敛速度与最终的拼接精度
https://blog.csdn.net/qq_29923461/article/details/120785717

一、矩阵

正交矩阵

矩阵迹（trace）, 行列式（determinate）_矩阵的迹-CSDN博客

如果：AAt=E（E为单位矩阵，At表示“矩阵A的转置矩阵”。）或AtA=E，则n阶实矩阵A称为正交矩阵.

主要性质：

矩阵的迹

矩阵的逆、迹、秩_逆矩阵的迹-CSDN博客

如何理解矩阵的「秩」？ - 知乎

矩阵的迹是特征值的加和，也即矩阵A的主对角线元素的总和。

二、ICP点云原理推导

迭代优化：

找到对应点后，我们就构建了两组对应的点集:

方法

有两种方法可以计算

1、利用线性代数的求解SVD

2、非线性优化方式Ax=B

非线性优化的方法：

点云配准: 刚性ICP中 Point-to-Point 和 Point-to-Plane 公式推导 ==> 帮助你代码实现 - 知乎

点云刚性配准：point2point 和 point2plane 代码 - 知乎


void ICP::point2point(float weight, Eigen::MatrixXf& A, Eigen::VectorXf& b)
{
 
	A.resize(3*m_srcOfPair.size(), 6);
	b.resize(3 * m_srcOfPair.size());
 
	for (int j = 0; j < m_srcOfPair.size(); j++)
	{
		Eigen::Vector3f sourceCoordinates;
		sourceCoordinates = m_srcOfPair[j];
 
		Eigen::Vector3f targetCoordinates;
		targetCoordinates = m_targetOfPair[j];
 
		A(3 * j, 0) = 0;
		A(3 * j, 1) = sourceCoordinates[2];//源模型中当前点的z
		A(3 * j, 2) = -sourceCoordinates[1];//源模型中当前点的y
		A(3 * j, 3) = 1;
		A(3 * j, 4) = 0;
		A(3 * j, 5) = 0;
 
		A(3 * j + 1, 0) = -sourceCoordinates[2];
		A(3 * j + 1, 1) = 0;
		A(3 * j + 1, 2) = sourceCoordinates[0];
		A(3 * j + 1, 3) = 0;
		A(3 * j + 1, 4) = 1;
		A(3 * j + 1, 5) = 0;
 
		A(3 * j + 2, 0) = sourceCoordinates[1];
		A(3 * j + 2, 1) = -sourceCoordinates[0];
		A(3 * j + 2, 2) = 0;
		A(3 * j + 2, 3) = 0;
		A(3 * j + 2, 4) = 0;
		A(3 * j + 2, 5) = 1;
 
		b[3 * j] = targetCoordinates[0] - sourceCoordinates[0];
		b[3 * j + 1] = targetCoordinates[1] - sourceCoordinates[1];
		b[3 * j + 2] = targetCoordinates[2] - sourceCoordinates[2];
 
	}
 
	A *= weight;
	b *= weight;
 
}

线性代数的求解-SVD

下面这位博主已经写的很好了，但是在我看的阅读手推的时候，发现了一些细节性的东西，下面是我对整个流程的理解和细节补充。

点云配准经典方法——ICP 原理推导及PCL实现-CSDN博客

原理

前提：

在点云配准的时候如果发现原点云的个数大于目标点云的个数，那么我们的原点云要进行下采样（随机采样）等的相关操作，让原点云的个数小于等于目标点云的个数，不然下面这个公式是不成立的（个人理解）

求解最优变换：

三维点云配准 -- ICP 算法原理及推导 - 知乎

计算最优旋转 R

计算两个点云的残差和最小

我们可以先计算出各自的点云质心

$p_{i}^{s}=p_{i}^{s}-\bar{ps}$ 原始点云的质心

$p_{i}^{t}=p_{i}^{t}-\bar{pt}$ 目标点云的质心

我们不考虑平移：loss 函数可以写成;

$F(R)= \sum_{i}^{N}||R*p_{i}^{s}-p_{i}^{t}||^{2}$

去中心化，将点云的质心带入

下面这副图是表示去中心化的东西，下面的没有带入，因为写起来麻烦

用的还是旧的点云质心。

我们先化简

$R*R^{t}=E$

我们带入到 $F(R)= \sum_{i}^{N}||R*p_{i}^{s}-p_{i}^{t}||^{2}$ 中

所以后面就只有

$R*=-2arg min(\sum_{i}^{N}(p_{s}^{i})^{T}Rp_{s}^{i}))$

我们取掉常数-2，可以转换为

$R*=arg max(\sum_{i}^{N}(p_{s}^{i})^{T}Rp_{s}^{i}))$

那么我们可以通过迹来计算

$R*=arg max(\sum_{i}^{N}(P_{s}^{i})^{T}RP_{s}^{i}))$

trace(AB)=trace(BA) ,这里要求AB是一个矩阵，但是A、B不一定是一个方阵

那么

$trace(P_{t}^{T}RP_{s})=trace(RP_{t}^{T}P_{s})$

下面我们定义一个矩阵H

$H=P_{t}^{T}P_{s}$

对H进行矩阵分解 $H=U\sum V^{T}$ svd 分解

$trace(P_{t}^{T}RP_{s})=trace(RP_{t}^{T}P_{s}) =trace(RU\sum V^{T}) =trace(\sum V^{T}RU)$

从上面我们可以看出 U、V、R都是正交矩阵，E是单位矩阵，那么 VRU 也是正交矩阵

那么我们可以计算

$M=V^{T}RU=\begin{bmatrix} m11& m12 & m13\\ m21& m22 & m23\\ m31& m32 & m33\\ \end{bmatrix}$

将M矩阵带入到迹中

$trace(P_{t}^{T}RP_{s})=trace(RP_{t}^{T}P_{s}) =trace(RU\sum V^{T}) =trace(\sum V^{T}RU)=trace(\sum M)$

$trace(EM)=trace(\begin{bmatrix} \sigma 11& 0 & 0\\ 0& \sigma 22 & 0\\ 0&0 & \sigma 33\\ \end{bmatrix} *\begin{bmatrix} m11& m12 & m13\\ m21& m22 & m23\\ m31& m32 & m33\\ \end{bmatrix})$

$=\sigma 11m11+\sigma 22m22+\sigma 33m33$

根据奇异值非负的性质和正交矩阵的性质（正交矩阵中的元素绝对值不大于 1），容易证得只有当 M 为单位阵时 trace(ΣM) 最大，即：

$V^{T}RU=E$

$R=VU^{T} => R*=R=VU^{T}$

三、计算流程

核心逻辑

有两个点云，A（目标点云） B（原点云），然后我们能见B点云一步一步的靠近A点云的过程

0、点云预处理，滤波等

1、对两个点云的点数比较，有必要就要做采样处理

2、计算两个点云的质心 $\mu ^{_{p}} \mu ^{_{q}}$ ，我们可以设点R=E并且计算 t*= $\mu ^{_{p}} - \mu ^{_{q}}$

3、去中心化，并将上面的公式简化成P、Q的形式

4、H=PQ ，将H矩阵进行svd 矩阵分解 $H=U\Sigma V^{T}$

5、计算R T

6、计算两个点云的间距，计算loss 函数，并判断时候收敛，继续迭代，返回到2


 
#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
 
#include <pcl/visualization/pcl_visualizer.h>
 
#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>
#include <pcl/filters\voxel_grid.h>
 
using namespace std;
 
#if 1
 
int main()
{
	/*自己构建点云*/
	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_icp(new pcl::PointCloud<pcl::PointXYZ>);//匹配后的点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr my_cloud_icp(new pcl::PointCloud<pcl::PointXYZ>);//匹配后的点云
	/*自己构建的点云 */
	/*填充源点云*/
	cloud_source->width = 10000;
	cloud_source->height = 1;
	cloud_source->is_dense = false;
	cloud_source->points.resize(cloud_source->width * cloud_source->height);
	for (size_t i = 0; i < cloud_source->points.size(); ++i)
	{
		cloud_source->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
		cloud_source->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
		//先测试二维数据
		cloud_source->points[i].z =1;
	}
	/*构建变换矩阵*/
	float d_yaw = 30;//变化航向角度 单位度
	float d_x = 10;//变换平移x
	float d_y = 10;//变换平移y
 
	float s = sin(d_yaw * M_PI / 180);
	float c = cos(d_yaw * M_PI / 180);
	Eigen::Matrix3f T;//变换矩阵
	T << c, -s, d_x,
		s, c, d_y,
		0, 0, 1;
 
	cout << "888888888888" << endl;
	cout << T << endl;
 
	cout << "888888888888" << endl;
 
	//将源点云赋值给目标点云
	*cloud_target = *cloud_source;
 
	/*将源点云经过变换矩阵,获得目标点云*/
	for (size_t i = 0; i < cloud_source->points.size(); ++i)
	{
		Eigen::Vector3f point_in, point_out;
		point_in << cloud_source->points[i].x, cloud_source->points[i].y, cloud_source->points[i].z;
		point_out = T * point_in;
		cloud_target->points[i].x = point_out[0];
		cloud_target->points[i].y = point_out[1];
		cloud_target->points[i].z = point_out[2];
	}
 
	/*求两个点云的几何中心*/
	int num_points = cloud_target->points.size();
	Eigen::Vector3f sum_point_in = Eigen::Vector3f::Zero();
	Eigen::Vector3f sum_point_out = Eigen::Vector3f::Zero();
	for (size_t i = 0; i < num_points; ++i)
	{
		sum_point_in[0] = sum_point_in[0] + cloud_source->points[i].x;
		sum_point_in[1] = sum_point_in[1] + cloud_source->points[i].y;
		sum_point_in[2] = sum_point_in[2] + cloud_source->points[i].z;
 
		sum_point_out[0] = sum_point_out[0] + cloud_target->points[i].x;
		sum_point_out[1] = sum_point_out[1] + cloud_target->points[i].y;
		sum_point_out[2] = sum_point_out[2] + cloud_target->points[i].z;
	}
 
	// 将点云保存起来
 
	pcl::io::savePCDFileBinaryCompressed("C:\\Users\\Albert\\Desktop\\halcon_to_pcl\\cloud_target.pcd",*cloud_target);
	pcl::io::savePCDFileBinaryCompressed("C:\\Users\\Albert\\Desktop\\halcon_to_pcl\\cloud_source.pcd", *cloud_source);
	Eigen::Vector3f target_centroid_my, source_centroid_my;
	//源点云几何中心
	target_centroid_my[0] = sum_point_in[0] / num_points;
	target_centroid_my[1] = sum_point_in[1] / num_points;
	target_centroid_my[2] = sum_point_in[2] / num_points;
 
	//目标点云几何中心
	source_centroid_my[0] = sum_point_out[0] / num_points;
	source_centroid_my[1] = sum_point_out[1] / num_points;
	source_centroid_my[2] = sum_point_out[2] / num_points;
 
	Eigen::Vector4f pcl_target_centroid,pcl_source_centroid;
	// 质心
	pcl::compute3DCentroid(*cloud_source, pcl_source_centroid);
	pcl::compute3DCentroid(*cloud_target, pcl_target_centroid);
 
	/*cout << "My      " << endl;
	cout << target_centroid_my << endl;
	cout << endl;
	cout << "PCL         " << endl;
	cout << pcl_target_centroid << endl;
	cout << endl;
	cout << "My      " << endl;
	cout << pcl_source_centroid << endl;
	cout << endl;
	cout << "PCL         " << endl;
	cout << source_centroid_my << endl;
	cout << endl;*/
 
	//  从上面我们可以得到两个点云，用来做匹配
 
 
	  // 很重要的一步  去中心化
	for (size_t i = 0; i < num_points; ++i)
	{
		//源点云去中心化
		cloud_source->points[i].x = cloud_source->points[i].x - source_centroid_my[0];
		cloud_source->points[i].y = cloud_source->points[i].y - source_centroid_my[1];
		cloud_source->points[i].z = cloud_source->points[i].z - source_centroid_my[2];
 
		//目标点云去中心化
		cloud_target->points[i].x = cloud_target->points[i].x - target_centroid_my[0];
		cloud_target->points[i].y = cloud_target->points[i].y - target_centroid_my[1];
		cloud_target->points[i].z = cloud_target->points[i].z - target_centroid_my[2];
	}
 
	// 计算w 矩阵
 
		 /*求W矩阵*/
	Eigen::Matrix3f W = Eigen::Matrix3f::Zero();//声明W矩阵
	for (size_t i = 0; i < num_points; ++i)
	{
		Eigen::Vector3f point_in, point_out;
		point_in << cloud_source->points[i].x, cloud_source->points[i].y, cloud_source->points[i].z;
		point_out << cloud_target->points[i].x, cloud_target->points[i].y, cloud_target->points[i].z;
		W = W + point_out * point_in.transpose();//累加求和
	}
 
 
	// 
	//对W矩阵进行SVD分解
	Eigen::JacobiSVD<Eigen::MatrixXf> svd(W, Eigen::ComputeThinU | Eigen::ComputeThinV);
	//求V和U
	Eigen::Matrix3f V = svd.matrixV(), U = svd.matrixU();
 
 
 
	/*求R和t*/
	Eigen::Matrix3f R = U * V.transpose();//这里公式和课件里不一致
	Eigen::Vector3f t = target_centroid_my - R * source_centroid_my;
	std::cout << "R" << std::endl << R << std::endl;
	std::cout << "t" << std::endl << t << std::endl;
 
	// 将3*3 的矩阵变成4*4 的矩阵
	Eigen::Matrix4f RT = Eigen::Matrix4f::Identity();
	RT.block<3, 3>(0, 0) = R.matrix();
	RT.block<3,1>(0, 3) = t.matrix();
	cout << "------------------------------      " << endl;
	cout << RT << endl;
	cout << "------------------------------      " << endl;
	// 
 
	 // 通过PCL 的ICP进行计算
 
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setInputSource(cloud_source);
	icp.setInputTarget(cloud_target);
	icp.setTransformationEpsilon(1e-10);
	icp.setMaxCorrespondenceDistance(100);
	icp.setEuclideanFitnessEpsilon(0.001);
	icp.setMaximumIterations(10000);
	icp.setUseReciprocalCorrespondences(false);
	icp.align(*cloud_icp);
    Eigen::Matrix4f  G = icp.getFinalTransformation();
 
 
	cout << "pcl -icp " << endl;
	cout << G << endl;
 
 
 	pcl::transformPointCloud(*cloud_source,*my_cloud_icp, RT);
	pcl::io::savePCDFileBinaryCompressed("C:\\Users\\Albert\\Desktop\\halcon_to_pcl\\pcl_icp.pcd", *cloud_icp);
	pcl::io::savePCDFileBinaryCompressed("C:\\Users\\Albert\\Desktop\\halcon_to_pcl\\myicp.pcd", *my_cloud_icp);
 
 
	system("pause");
}
 
#endif

四、PCL源码

align 对齐函数

pcl::Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output)

主要是将点云的个数对应一致


//
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output)
{
  align (output, Matrix4::Identity ());
}
 
//
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output, const Matrix4& guess)
{
  if (!initCompute ()) 
    return;
 
  // Resize the output dataset
  if (output.points.size () != indices_->size ())
    output.points.resize (indices_->size ());
  // Copy the header
  output.header   = input_->header;
  // Check if the output will be computed for all points or only a subset
  if (indices_->size () != input_->points.size ())
  {
    output.width    = static_cast<uint32_t> (indices_->size ());
    output.height   = 1;
  }
  else
  {
    output.width    = static_cast<uint32_t> (input_->width);
    output.height   = input_->height;
  }
  output.is_dense = input_->is_dense;
 
  // Copy the point data to output
  for (size_t i = 0; i < indices_->size (); ++i)
    output.points[i] = input_->points[(*indices_)[i]];
 
  // Set the internal point representation of choice unless otherwise noted
  if (point_representation_ && !force_no_recompute_) 
    tree_->setPointRepresentation (point_representation_);
 
  // Perform the actual transformation computation
    // 收敛 
  converged_ = false;
  final_transformation_ = transformation_ = previous_transformation_ = Matrix4::Identity ();
 
  // Right before we estimate the transformation, we set all the point.data[3] values to 1 to aid the rigid 
  // transformation
  for (size_t i = 0; i < indices_->size (); ++i)
    output.points[i].data[3] = 1.0;
 
  // 开始计算旋转方程
  computeTransformation (output, guess);
 
  deinitCompute ();
}

主要的函数是 computeTransformation 函数

computeTransformation函数


///
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation (
    PointCloudSource &output, const Matrix4 &guess)
{
  // Point cloud containing the correspondences of each point in <input, indices>
  PointCloudSourcePtr input_transformed (new PointCloudSource);
 
  nr_iterations_ = 0;
  converged_ = false;
 
  // Initialise final transformation to the guessed one
  final_transformation_ = guess;
 
  // If the guessed transformation is non identity
  if (guess != Matrix4::Identity ())
  {
    input_transformed->resize (input_->size ());
     // Apply guessed transformation prior to search for neighbours
    transformCloud (*input_, *input_transformed, guess);
  }
  else
    *input_transformed = *input_;
 
  transformation_ = Matrix4::Identity ();
 
  // Make blobs if necessary
  determineRequiredBlobData ();
  PCLPointCloud2::Ptr target_blob (new PCLPointCloud2);
  if (need_target_blob_)
    pcl::toPCLPointCloud2 (*target_, *target_blob);
 
  // Pass in the default target for the Correspondence Estimation/Rejection code
  correspondence_estimation_->setInputTarget (target_);   // 目标点云
 
 
  if (correspondence_estimation_->requiresTargetNormals ())
    correspondence_estimation_->setTargetNormals (target_blob);
 
 
  // Correspondence Rejectors need a binary blob
  for (size_t i = 0; i < correspondence_rejectors_.size (); ++i)
  {
    registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
    if (rej->requiresTargetPoints ())
      rej->setTargetPoints (target_blob);
    if (rej->requiresTargetNormals () && target_has_normals_)
      rej->setTargetNormals (target_blob);
  }
 
  convergence_criteria_->setMaximumIterations (max_iterations_);  // 最大迭代次数
  convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);  //  icp.setEuclideanFitnessEpsilon(0.001); 
  convergence_criteria_->setTranslationThreshold (transformation_epsilon_);  // icp.setTransformationEpsilon(1e-5);
  convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_); //
 
  // Repeat until convergence
  // 开始迭代 直到 收敛
  do
  {
    // Get blob data if needed
    PCLPointCloud2::Ptr input_transformed_blob;
    if (need_source_blob_)
    {
      input_transformed_blob.reset (new PCLPointCloud2);
      toPCLPointCloud2 (*input_transformed, *input_transformed_blob);
    }
    // Save the previously estimated transformation
    previous_transformation_ = transformation_;
 
    // Set the source each iteration, to ensure the dirty flag is updated
    correspondence_estimation_->setInputSource (input_transformed);
    if (correspondence_estimation_->requiresSourceNormals ())
      correspondence_estimation_->setSourceNormals (input_transformed_blob);
    // Estimate     correspondences 估计对应关系
    if (use_reciprocal_correspondence_)   // 使用对等对应
      correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_);
    else   //determineCorrespondences 
      correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_);
                                               // determineCorrespondences
  /*    该函数用于求取两幅点云的对应关系，主要流程如下：
        将 target 点云加入 kdtree
        遍历 source 点云中的每一个点，寻找对应 target 点云中的最近点
        将以上一组点云作为一对匹配关系保存下来*/
    //template <typename PointSource, typename PointTarget, typename Scalar> void
    //    pcl::registration::CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorrespondences
    // (pcl::Correspondences & correspondences, double max_distance)
    //{
    //    double max_dist_sqr = max_distance * max_distance;
 
    //    correspondences.resize(indices_->size());
 
    //    std::vector<int> index(1);
    //    std::vector<float> distance(1);
    //    pcl::Correspondence corr;
    //    unsigned int nr_valid_correspondences = 0;
 
    //    // Iterate over the input set of source indices
    //    for (std::vector<int>::const_iterator idx = indices_->begin(); idx != indices_->end(); ++idx)
    //    {
    //        tree_->nearestKSearch(input_->points[*idx], 1, index, distance);
    //        if (distance[0] > max_dist_sqr)
    //            continue;
 
    //        corr.index_query = *idx;
    //        corr.index_match = index[0];
    //        corr.distance = distance[0];
    //        correspondences[nr_valid_correspondences++] = corr;
    //    }
 
    //    correspondences.resize(nr_valid_correspondences);
    //}
 
    //if (correspondence_rejectors_.empty ())
    // 对应关系
    CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_));
 
 
 
    for (size_t i = 0; i < correspondence_rejectors_.size (); ++i)
    {
      registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
      PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", rej->getClassName ().c_str ());
      if (rej->requiresSourcePoints ())
        rej->setSourcePoints (input_transformed_blob);
      if (rej->requiresSourceNormals () && source_has_normals_)
        rej->setSourceNormals (input_transformed_blob);
      rej->setInputCorrespondences (temp_correspondences);
      rej->getCorrespondences (*correspondences_);
      // Modify input for the next iteration
      if (i < correspondence_rejectors_.size () - 1)
        *temp_correspondences = *correspondences_;
    }
 
    size_t cnt = correspondences_->size ();
    // Check whether we have enough correspondences
    if (static_cast<int> (cnt) < min_number_correspondences_)
    {
      PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
      convergence_criteria_->setConvergenceState(pcl::registration::DefaultConvergenceCriteria<Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
      converged_ = false;
      break;
    }
 
    // Estimate the transform
    transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);
                                              //estimateRigidTransformation
 
 
     //void pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation(
    //    const pcl::PointCloud<PointSource> &cloud_src,
    //    const pcl::PointCloud<PointTarget> &cloud_tgt,
    //    const pcl::Correspondences & correspondences,
    //    Matrix4 & transformation_matrix) const
    //{
    //    ConstCloudIterator<PointSource> source_it(cloud_src, correspondences, true);
    //    ConstCloudIterator<PointTarget> target_it(cloud_tgt, correspondences, false);
    //    estimateRigidTransformation(source_it, target_it, transformation_matrix);
    //}
 
 
    //template <typename PointSource, typename PointTarget, typename Scalar> inline void
    //    pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation(
    //        ConstCloudIterator<PointSource>& source_it,
    //        ConstCloudIterator<PointTarget>& target_it,
    //        Matrix4& transformation_matrix) const
    //{
    //    transformation_matrix.setIdentity();
 
    //    Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
    //    // 计算质心
    //    compute3DCentroid(source_it, centroid_src);
    //    compute3DCentroid(target_it, centroid_tgt);
    //    source_it.reset(); target_it.reset();
 
    //    // 减去质心
    //    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean;
    //    demeanPointCloud(source_it, centroid_src, cloud_src_demean);
    //    demeanPointCloud(target_it, centroid_tgt, cloud_tgt_demean);
 
    //    // SVD 分解
    //    getTransformationFromCorrelation(cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
    //}
        //template <typename PointSource, typename PointTarget, typename Scalar> void
    //    pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation(
    //        const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_src_demean,
    //        const Eigen::Matrix<Scalar, 4, 1>& centroid_src,
    //        const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_tgt_demean,
    //        const Eigen::Matrix<Scalar, 4, 1>& centroid_tgt,
    //        Matrix4& transformation_matrix) const
    //{
    //    transformation_matrix.setIdentity();
 
    //    // Assemble the correlation matrix H = source * target'
    //    Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean * cloud_tgt_demean.transpose()).topLeftCorner(3, 3);
 
    //    // Compute the Singular Value Decomposition  SVD分解H矩阵 
    //    Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3> > svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
    //    Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU();
    //    Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV();
    //   计算 R 矩阵
    //    // Compute R = V * U'    
    //    if (u.determinant() * v.determinant() < 0)
    //    {
    //        for (int x = 0; x < 3; ++x)
    //            v(x, 2) *= -1;
    //    }
           // Compute R = V * U'    
    //    Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose();
 
    //    // Return the correct transformation
    //    transformation_matrix.topLeftCorner(3, 3) = R;
    //    const Eigen::Matrix<Scalar, 3, 1> Rc(R * centroid_src.head(3));
          // t*=pt-R*ps
    //    transformation_matrix.block(0, 3, 3, 1) = centroid_tgt.head(3) - Rc;
    //}
 
 
    // Tranform the data
    transformCloud (*input_transformed, *input_transformed, transformation_);
 
    // Obtain the final transformation    
    final_transformation_ = transformation_ * final_transformation_;
 
    ++nr_iterations_;
 
    // Update the vizualization of icp convergence
    //if (update_visualizer_ != 0)
    //  update_visualizer_(output, source_indices_good, *target_, target_indices_good );
 
    converged_ = static_cast<bool> ((*convergence_criteria_));
  }
  while (!converged_);
 
  // Transform the input cloud using the final transformation
  PCL_DEBUG ("Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n", 
      final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3),
      final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3),
      final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3),
      final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3));
 
  // Copy all the values
  output = *input_;
  // Transform the XYZ + normals
  transformCloud (*input_, output, final_transformation_);
}

但是里面有几个特别重要的函数

determineCorrespondences函数

determineRequiredBlobData()函数

estimateRigidTransformation函数----非常重要的函数

determineCorrespondences函数


/*    该函数用于求取两幅点云的对应关系，主要流程如下：
        将 target 点云加入 kdtree
        遍历 source 点云中的每一个点，寻找对应 target 点云中的最近点
        将以上一组点云作为一对匹配关系保存下来*/
        template <typename PointSource, typename PointTarget, typename Scalar> void
            pcl::registration::CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorrespondences
         (pcl::Correspondences & correspondences, double max_distance)
        {
            double max_dist_sqr = max_distance * max_distance;
        
            correspondences.resize(indices_->size());
        
            std::vector<int> index(1);
            std::vector<float> distance(1);
            pcl::Correspondence corr;
            unsigned int nr_valid_correspondences = 0;
        
            // Iterate over the input set of source indices
            for (std::vector<int>::const_iterator idx = indices_->begin(); idx != indices_->end(); ++idx)
            {
                tree_->nearestKSearch(input_->points[*idx], 1, index, distance);
                if (distance[0] > max_dist_sqr)
                    continue;
        
                corr.index_query = *idx;
                corr.index_match = index[0];
                corr.distance = distance[0];
                correspondences[nr_valid_correspondences++] = corr;
            }
        
            correspondences.resize(nr_valid_correspondences);
        }

determineRequiredBlobData函数


template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::determineRequiredBlobData ()
{
  need_source_blob_ = false;
  need_target_blob_ = false;
  // Check estimator
  need_source_blob_ |= correspondence_estimation_->requiresSourceNormals ();
  need_target_blob_ |= correspondence_estimation_->requiresTargetNormals ();
  // Add warnings if necessary
  if (correspondence_estimation_->requiresSourceNormals () && !source_has_normals_)
  {
      PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects source normals, but we can't provide them.\n", getClassName ().c_str ());
  }
  if (correspondence_estimation_->requiresTargetNormals () && !target_has_normals_)
  {
      PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects target normals, but we can't provide them.\n", getClassName ().c_str ());
  }
  // Check rejectors
  for (size_t i = 0; i < correspondence_rejectors_.size (); i++)
  {
    registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
    need_source_blob_ |= rej->requiresSourcePoints ();
    need_source_blob_ |= rej->requiresSourceNormals ();
    need_target_blob_ |= rej->requiresTargetPoints ();
    need_target_blob_ |= rej->requiresTargetNormals ();
    if (rej->requiresSourceNormals () && !source_has_normals_)
    {
      PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects source normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
    }
    if (rej->requiresTargetNormals () && !target_has_normals_)
    {
      PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects target normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
    }
  }
 
}

estimateRigidTransformation函数


 
void pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation(
    const pcl::PointCloud<PointSource> &cloud_src,
    const pcl::PointCloud<PointTarget> &cloud_tgt,
    const pcl::Correspondences & correspondences,
    Matrix4 & transformation_matrix) const
{
    ConstCloudIterator<PointSource> source_it(cloud_src, correspondences, true);
    ConstCloudIterator<PointTarget> target_it(cloud_tgt, correspondences, false);
    estimateRigidTransformation(source_it, target_it, transformation_matrix);
}
 
 
template <typename PointSource, typename PointTarget, typename Scalar> inline void
    pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation(
        ConstCloudIterator<PointSource>& source_it,
        ConstCloudIterator<PointTarget>& target_it,
        Matrix4& transformation_matrix) const
{
    transformation_matrix.setIdentity();
 
    Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
    // 计算质心
    compute3DCentroid(source_it, centroid_src);
    compute3DCentroid(target_it, centroid_tgt);
    source_it.reset(); target_it.reset();
 
    // 减去质心
    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean;
    demeanPointCloud(source_it, centroid_src, cloud_src_demean);
    demeanPointCloud(target_it, centroid_tgt, cloud_tgt_demean);
 
    // SVD 分解
    getTransformationFromCorrelation(cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
}
  //template <typename PointSource, typename PointTarget, typename Scalar> void
    pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation(
        const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_src_demean,
        const Eigen::Matrix<Scalar, 4, 1>& centroid_src,
        const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_tgt_demean,
        const Eigen::Matrix<Scalar, 4, 1>& centroid_tgt,
        Matrix4& transformation_matrix) const
{
    transformation_matrix.setIdentity();
 
    // Assemble the correlation matrix H = source * target'
    Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean * cloud_tgt_demean.transpose()).topLeftCorner(3, 3);
 
    // Compute the Singular Value Decomposition  SVD分解H矩阵 
    Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3> > svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU();
    Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV();
   计算 R 矩阵
    // Compute R = V * U'    
    if (u.determinant() * v.determinant() < 0)
    {
        for (int x = 0; x < 3; ++x)
            v(x, 2) *= -1;
    }
     // Compute R = V * U'    
    Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose();
 
    // Return the correct transformation
    transformation_matrix.topLeftCorner(3, 3) = R;
    const Eigen::Matrix<Scalar, 3, 1> Rc(R * centroid_src.head(3));
    // t*=pt-R*ps
    transformation_matrix.block(0, 3, 3, 1) = centroid_tgt.head(3) - Rc;
}

ConstCloudIterator 函数

该函数被调用了两次，目的是将 source 与 target 内的点根据对应关系对应起来，即通过索引直接对应，source[i] 对应 target[i]。


 
template <class PointT>
pcl::ConstCloudIterator<PointT>::ConstCloudIterator (
    const PointCloud<PointT>& cloud, const Correspondences& corrs, bool source)
{
    std::vector<int> indices;
    indices.reserve (corrs.size ());
    if (source)
    {
        for (typename Correspondences::const_iterator indexIt = corrs.begin (); indexIt != corrs.end (); ++indexIt)
            indices.push_back (indexIt->index_query);
    }
    else
    {
        for (typename Correspondences::const_iterator indexIt = corrs.begin (); indexIt != corrs.end (); ++indexIt)
            indices.push_back (indexIt->index_match);
    }
    iterator_ = new typename pcl::ConstCloudIterator<PointT>::ConstIteratorIdx (cloud, indices);
}

五、测试

CC点云匹配

CloudCompare 技巧四点云匹配_cloudcompare点云配准-CSDN博客

code 测试


 
#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
 
#include <pcl/visualization/pcl_visualizer.h>
 
#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>
#include <pcl/filters\voxel_grid.h>
 
using namespace std;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
 
PointCloud::Ptr cloud_target(new PointCloud);
PointCloud::Ptr cloud_source(new PointCloud);
PointCloud::Ptr cloud_icp(new PointCloud);
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("global_visualizer"));
 
const double translation_step = 0.005;  // 设定一个平移的步长
const double rotation_step = M_PI / 72;  // 设定一个旋转的步长
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event, void* viewer_void);
Eigen::Affine3f G = Eigen::Affine3f::Identity();
 
int main() {
	//加载点云
	string target_path = "C:\\Users\\Albert\\Desktop\\halcon_to_pcl\\1.pcd";
	string source_path = "C:\\Users\\Albert\\Desktop\\halcon_to_pcl\\2.pcd";
	pcl::io::loadPCDFile(target_path, *cloud_target);
	pcl::io::loadPCDFile(source_path, *cloud_source);
 
	//点云降采样
	pcl::VoxelGrid<pcl::PointXYZ> VG;
	VG.setInputCloud(cloud_target);
	VG.setLeafSize(0.001f, 0.001f, 0.001f);
	VG.filter(*cloud_target);
 
	VG.setInputCloud(cloud_source);
	VG.setLeafSize(0.001f, 0.001f, 0.001f);
	VG.filter(*cloud_source);
 
	//可视化相关
	int v1 = 1;
	viewer->createViewPort(0.0, 0.0, 1.0, 1.0, v1);
	viewer->setBackgroundColor(255, 255, 255, v1);
	viewer->addPointCloud(cloud_target, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud_target, 0.0, 0.0, 0.0), "cloud_target", v1);
	viewer->addPointCloud(cloud_source, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud_source, 255.0, 0.0, 0.0), "cloud_source", v1);
	viewer->registerKeyboardCallback(keyboardEventOccurred, (void*)viewer.get());
 
 
	while (!viewer->wasStopped()) {
		// 应用变换
		pcl::transformPointCloud(*cloud_source, *cloud_source, G);
		G = Eigen::Affine3f::Identity();
		
 
		// 更新视图
		viewer->removePointCloud("cloud_source");
		viewer->addPointCloud<pcl::PointXYZ>(cloud_source, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud_source, 255.0, 0.0, 0.0), "cloud_source", v1);
		viewer->removePointCloud("cloud_target");
		viewer->addPointCloud<pcl::PointXYZ>(cloud_target, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud_target, 0.0, 0.0, 0.0), "cloud_target", v1);
 
		viewer->spinOnce(10);  // 每次更新等待10ms
	}
 
}
 
//键盘回调函数
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event, void* viewer_void)
{
	if (event.keyDown())
	{
		if (event.getKeySym() == "space") {
			pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
			icp.setInputSource(cloud_source);
			icp.setInputTarget(cloud_target);
			icp.setTransformationEpsilon(1e-10);
			icp.setMaxCorrespondenceDistance(100);
			icp.setEuclideanFitnessEpsilon(0.001);
			icp.setMaximumIterations(10000);
			icp.setUseReciprocalCorrespondences(false);
 
			icp.align(*cloud_icp);
			G = icp.getFinalTransformation();
 
		}
		else 
		{
			switch (event.getKeySym()[0])
			{
			case 'w':
				G.translation() += Eigen::Vector3f(translation_step, 0, 0);
				break;
			case 's':
				G.translation() -= Eigen::Vector3f(translation_step, 0, 0);
				break;
			case 'a':
				G.translation() -= Eigen::Vector3f(0, translation_step, 0);
				break;
			case 'd':
				G.translation() += Eigen::Vector3f(0, translation_step, 0);
				break;
			case 'z':
				G.translation() += Eigen::Vector3f(0, 0, translation_step);
				break;
			case 'c':
				G.translation() -= Eigen::Vector3f(0, 0, translation_step);
				break;
			case 'r':
				G.rotate(Eigen::AngleAxisf(rotation_step, Eigen::Vector3f::UnitX()));
				break;  // Roll
			case 'p':
				G.rotate(Eigen::AngleAxisf(rotation_step, Eigen::Vector3f::UnitY()));
				break;  // Pitch
			case 'y':
				G.rotate(Eigen::AngleAxisf(rotation_step, Eigen::Vector3f::UnitZ()));
				break;  // Yaw
			default: break;
			}
		}
	}
	cout << "变换矩阵   " << endl;
	cout << G.matrix() << endl;
	cout << endl;
	cout << endl;
	cout << endl;
}

六、ICP点对点的优缺点

优点

简单，不需要对点云进行分割和特征提取

缺点

计算速度慢，开销大

只考虑点对点之间的距离，缺少对点云特征的利用

对R t的设定比较重要会影响速度

很容易拿到局部最优的R t

点云的噪点影响很大，所以一般在进行ICP迭代的时候要先进行一次滤波

推广

point-to-plane

点对面的计算考虑到点云的特征和结构，不会陷入局部最优

plane-to-plane

考虑点云结构和特征，计算面到面的距离

Generalized ICP （GICP）

考虑Point-to-Point plane-to-plane plane-to-plane,这样精度就会提高

Normal Iterative Closest Point(NICP)

考虑法向量和局部曲率，更进一步的进行匹配

声明：本文内容由网友自发贡献，转载请注明出处：【wpsshop博客】

点云配准（1）ICP 点对点配准以及原理分析

概述

一、矩阵

正交矩阵

主要性质：

矩阵的迹

二、ICP点云原理推导

迭代优化：

方法

非线性优化的方法：

线性代数的求解-SVD

原理

前提：

求解最优变换：

计算最优旋转 R

三、 计算流程

核心逻辑

四、PCL源码

align 对齐函数

computeTransformation函数

determineCorrespondences函数

determineRequiredBlobData函数

estimateRigidTransformation函数

ConstCloudIterator 函数

五、测试

CC点云匹配

code 测试

六、ICP点对点的优缺点

优点

缺点

推广

point-to-plane

plane-to-plane

Generalized ICP （GICP）

Normal Iterative Closest Point(NICP)

三、计算流程