pcl 添加源点云与目标点云匹配点到Correspondences中，并用随机采样算法（ransac）筛选内点

匹配点一般通过发现计算或者先图像匹配在通过相机坐标参数变换转化为三维对应关系。。。

#include <iostream>                 //标准输入输出头文件
#include <pcl/io/pcd_io.h>         //I/O操作头文件
#include <pcl/point_types.h>        //点类型定义头文件
#include <pcl/registration/icp.h>   //ICP配准类相关头文件
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
int main(int argc, char** argv)
{
	typedef pcl::PointXYZ Point;
	//创建两个pcl::PointCloud<pcl::PointXYZ>共享指针，并初始化它们
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
// 随机填充点云
	cloud_in->width = 300;               //设置点云宽度
	cloud_in->height = 1;               //设置点云为无序点
	cloud_in->is_dense = false;
	cloud_in->points.resize(cloud_in->width * cloud_in->height);
	for (size_t i = 0; i < cloud_in->points.size(); ++i)
	{
		cloud_in->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
		cloud_in->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
		cloud_in->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
	}
	std::cout << "Saved " << cloud_in->points.size() << " data points to input:"//打印处点云总数
		<< std::endl;

	for (size_t i = 0; i < cloud_in->points.size(); ++i) std::cout << "    " <<    //打印处实际坐标
		cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<
		cloud_in->points[i].z << std::endl;
	*cloud_out = *cloud_in;
	std::cout << "size:" << cloud_out->points.size() << std::endl;

	//实现一个简单的点云刚体变换，以构造目标点云
	for (size_t i = 0; i < cloud_in->points.size(); ++i)
		cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;
	std::cout << "Transformed " << cloud_in->points.size() << " data points:"
		<< std::endl;
	for (size_t i = 0; i < cloud_out->points.size(); ++i)     //打印构造出来的目标点云
		std::cout << "    " << cloud_out->points[i].x << " " <<
		cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;
	boost::shared_ptr<pcl::Correspondences> correspondence_all(new pcl::Correspondences);
	boost::shared_ptr<pcl::Correspondences> correspondence_inlier(new pcl::Correspondences);
	pcl::Correspondences correspondences;
	//随机产生匹配点索引
	for (int i = 0; i < 20; i++)
	{
		pcl::Correspondence Correspondence;
		Correspondence.index_match = i;   //目标点云
		Correspondence.index_query = i * 3;//源点云
		correspondences.push_back(Correspondence);
	}
	*correspondence_all = correspondences;


	pcl::registration::CorrespondenceRejectorSampleConsensus<Point> ransac;
	ransac.setInputSource(cloud_in);
	ransac.setInputTarget(cloud_out);
	ransac.setMaxIterations(200);
	ransac.setInlierThreshold(3);
	ransac.getRemainingCorrespondences(*correspondence_all, *correspondence_inlier);

	std::cout << "ransac前：\n ";
	for (int i = 0; i < correspondence_inlier->size(); i++)
	{
		std::cout << "here are correspondence_inlier " << i << "index_match:" << correspondence_all->at(i).index_match << "index_query" << correspondence_all->at(i).index_query << "\n";
	}

	std::cout << "ransac后：\n ";
	for (int i = 0; i < correspondence_inlier->size(); i++)
	{
		std::cout << "here are correspondence_inlier " << i << "index_match:" << correspondence_inlier->at(i).index_match << "index_query" << correspondence_inlier->at(i).index_query << "\n";
	}
	return (0);
}