当前位置:   article > 正文

点云(刚性)配准--icp_点云 刚性配准

点云 刚性配准

前言

提到配准算法,icp认第二,没哪种算法敢认第一,可见,icp这道坎是绕不过去的,在本文中,会重点介绍icp的原理与实现方案,同时,也会结合pcl中的代码进行详细介绍。

一、icp原理与步骤

1.1 目标

从本质上讲,icp类算法的基本原理是:对于两组点云PQ,计算旋转矩阵R与旋转矩阵T,使目标函数E最小:
E = 1 2 \frac {1}{2} 21 ∑ \sum ∣ ∣ q − R ∗ p − T ∣ ∣ 2 ||q - R * p - T ||^2 qRpT2
其中, q ∈ Q q \in Q qQ p ∈ P p \in P pP

1.2 原理

icp Iterated Closest Point中文翻译未迭代最近点算法,其核心在迭代,通过不断迭代使点云数据达到收敛:首先,需要计算QP中的对应点对,计算对应点对的方法比较多,按照原理可以分为多种变体,有Point to PointPoint to plane等,同时,为了提高对应点计算的速度,还可以合理的使用kd treeoctree等数据结构;然后,根据计算的点对关系计算转换矩阵(旋转与平移)。转换矩阵的求解大体分为两种:利用线性代数的求解(SVD奇异值分解),及利用非线性优化方式的方式求解(类似于BA, bundle Adjustment),其中奇异值分解的方法因为准确、稳定以及高效的特点被广泛运用(在Eigen中已经提供SVD分解方法,本文不做过多研究)。

1.3步骤

实现icp的主要步骤有:
步骤1:点云归一化即将两个点云的某一特征点(通常会使用质心,也可以选取重心或其他合理的点)平移至共同坐标原点,其目的是为了方便计算对应点对;
步骤2:确定对应点对即在关系模糊的两个点云中,更合理的预判对应关系,经典icp中是根据point to point的欧式距离进行判断,假设两片点云中欧式距离最近的点作为对应点。该方法具有一定的普适性,在后续优化中,也出现了以point to plane作为对应点的计算规则,该对应关系的查找占用大量资源,故pcl中利用kd_tree的数据结构实现查找的加速,当然,也可以通过cuda实现加速。
步骤3:矩阵计算对步骤2中提取的有序点对进行矩阵计算,主要方法有SVDBA的方法(具体方法参考1.4),获得RT;
步骤4:变换点云:将计算获取的RT作用于正片点云;
步骤5:计算目标函数以点对间的欧式距离和作为目标函数;
步骤6:判断目标函数与阈值的关系,决定是否需要进行下一轮的迭代。

1.4核心计算方法

首先计算两组点云QP的质心,分别表示为 μ q \mu^q μq μ p \mu^p μp,计算公式为:
{ μ q = 1 N ∑ i q i , q i ∈ Q μ p = 1 N ∑ i p i , p i ∈ P {μq=1Niqi,qiQμp=1Nipi,piP {μq=N1iqi,qiQμp=N1ipi,piP
其次,对于PQ中的数据进行归一化处理,即将点云移至中心点,具体计算公式为:
{ q i = q i − μ q p i = p i − μ p {qi=qiμqpi=piμp {qi=qiμqpi=piμp
然后,构建矩阵C(该矩阵为3*3):
C = ∑ i = 1 N p i q i T C = \sum_{i=1}^{N}p_iq_i^T C=i=1NpiqiT
最后,对矩阵C进行SVD分解:
C = U ∑ V T C = U \sum V^T C=UVT
得到RT:
{ R = V U T T = μ q − R μ p {R=VUTT=μqRμp {R=VUTT=μqRμp

1.5算法流程

根据自己的理解以及程序实现中的感悟,在这里画一个蹩脚的流程图,简要说明下算法逻辑,如果有错误,烦请指正:

Y
N
开始
读取数据
设置参数
计算对应点对
计算对应点间的旋转矩阵
计算评价函数
判断是否满足要求
结束

画的有点丑,先凑合看下吧,在这个流程中,有两个环节是比较重要的:计算对应点对计算对应点间的旋转矩阵,其中计算对应点对是至关重要的,这就牵扯到如何树立一种标准,才能使两片点云重合的比较完美,因此有很多人在这个过程上做工作,才有了nicp变种,例如,point to point 、point to plane、plane to plane等;另一个计算对应点间的旋转矩阵也很重要,但是老实讲,个人认为这已经成为一步死棋,不管是利用SVD还是unmerga的方法,偏差总会有的。

二、代码解读

2.1 C++实现

为保持页面整洁性,故将相关代码以资源形式上传,连接为:
https://download.csdn.net/download/weixin_42823098/13703315

2.2 PCL实现与解析

2.2.1程序调用

    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setInputSource(source_cloud);
	icp.setInputTarget(target_cloud);
	icp.setMaxCorrespondenceDistance(30.0);//点之间最大距离,影响大,鞋面是4.0,5.0
	icp.setTransformationEpsilon(1e-10);
	icp.setEuclideanFitnessEpsilon(0.00001);
	icp.setMaximumIterations(2500);
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_out(new         pcl::PointCloud<pcl::PointXYZ>);
	icp.align(*source_out);//Final是source_cloud变换后的点云

	float score = icp.getFitnessScore();
	//std::cout << "计算误差 = " << score << std::endl;

	Eigen::Matrix4f trans = icp.getFinalTransformation();
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14

2.2.2源码解析–求解矩阵

对源码进行解析,在计算对应点之间的旋转+平移矩阵时,主要解法有两种,其一为,采用Umeyama算法,其二为采用SVD分解的方式,主要调用关系(盗图参考)与部分核心源码如下:在这里插入图片描述
下列源码为采用Umeyama算法(最小二乘估计)计算对应点对关系:

    Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_src (3, npts);
    Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_tgt (3, npts);
    //cloud_src与cloud_tgt是点对关系
    for (int i = 0; i < npts; ++i)
    {
      cloud_src (0, i) = source_it->x;
      cloud_src (1, i) = source_it->y;
      cloud_src (2, i) = source_it->z;
      ++source_it;

      cloud_tgt (0, i) = target_it->x;
      cloud_tgt (1, i) = target_it->y;
      cloud_tgt (2, i) = target_it->z;
      ++target_it;
    }
    // Call Umeyama directly from Eigen (PCL patched version until Eigen is released)
    //计算cloud_src与cloud_tat的旋转矩阵
    transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false);
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18

下列源码为采用SVD算法计算对应点对关系:

source_it.reset (); target_it.reset ();
    // <cloud_src,cloud_src> is the source dataset
    transformation_matrix.setIdentity ();

    Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
    // Estimate the centroids of source, target
    compute3DCentroid (source_it, centroid_src);
    compute3DCentroid (target_it, centroid_tgt);
    source_it.reset (); target_it.reset ();

    // Subtract the centroids from source, target
    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);

    getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16

2.2.3源码解析–计算对应点对

上述讲解了如何计算对应点对的转换矩阵,现在讲解下如何计算对应点对,经典icp指的的point to point的方法,即以点与点之间的某种关系作为一个度量值,pcl中的icp用的就是这种方法,其基本思路为:计算目标点的最近欧式距离,若小于某个阈值,则认为是对应点,若大于某个阈值,则认为是无效点,下面展示point to point的源码片段:

  //目录\pcl\registration\impl\correspondence_estimation
 // Iterate over the input set of source indices
    for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
    {
      tree_->nearestKSearch ((*input_)[*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;
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/不正经/article/detail/122604
推荐阅读
相关标签
  

闽ICP备14008679号