点云配准 2-icp中源点云和目标点云其对应点的查找

一、声明

本人作为初学者，才开始接触点云配准这一块，如有错误地方，望大家指出，我将及时修改，共同进步。其次这一部分主要是由于我对于PCL库中对于源点云和目标点云对应关系查找感到好奇，而去研究的，是对于PCL库中代码的理解。

二、预备知识

一、大家需要提前预备学习Kd-tree以及最近邻收缩和半径区域收缩。

三、icp算法配准流程

点云配准步骤上可以分为粗配准（Coarse Registration）和精配准（Fine Registration）两个阶段。

粗配准：是指在点云相对位姿完全未知的情况下对点云进行配准，找到一个可以让两块点云相对近似的旋转平移变换矩阵，进而将待配准点云数据转换到统一的坐标系内，可以为精配准提供良好的初始值。常见粗配准算法：

精配准：是指在粗配准的基础上，让点云之间的空间位置差异最小化，得到一个更加精准的旋转平移变换矩阵。该算法的运行速度以及向全局最优化的收敛性却在很大程度上依赖于给定的初始变换估计以及在迭代过程中对应关系的确立。所以需要各种粗配准技术为ICP算法提供较好的位置，在迭代过程中确立正确对应点集能避免迭代陷入局部极值，决定了算法的收敛速度和最终的配准精度。最常见的精配准算法是ICP及其变种。

• ICP 迭代最近点算法（Iterative Cloest Point）
• 1、数据点获取
• 2、寻找关键点
• 3、对选择的所有关键点计算其特征秒速
• 4、寻找对应点（icp用的是平方差距离，kd-tree的形式保存目标点云，便于源点云找其对应点，最近邻查找）
• 5、根据寻找的对应点进行刚体变换（就算旋转矩阵R和平移量T）
• 6、计算误差（如果误差满足条件，跳出循环，反之继续循环）

如果大家有兴趣加深对于icp的理解，博主我自己手敲了第5点代码实现（C++版本）：

(10条消息) 点云配准1-ICP算法 原理代码实现_行码阁119的博客-CSDN博客

四、PCL对应库函数讲解

一、PCL库中对于源点云和目标点云对应关系的库函数：correspondence_estimation.hpp

 其中对应的函数为：

        //确定输入和目标云之间的对应关系。
        //对应找到的对应关系（查询点索引、目标点索引、距离） 
        //max_distance 对应之间的最大允许距离 
        /** \brief Determine the correspondences between input and target cloud.
          * \param[out] correspondences the found correspondences (index of query point, index of target point, distance)
          * \param[in] max_distance maximum allowed distance between correspondences
          */
        virtual void 
        determineCorrespondences (pcl::Correspondences &correspondences,
                                  double max_distance = std::numeric_limits<double>::max ()) = 0;

---

二、函数代码为（函数代码注释已经比较详细）

思路就是：先将目标点云数据转换为kd-tree保存形式，然后在依次将原点云数据进行查找其对应关系：

//确定对应关系
template <typename PointSource, typename PointTarget, typename Scalar> void
CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorrespondences (
    pcl::Correspondences &correspondences, double max_distance)
{
   //如果没有初始化计算 
  if (!initCompute ())
    return;
  //最大距离的平方
  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;


  //这里判断源点云和目标点云的数据类新是否相同，不同则进入else，反之进行对应点的查找
  if (isSamePointType<PointSource, PointTarget> ())
  {
     //迭代源索引的输入集,输入集
    // Iterate over the input set of source indices

    for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
    {
      //这里是将源点云数据依次去查找其在目标点云中的对应点
     //大家可能对于这里tree的理解存在疑惑：tree是指向用于目标数据集的空间搜索对象的指针
     //定义于：        using KdTree = pcl::search::KdTree<PointTarget>;
     //                using KdTreePtr = typename KdTree::Ptr;   
    //                 KdTreePtr tree_;
    //这里意思是指向目标点云，且由kd_tree的形式保存，所以就不要说怎么没有把目标点云传入函数
      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
      correspondences[nr_valid_correspondences++] = corr;
    }
  }
  else
  {
    //声明一个对应点的格式
    PointTarget pt;

    // Iterate over the input set of source indices
    for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
    {
      // 将源数据复制到目标 PointTarget 格式，以便我们可以在树中搜索 
       //好理解：就是转化为目标点云一样的数据形式（目标点云转化为kd-tree保存的形式），便于对应点的查找，
      copyPoint (input_->points[*idx], pt);

      tree_->nearestKSearch (pt, 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);
  deinitCompute ();
}

五、实验结果

在上诉代码的最后加上一下代码：

    std::cout  << std::endl;
    for (int i = 0; i < nr_valid_correspondences; i++)
    {
        std::cout << "对应点索引nr_valid_correspondences：" << i+1 << ":" << std::endl;
        std::cout << "源点云的索引：" << correspondences[i].index_query << " " << "目标点云的索引：" << correspondences[i].index_match << " " << "两者之间的距离：" << correspondences[i].distance << std::endl;
    }

主函数代码：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>

int
main(int argc, char** argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>(5, 1));
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);

    // 设置点云数据（5个）
    for (auto& point : *cloud_in)
    {
        point.x = 1024 * rand() / (RAND_MAX + 1.0f);
        point.y = 1024 * rand() / (RAND_MAX + 1.0f);
        point.z = 1024 * rand() / (RAND_MAX + 1.0f);
    }
    //打印出输入点云的个数
    std::cout << "Saved " << cloud_in->size() << " data points to input:" << std::endl;
    //打印源点云数据
    for (auto& point : *cloud_in)
        std::cout << point << std::endl;
    //将源点云数据赋给目标点云
    *cloud_out = *cloud_in;
    //打印出目标点云的个数
    std::cout << "size:" << cloud_out->size() << std::endl;
    //将目标点云沿着x方向移动0.7个单位
    for (auto& point : *cloud_out)
        point.x += 0.7f;
    //答应目标点云的个数
    std::cout << "Transformed " << cloud_in->size() << " data points:" << std::endl;
    //打印出目标点云数据
    for (auto& point : *cloud_out)
        std::cout << point << std::endl;
    //icp算法
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    //传入源点云
    icp.setInputSource(cloud_in);
    //传入目标点云
    icp.setInputTarget(cloud_out);

    //设置点云Final
    pcl::PointCloud<pcl::PointXYZ> Final;
    //该函数的作用是将转变结束的点云保存到Final
    icp.align(Final);

    std::cout << "has converged:" << icp.hasConverged() << " score: " <<
        icp.getFitnessScore() << std::endl;
    std::cout << std::endl;
    std::cout << icp.getFinalTransformation() << std::endl;
    //打印出最后完成的点云数据
    std::cout << Final << std::endl;
    for (auto& point : Final)
        std::cout << point << std::endl;

    return (0);
}

结果（由于只创建了5个点云，方便与查看）：

对应点索引nr_valid_correspondences：1:
源点云的索引：0 目标点云的索引：0 两者之间的距离：0.49
对应点索引nr_valid_correspondences：2:
源点云的索引：1 目标点云的索引：1 两者之间的距离：0.490017
对应点索引nr_valid_correspondences：3:
源点云的索引：2 目标点云的索引：2 两者之间的距离：0.490017
对应点索引nr_valid_correspondences：4:
源点云的索引：3 目标点云的索引：3 两者之间的距离：0.490017
对应点索引nr_valid_correspondences：5:
源点云的索引：4 目标点云的索引：4 两者之间的距离：0.490017

对应点索引nr_valid_correspondences：1:
源点云的索引：0 目标点云的索引：0 两者之间的距离：1.3468e-08
对应点索引nr_valid_correspondences：2:
源点云的索引：1 目标点云的索引：1 两者之间的距离：7.45058e-09
对应点索引nr_valid_correspondences：3:
源点云的索引：2 目标点云的索引：2 两者之间的距离：7.45058e-09
对应点索引nr_valid_correspondences：4:
源点云的索引：3 目标点云的索引：3 两者之间的距离：2.23517e-08
对应点索引nr_valid_correspondences：5:
源点云的索引：4 目标点云的索引：4 两者之间的距离：1.21072e-08

对应点索引nr_valid_correspondences：1:
源点云的索引：0 目标点云的索引：0 两者之间的距离：4.60366e-08
对应点索引nr_valid_correspondences：2:
源点云的索引：1 目标点云的索引：1 两者之间的距离：4.47035e-08
对应点索引nr_valid_correspondences：3:
源点云的索引：2 目标点云的索引：2 两者之间的距离：1.58325e-08
对应点索引nr_valid_correspondences：4:
源点云的索引：3 目标点云的索引：3 两者之间的距离：3.56231e-08
对应点索引nr_valid_correspondences：5:
源点云的索引：4 目标点云的索引：4 两者之间的距离：4.09782e-08

对应点索引nr_valid_correspondences：1:
源点云的索引：0 目标点云的索引：0 两者之间的距离：4.44977e-08
对应点索引nr_valid_correspondences：2:
源点云的索引：1 目标点云的索引：1 两者之间的距离：4.47035e-08
对应点索引nr_valid_correspondences：3:
源点云的索引：2 目标点云的索引：2 两者之间的距离：2.70084e-08
对应点索引nr_valid_correspondences：4:
源点云的索引：3 目标点云的索引：3 两者之间的距离：5.98375e-08
对应点索引nr_valid_correspondences：5:
源点云的索引：4 目标点云的索引：4 两者之间的距离：5.21541e-08

对应点索引nr_valid_correspondences：1:
源点云的索引：0 目标点云的索引：0 两者之间的距离：3.73907e-08
对应点索引nr_valid_correspondences：2:
源点云的索引：1 目标点云的索引：1 两者之间的距离：1.01514e-07
对应点索引nr_valid_correspondences：3:
源点云的索引：2 目标点云的索引：2 两者之间的距离：9.31323e-08
对应点索引nr_valid_correspondences：4:
源点云的索引：3 目标点云的索引：3 两者之间的距离：1.04541e-07
对应点索引nr_valid_correspondences：5:
源点云的索引：4 目标点云的索引：4 两者之间的距离：8.19564e-08

对应点索引nr_valid_correspondences：1:
源点云的索引：0 目标点云的索引：0 两者之间的距离：1.46115e-07
对应点索引nr_valid_correspondences：2:
源点云的索引：1 目标点云的索引：1 两者之间的距离：2.45869e-07
对应点索引nr_valid_correspondences：3:
源点云的索引：2 目标点云的索引：2 两者之间的距离：6.79865e-08
对应点索引nr_valid_correspondences：4:
源点云的索引：3 目标点云的索引：3 两者之间的距离：2.19792e-07
对应点索引nr_valid_correspondences：5:
源点云的索引：4 目标点云的索引：4 两者之间的距离：1.20141e-07

对应点索引nr_valid_correspondences：1:
源点云的索引：0 目标点云的索引：0 两者之间的距离：2.15677e-07
对应点索引nr_valid_correspondences：2:
源点云的索引：1 目标点云的索引：1 两者之间的距离：3.06405e-07
对应点索引nr_valid_correspondences：3:
源点云的索引：2 目标点云的索引：2 两者之间的距离：1.20141e-07
对应点索引nr_valid_correspondences：4:
源点云的索引：3 目标点云的索引：3 两者之间的距离：3.37372e-07
对应点索引nr_valid_correspondences：5:
源点云的索引：4 目标点云的索引：4 两者之间的距离：1.94646e-07

对应点索引nr_valid_correspondences：1:
源点云的索引：0 目标点云的索引：0 两者之间的距离：2.55648e-07
对应点索引nr_valid_correspondences：2:
源点云的索引：1 目标点云的索引：1 两者之间的距离：4.80562e-07
对应点索引nr_valid_correspondences：3:
源点云的索引：2 目标点云的索引：2 两者之间的距离：1.86265e-07
对应点索引nr_valid_correspondences：4:
源点云的索引：3 目标点云的索引：3 两者之间的距离：5.88596e-07
对应点索引nr_valid_correspondences：5:
源点云的索引：4 目标点云的索引：4 两者之间的距离：3.28757e-07

对应点索引nr_valid_correspondences：1:
源点云的索引：0 目标点云的索引：0 两者之间的距离：3.7762e-07
对应点索引nr_valid_correspondences：2:
源点云的索引：1 目标点云的索引：1 两者之间的距离：6.00703e-07
对应点索引nr_valid_correspondences：3:
源点云的索引：2 目标点云的索引：2 两者之间的距离：2.98955e-07
对应点索引nr_valid_correspondences：4:
源点云的索引：3 目标点云的索引：3 两者之间的距离：5.43892e-07
对应点索引nr_valid_correspondences：5:
源点云的索引：4 目标点云的索引：4 两者之间的距离：5.28991e-07

对应点索引nr_valid_correspondences：1:
源点云的索引：0 目标点云的索引：0 两者之间的距离：6.16667e-07
对应点索引nr_valid_correspondences：2:
源点云的索引：1 目标点云的索引：1 两者之间的距离：6.00703e-07
对应点索引nr_valid_correspondences：3:
源点云的索引：2 目标点云的索引：2 两者之间的距离：4.18164e-07
对应点索引nr_valid_correspondences：4:
源点云的索引：3 目标点云的索引：3 两者之间的距离：8.23289e-07
对应点索引nr_valid_correspondences：5:
源点云的索引：4 目标点云的索引：4 两者之间的距离：5.52274e-07
has converged:1 score: 7.32622e-07

六、结论

通过上述代码的运行，我们可以得出结论：PCL库是将目标点云的数据先转换为kd_tree的数据形式进行保存（这里用词可能不当，但是就是这个意思），然后将源点云单个的传入其对应的函数进行查找，这里使用的是最近邻查找，通过计算两者之间的MSE距离，判断是否是其对应点，是的话，保存两者之间的索引以及两者之间的距离。然后源点云和目标点云之间的对应点就找到了。