PCL配准中的对应关系估计与拒绝接口与实例

在之前的博客中系统地总结了PCL配准模块的主要功能和ICP算法的流程，今天讨论其中对应关系估计与拒绝，即matching 与rejection环节。

1pcl中匹配对的表示方法

pcl的单个匹配对可以用pcl::Correspondence表示,pcl::Correspondence是个结构体，其包含四个元素：

struct pcl::Correspondence
{
	//query (source) point的索引
	int index_query ;
	// the matching (target) point的索引
	int index_match;
	//匹配对之间的距离
 	float distance;
 	//权重
 	float weight; 	
}

可以利用索引获得对应的匹配对点云。
多个匹配对 利用pcl::Correspondences表示，将单个pcl::Correspondence存放在pcl::Correspondences中，pcl::Correspondences定义如下：

using pcl::Correspondences = typedef std::vector< pcl::Correspondence, Eigen::aligned_allocator<pcl::Correspondence> >

可以发现：pcl::Correspondences就是一个vector，因此可以利用STL vector的一切操作，操作pcl::Correspondences。

2 pcl中对应关系估计的接口

接口1：

 /** \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
          */
        void 
        determineCorrespondences (pcl::Correspondences &correspondences,
                                  double max_distance = std::numeric_limits<double>::max ());

接口2：

        /** \brief Determine the reciprocal correspondences between input and target cloud.
         * A correspondence is considered reciprocal if both Src_i has Tgt_i as a 
         * correspondence, and Tgt_i has Src_i as one.
         *
         * \param[out] correspondences the found correspondences (index of query and target point, distance)
         * \param[in] max_distance maximum allowed distance between correspondences
         */
       void 
       determineReciprocalCorrespondences (pcl::Correspondences &correspondences,
                                           double max_distance = std::numeric_limits<double>::max ()) ;

二者的区别主要在于：determineCorrespondences（）表示单向匹配，即source点云搜索target中最近邻点，表示为一个匹配对
而determineReciprocalCorrespondences （）则要求双向匹配，即source和target点云中的匹配对，互为最近邻点才算是一个匹配对。

3PCL中对应关系拒绝的接口

唯一接口：

/** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
         * \param[in] original_correspondences the set of initial correspondences given
         * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
         */
       void
       getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 
                                    pcl::Correspondences& remaining_correspondences);

输入匹配好的pcl::Correspondences，输出保留的pcl::Correspondences。

4示例

参考链接
利用欧式距离估计对应关系，利用ransac拒绝错误的匹配对。

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>//使用随机样本一致性来识别inliers

using namespace std;
int
main(int argc, char** argv)
{
    // 加载第一次扫描点云数据作为目标云
    pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("A3  - Cloud.pcd", *target) == -1)
    {
        PCL_ERROR("读取目标点云失败 \n");
        return (-1);
    }
    cout << "从目标点云中读取 " << target->size() << " 个点" << endl;

    // 加载从新视角得到的第二次扫描点云数据作为源点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("B3  - Cloud.pcd", *source) == -1)
    {
        PCL_ERROR("读取源标点云失败 \n");
        return (-1);
    }
    cout << "从源点云中读取 " << source->size() << " 个点" << endl;

    //---------初始化对象获取匹配点对----------------------
    pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>core;
    core.setInputSource(source);
    core.setInputTarget(target);
    boost::shared_ptr<pcl::Correspondences> correspondence_all(new pcl::Correspondences);
    core.determineCorrespondences(*correspondence_all);//确定输入点云与目标点云之间的对应关系
    //------------RANSAC筛选内点----------------------------
    boost::shared_ptr<pcl::Correspondences> correspondence_inlier(new pcl::Correspondences);
    pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZ> ransac;
    ransac.setInputSource(source);
    ransac.setInputTarget(target);
    ransac.setMaximumIterations(200);//设置最大迭代次数
    ransac.setInlierThreshold(0.05);//设置对应点之间的最大距离
    //ransac.setRefineModel(true);//指定是否应该使用inliers的方差在内部细化模型
    ransac.getRemainingCorrespondences(*correspondence_all, *correspondence_inlier);
    //-----------输出必要信息到控制台------------------------
    cout << "ransac前的匹配点对有：" << correspondence_all->size() << "对" << endl;
    cout << "ransac后的匹配点对有：" << correspondence_inlier->size() << "对" << endl;

    cout << "ransac前的匹配点对：\n ";
    for (int i = 0; i < correspondence_all->size(); i++)
    {
        cout  << i << "\tindex_match:\t" << correspondence_all->at(i).index_match << "\tindex_query:\t" << correspondence_all->at(i).index_query << "\n";
    }

    cout << "ransac后的匹配点对：\n ";
    for (int i = 0; i < correspondence_inlier->size(); i++)
    {
        cout  << i << "\tindex_match:\t" << correspondence_inlier->at(i).index_match << "\tindex_query:\t" << correspondence_inlier->at(i).index_query << "\n";
    }

    boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer(new pcl::visualization::PCLVisualizer("显示点云"));
    viewer->setBackgroundColor(0, 0, 0);  //设置背景颜色为黑色
    // 对目标点云着色可视化 (red).
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>target_color(target, 255, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(target, target_color, "target cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");
    // 对源点云着色可视化 (green).
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>input_color(source, 0, 255, 0);
    viewer->addPointCloud<pcl::PointXYZ>(source, input_color, "input cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "input cloud");
    //对应关系可视化
    viewer->addCorrespondences<pcl::PointXYZ>(source, target, *correspondence_inlier, "correspondence");
    //viewer->initCameraParameters();
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }

   
    return 0;
}