C++ PCL点云配准源码实例

程序示例精选
C++ PCL点云配准源码实例
如需安装运行环境或远程调试，见文章底部个人QQ名片，由专业技术人员远程协助！

前言

这篇博客针对《C++ PCL点云配准源码实例》编写代码，代码整洁，规则，易读。 学习与应用推荐首选。

---

运行结果

---

文章目录

一、所需工具软件
二、使用步骤
       1. 主要代码
       2. 运行结果
三、在线协助

一、所需工具软件

       1. VS2019
       2. C++

二、使用步骤

代码如下（示例）：

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
using namespace std;

//点云可视化
void visualize_pcd(PointCloud::Ptr pcd_src, PointCloud::Ptr pcd_tgt, PointCloud::Ptr pcd_final)
{

	pcl::visualization::PCLVisualizer viewer("registration Viewer");
	pcl::visualization::PointCloudColorHandlerCustom<PointT> final_h(pcd_final, 0, 0, 255);
	viewer.addPointCloud(pcd_src, src_h, "source cloud");
	viewer.addPointCloud(pcd_tgt, tgt_h, "tgt cloud");
	viewer.addPointCloud(pcd_final, final_h, "final cloud");
	viewer.addCoordinateSystem(1.0);
	while (!viewer.wasStopped())
	{
		viewer.spinOnce(100);
		//boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
}

//由旋转平移矩阵计算旋转角度
void matrix2angle(Eigen::Matrix4f& result_trans, Eigen::Vector3f& result_angle)
{
	double ax, ay, az;
	if (result_trans(2, 0) == 1 || result_trans(2, 0) == -1)
	{
		az = 0;
		double dlta;
		dlta = atan2(result_trans(0, 1), result_trans(0, 2));
		if (result_trans(2, 0) == -1)
		{
			ay = M_PI / 2;
		}
		else
		{
			ay = -M_PI / 2;
		}
	}
	else
	{
		ay = -asin(result_trans(2, 0));
		ax = atan2(result_trans(2, 1) / cos(ay), result_trans(2, 2) / cos(ay));
	}
	result_angle << ax, ay, az;
}

int
main(int argc, char** argv)
{
	//1. 加载点云文件
	PointCloud::Ptr  cloud_src_o(new PointCloud); //源点云，待配准
	pcl::io::loadPCDFile("rabbit_source.pcd", *cloud_src_o);
	PointCloud::Ptr cloud_tgt_o(new PointCloud);//目标点云
	pcl::io::loadPCDFile("rabbit_target.pcd", *cloud_tgt_o);
	cout << "读入点云完成！" << endl;
	PointCloud::Ptr  cloud_src(new PointCloud);
	pcl::VoxelGrid<PointT>  voxel_grid;

	voxel_grid.setLeafSize(0.3, 0.3, 0.3);
	voxel_grid.setInputCloud(cloud_src_o);
	cout << "down size *cloud_src_o from " << cloud_src_o->size() << "to" << cloud_src->size() << endl;
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_src;
	ne_src.setInputCloud(cloud_src);
	pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree< pcl::PointXYZ>());
	ne_src.setSearchMethod(tree_src);
	PointCloud::Ptr cloud_tgt(new PointCloud);
	pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_2;
	voxel_grid_2.setLeafSize(0.3, 0.3, 0.3);
	voxel_grid_2.setInputCloud(cloud_tgt_o);
	voxel_grid_2.filter(*cloud_tgt);
	cout << "down size *cloud_tgt_o.pcd from " << cloud_tgt_o->size() << "to" << cloud_tgt->size() << endl;

	//6.对目标点云进行法线估计
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_tgt;
	ne_tgt.setInputCloud(cloud_tgt);
	pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_tgt(new pcl::search::KdTree< pcl::PointXYZ>());
	ne_tgt.setSearchMethod(tree_tgt);
	pcl::PointCloud<pcl::Normal>::Ptr cloud_tgt_normals(new pcl::PointCloud< pcl::Normal>);
	//ne_tgt.setKSearch(20);
	ne_tgt.setRadiusSearch(0.02);
	ne_tgt.compute(*cloud_tgt_normals);
	pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_tgt;
	fpfh_tgt.setInputCloud(cloud_tgt);
	fpfh_tgt.setInputNormals(cloud_tgt_normals);
	pcl::search::KdTree<PointT>::Ptr tree_tgt_fpfh(new pcl::search::KdTree<PointT>);
	//SAC配准
	pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> scia;
	scia.setInputSource(cloud_src);
	scia.setInputTarget(cloud_tgt);
	//scia.setMinSampleDistance(1);
	//scia.setNumberOfSamples(2);
	//scia.setCorrespondenceRandomness(20);
	PointCloud::Ptr sac_result(new PointCloud);
	scia.align(*sac_result);
	cout << "sac has converged:" << scia.hasConverged() << "  score: " << scia.getFitnessScore() << endl;
	Eigen::Matrix4f sac_trans;
	sac_trans = scia.getFinalTransformation();
	cout << sac_trans << endl;

	//icp配准
	PointCloud::Ptr icp_result(new PointCloud);
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	//Set the max correspondence distance to 4cm (e.g., correspondences with higher distances will be ignored)
	icp.setMaxCorrespondenceDistance(0.04);
	// 最大迭代次数
	icp.setMaximumIterations(50);
	// 两次变化矩阵之间的差值
	icp.setTransformationEpsilon(1e-10);
	cout << "ICP has converged:" << icp.hasConverged()
		<< " score: " << icp.getFitnessScore() << endl;
	Eigen::Matrix4f icp_trans;
	icp_trans = icp.getFinalTransformation();
	cout << icp_trans << endl;
	//使用创建的变换对未过滤的输入点云进行变换
	pcl::transformPointCloud(*cloud_src_o, *icp_result, icp_trans);

	//计算误差
	Eigen::Vector3f ANGLE_origin;
	ANGLE_origin << 0, 0, M_PI / 5;
	double error_x, error_y, error_z;
	matrix2angle(icp_trans, ANGLE_result);
	error_x = fabs(ANGLE_result(0)) - fabs(ANGLE_origin(0));
	error_y = fabs(ANGLE_result(1)) - fabs(ANGLE_origin(1));
	error_z = fabs(ANGLE_result(2)) - fabs(ANGLE_origin(2));

	//可视化
	visualize_pcd(cloud_src_o, cloud_tgt_o, icp_result);
	return (0);

}

运行结果

三、在线协助：

如需安装运行环境或远程调试，见文章底部个人 QQ 名片，由专业技术人员远程协助！

1）远程安装运行环境，代码调试
2）Visual Studio, Qt, C++, Python编程语言入门指导
3）界面美化
4）软件制作
5）云服务器申请
6）网站制作

当前文章连接：https://blog.csdn.net/alicema1111/article/details/132666851
个人博客主页：https://blog.csdn.net/alicema1111?type=blog
博主所有文章点这里：https://blog.csdn.net/alicema1111?type=blog

博主推荐：
Python人脸识别考勤打卡系统：
https://blog.csdn.net/alicema1111/article/details/133434445
Python果树水果识别：https://blog.csdn.net/alicema1111/article/details/130862842
Python+Yolov8+Deepsort入口人流量统计：https://blog.csdn.net/alicema1111/article/details/130454430
Python+Qt人脸识别门禁管理系统：https://blog.csdn.net/alicema1111/article/details/130353433
Python+Qt指纹录入识别考勤系统：https://blog.csdn.net/alicema1111/article/details/129338432
Python Yolov5火焰烟雾识别源码分享：https://blog.csdn.net/alicema1111/article/details/128420453
Python+Yolov8路面桥梁墙体裂缝识别：https://blog.csdn.net/alicema1111/article/details/133434445