PCL交互迭代最近点方法

最新推荐文章于 2025-06-21 08:41:23 发布

Rson的学习笔记

最新推荐文章于 2025-06-21 08:41:23 发布

阅读量1.4k

点赞数

CC 4.0 BY-SA版权

分类专栏： C++ PCL 文章标签： PCL ICP 交互操作

本文链接：https://blog.csdn.net/u014801811/article/details/79979764

**代码出处：**http://pointclouds.org/documentation/tutorials/interactive_icp.php#interactive-icp

交互迭代最近法允许用户每次按下“空格”键，一次ICP迭代算法执行，并更新视图。
此程序主要步骤：

1、导入点云数据

PCL提供IO模块直接读入点云文件。

pcl::io::loadPCDFile("dragon_source.pcd", *cloud_in)

注：控制台参数输入时，cd到exe路径，执行 "XX arg1 arg2 …"

2、旋转平移获取待配准点云数据

pcl中矩阵的定义及赋值：

//定义旋转矩阵和平移矩阵
	Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();

	double theta = M_PI / 8;//旋转角度
	transformation_matrix(0,0) = cos(theta);
	transformation_matrix(0,1) = -sin(theta);
	transformation_matrix(1,0) = sin(theta);
	transformation_matrix(1,1) = cos(theta);
	
	transformation_matrix(0, 3) = 0;//沿x轴移动0.m
	transformation_matrix(1, 3) = 0;//沿y轴移动0.m
	transformation_matrix(2, 3) = 0.04;//沿z轴移动0.04m

	pcl::transformPointCloud(*cloud_in, *cloud_icp, transformation_matrix);//执行变换

3、icp配准

	pcl::IterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ> icp;//配准对象
	icp.setInputSource(cloud_icp);
	icp.setInputTarget(cloud_in);
	/*icp.setEuclideanFitnessEpsilon(0.1);
	icp.setRANSACIterations(10);
	icp.setTransformationEpsilon(1e-6);*/
	icp.align(*cloud_icp);

	icp.setMaximumIterations(1);//迭代一次

4、点云可视化

//可视化
	pcl::visualization::PCLVisualizer viewer("ICP demo");
	//创建两个视口
	int v1(0);
	int v2(1);
	viewer.createViewPort(0, 0, 0.5, 1, v1);
	viewer.createViewPort(0.5, 0, 1, 1, v2);
	//颜色
	float bckgr_gray_level = 0.0;//黑色
	float txt_gray_lvl = 1.0 - bckgr_gray_level;

	//原始点云 白色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_in_color_h
		(cloud_in, (int)