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

最新推荐文章于 2025-04-29 16:23:34 发布

Dorwin666

最新推荐文章于 2025-04-29 16:23:34 发布

阅读量2.5k

点赞数 3

本文链接：https://blog.csdn.net/Dorwin666/article/details/89203864

版权

本文介绍了一种基于点云的匹配方法，通过使用RANSAC算法进行点云配准，实现了从二维图像到三维空间的转换。示例代码展示了如何随机生成点云数据，构造目标点云，并通过RANSAC算法筛选出有效的对应点。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

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


#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);
}