点云配准2：icp算法在PCL1.10.0上的实现+源码解析

已于 2022-07-06 20:27:54 修改
阅读量1w 收藏 200
点赞数 35
分类专栏： 点云配准(PointCloud Registration) 点云算法 文章标签： icp算法 算法 计算机视觉 c++
于 2020-11-18 11:22:20 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_41102371/article/details/109763866
版权

目录

本文最后实现的配准实例

在这里插入图片描述

点云配准系列

点云配准1：配准基础及icp算法
点云配准2：icp算法在PCL1.10.0上的实现+源码解析
点云配准3：3d-ndt算法在pcl上的实现以及参数设置
点云配准4：cloudcompare的使用以及点云配准功能
点云配准5：4pcs算法在pcl上的实现
点云配准6：tricp算法在pcl上的实现
点云配准论文阅读笔记–Efficient Variants of the ICP Algorithm
点云配准论文阅读笔记–Comparing ICP variants on real-world data sets
点云配准论文阅读笔记–(4PCS)4-Points Congruent Sets for Robust Pairwise Surface Registration
点云配准论文阅读笔记–3d-dnt博士论文

准备

本实例是在visual studio2019+pcl1.10.0上进行，环境配置方法在之前的博客已进行详细说明：
vs2019配置pcl1.10.0+点云可视化示例

程序结构

主程序

在这里插入图片描述
可视化：
在这里插入图片描述
（上述关系图的生成工具：mindmaster）
几个说明

1、为什么要降采样

将上万的点降采样至几百个，能大幅提升算法的速度。因为几百个点的运算，与几万个点的运算比，速度差异是巨大的。

2、体素降采样原理

通过输人的点云数据创建一个三维体素栅格(可把体素栅格想象为微小的空间三维立方体的集合)，然后在每个体索(即边长为m、n、o的三维小立方体)内，用体索中所有点的重心来近似显示体素中其他点，这样该体素内所有点就用一个重心点最终表示。这样确实可以减少点云的数据量，并且像对于斯坦福兔子点云模型这样不含噪点的模型来说，体素降采样后的点云配准速度大大提升。本文代码中的体素使用0.01x0.01x0.01的立方体。

3、点云更新

降采样点云只是用来参与运算从而获得变换矩阵，未降采样的点云的配准才是我们需要的，因此每次得到的矩阵还要用于去更新未降采样的源点云。

icp

在这里插入图片描述

配准前的参数设置

代码里面的MSE：
MSE是两点云距离的均方误差，是 每个点云的距离之和 除以点对 所得到的值
下文中的pre_mse为上一次迭代的mse
上述关系图中只使用了一个配准时可以设置的参数，实际上可以使用的还有这些

icp.setEuclideanFitnessEpsilon(0.00001);// 0.001% of the previous MSE (relative error)  （registration.h）
icp.convergence_criteria_->setAbsoluteMSE(1e-6);//（default——convergence_criteria.h）
icp.convergence_criteria_->setMaximumIterationsSimilarTransforms(3);//（default——convergence_criteria.h）
icp.setMaxCorrespondenceDistance(1);//对应点间的最大距离(单位为m)（registration.h）
icp.setMaximumIterations(iterations);//最大迭代次数，超过这个次数，无论算法有没有找到最优解都停止

其中：
1、icp.setEuclideanFitnessEpsilon(0.00001);
abs(pre_mse-mse)/mse来与0.00001作比较，如果小于0.00001则可认为两次迭代已经很接近了，可能已经达到最优解了
2、icp.convergence_criteria_->setAbsoluteMSE(1e-6);
abs(pre_mse-mse)两次迭代的绝对差，小于1e-6可认为两次迭代已经很接近了，可能已经达到最优解了
3、icp.convergence_criteria_->setMaximumIterationsSimilarTransforms(3);
当上述两个判定连续3次都是满足的，那么认为算法已经找到最优解

icp配准算法内部

在主程序icp.align（）处加断点，然后F11进入单步调试。
在这里插入图片描述
icp.align（）是配准的入口，在registration.hpp中
在这里插入图片描述
首先是初始化
initCompute ()是初始化函数，会在里面对目标点云建立kd树搜索
在这里插入图片描述
后面是其他一些初始化，就不一一细讲
到computeTransformation (output, guess);
在这里插入图片描述
进入配准的下一步
computeTransformation ()在icp.hpp里面
在这里插入图片描述
接着对我们在主函数里面设置的参数赋值给收敛准则convergence_criteria_
在这里插入图片描述
下面是个do…while语句，意在执行一次配准，再判定是否满足算法停止条件，不满足则继续迭代，
在这里插入图片描述
correspondence_estimation_->setInputSource (input_transformed);中的input_transformed会在每次迭代完成后由求得的变换矩阵进行更新
correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_);是查找待配准点云在目标点云中的对应最近邻点对，查找的结果放在correspondences_中
接着就是用对应点对（correspondences_）+待配准点云（input_transformed）+目标点云（target_）进行变换矩阵的求解
求解到的变换矩阵放在transformation_里
然后用transformCloud（）函数以变换矩阵transformation_对待配准点云（input_transformed）进行更新
迭代次数+1
while判定是否满足算法停止条件，若不满足则继续执行以上步骤

在这里插入图片描述

对应点对确定（determineCorrespondences）

correspondence_estimation_->determineCorrespondences ();函数在correspondence_esitimation.hpp中

在这里插入图片描述
核心的部分在此，在for循环里面查找每个点在目标点云中的最近点，并计算距离，若距离大于设定的阈值，则拒绝这个点对，否则将对应点对放入correspondences
在这里插入图片描述

变换矩阵求解estimateRigidTransformation

estimateRigidTransformation在transformation_esitimation_svd.hpp中
在这里插入图片描述
进入estimateRigidTransformation（）函数，内部是使用svd分解的算法进行变换矩阵的求解（pcl::umeyama（）函数），有兴趣的可以单步执行进去查看实现的源码，这里主要探究icp的实现过程，对此不深究。，

算法是否应该停止判定converged_ = static_cast ((*convergence_criteria_));

pcl::registration::DefaultConvergenceCriteria::hasConverged ()在default_convergence_criteria.hpp中
主要是判定迭代次数是否达到设定阈值，相对mse、绝对mse是否小于设定阈值等
在这里插入图片描述
并返回收敛状态
在这里插入图片描述

enum ConvergenceState
{
	CONVERGENCE_CRITERIA_NOT_CONVERGED,//0还未收敛
	CONVERGENCE_CRITERIA_ITERATIONS,//1超过最大迭代次数
	CONVERGENCE_CRITERIA_TRANSFORM,//2变换矩阵已经相似
	CONVERGENCE_CRITERIA_ABS_MSE,//3absolute_mse已满足
	CONVERGENCE_CRITERIA_REL_MSE,//4relative_mse已满足
	CONVERGENCE_CRITERIA_NO_CORRESPONDENCES,//5对应点太少（小于3）
	CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS//6
};

主程序代码

icp源码涉及太多，这里不给出，可自己在pcl点云库查看，或者自己单步调试看每一步的执行过程
以下给出主程序代码，每句代码均已加上注释

#include <iostream>  
#include <string>  
#include <pcl/io/ply_io.h>  //ply文件读取头文件
#include <pcl/point_types.h>  
#include <pcl/registration/icp.h> //配准所需头文件 
#include <pcl/visualization/pcl_visualizer.h>  //可视化所需头文件
#include <pcl/console/time.h>   // TicToc  //头文件计时
#include <pcl/io/pcd_io.h>//pcd文件读取
#include <pcl/filters/voxel_grid.h>//体素降采样滤波

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
bool next_iteration = false;//迭代flag
//打印矩阵函数
void print4x4Matrix(const Eigen::Matrix4d& matrix)
{
	printf("Rotation matrix :\n");
	printf("    | %6.5f %6.5f %6.5f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
	printf("R = | %6.5f %6.5f %6.5f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
	printf("    | %6.5f %6.5f %6.5f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
	printf("Translation vector :\n");
	printf("t = < %6.5f, %6.5f, %6.5f >\n\n", matrix(0, 3), matrix(1, 3), matrix(2, 3));
}
//键盘回调函数
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event, void* nothing)
{
	//如果按下空格键，next_iteration=true
	if (event.getKeySym() == "space" && event.keyDown())
		next_iteration = true;
}

int main()
{
	 //定义需要用到的点云（读入的，转换的，ICP调整的三个点云） 
	PointCloudT::Ptr cloud_in_tgt(new PointCloudT);  // Original point cloud  （目标点云）
	PointCloudT::Ptr cloud_toberegistration(new PointCloudT);  // Transformed point cloud	待配准的点云
	PointCloudT::Ptr cloud_icp_in(new PointCloudT);   //filtered and icp input point cloud 经预处理后传入icp算法的目标点云
	PointCloudT::Ptr cloud_icp_toberegistration(new PointCloudT);  //filtered and icp output point cloud   经预处理后传入icp算法的待配准点云

	//时差=time.tok-time.tic 用于计时
	pcl::console::TicToc time;
	time.tic();
	std::string filename = "point_source/bun000_StructuredASCII.pcd";//点云文件名
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename, *cloud_in_tgt) == -1)//*打开点云文件	
	{
		PCL_ERROR("Couldn't read file test_pcd.pcd\n");//输出错误原因：读取文件失败
		return(-1);//程序结束
	}
	//输出读取点云的点数与点云读取时间
	std::cout << "\nLoaded file " << filename << " (" << cloud_in_tgt->size() << " points) in " << time.toc() << " ms\n" << std::endl;
	/*将读取的点云复制，以提供配准传入的参数以及多个可视化窗口的显示
	* cloud_icp_in将作为icp算法的目标点云参数传入
	* cloud_toberegistration是icp算法的源点云（待配准点云）
	*/
	*cloud_icp_in = *cloud_in_tgt;
	*cloud_toberegistration = *cloud_in_tgt;

	//体素降采样滤波
	pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;//创建滤波器对象
	voxel_grid.setLeafSize(0.01, 0.01, 0.01);//设置体素大小
	voxel_grid.setInputCloud(cloud_in_tgt);//输入待降采样的点云
	voxel_grid.filter(*cloud_icp_in);//降采样后放入cloud_icp_in
	std::cout << "down size *cloud_in_tgt to" << cloud_icp_in->size() << endl;
	//同理将另一个点云降采样
	voxel_grid.setInputCloud(cloud_toberegistration);
	voxel_grid.filter(*cloud_icp_toberegistration);
	std::cout << "down size *cloud_toberegistration to" << cloud_icp_toberegistration->size() << endl;

	// 定义旋转平移的转换矩阵
	Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();
	// A rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)  
	double theta = M_PI / 4;  // The angle of rotation in radians 
	transformation_matrix(0, 0) = cos(theta);
	transformation_matrix(0, 1) = -sin(theta);
	transformation_matrix(1, 0) = sin(theta);
	transformation_matrix(1, 1) = cos(theta);
	//Z轴平移0.4米
	 //A translation on Z axis (0.4 meters)  
	transformation_matrix(2, 3) = 0.0;
	//打印出旋转矩阵R和平移T
	std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;
	print4x4Matrix(transformation_matrix);
	std::cout << std::endl << std::endl << std::endl;

	//用矩阵transformation_matrix将点云进行空间变换，得到的点云和目标点云间就有了空间的旋转平移关系，后面使用icp算法将其配准还原
	pcl::transformPointCloud(*cloud_toberegistration, *cloud_toberegistration, transformation_matrix);
	pcl::transformPointCloud(*cloud_icp_toberegistration, *cloud_icp_toberegistration, transformation_matrix);

	// Visualization 可视化 
	pcl::visualization::PCLVisualizer viewer("ICP demo");//窗口标题
	// Create two vertically separated viewports  
	int v1(0);//设置4个小窗口
	int v2(1);
	int v3(2);
	int v4(3);
	//设置小窗口的位置分布
	viewer.createViewPort(0.0, 0.5, 0.5, 1.0, v1);
	viewer.createViewPort(0.5, 0.5, 1.0, 1.0, v2);
	viewer.createViewPort(0.0, 0.0, 0.5, 0.5, v3);
	viewer.createViewPort(0.5, 0.0, 1.0, 0.5, v4);
	// The color we will be using  
	float bckgr_gray_level = 0.0;  // Black  黑
	float txt_gray_lvl = 1.0 - bckgr_gray_level;//文本颜色与背景相反

	viewer.addCoordinateSystem(1.0);//设置坐标轴，1.0是坐标轴的可视大小
	// Original point cloud is white  目标点云设置为白色
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h(cloud_in_tgt, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl);
	//在3个窗口中添加目标点云，目标点云在可视化的过程中是不会动的
	viewer.addPointCloud(cloud_in_tgt, cloud_in_color_h, "cloud_in_v1", v1);//显示cloud_in_tgt第一次的点云
	viewer.addPointCloud(cloud_in_tgt, cloud_in_color_h, "cloud_in_v2", v2);
	viewer.addPointCloud(cloud_icp_in, cloud_in_color_h, "cloud_in_v3", v3);

	//Transformed point cloud is green  经目标点云旋转平移后并且未降采样的点云设置为绿色，放在窗口1中
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h(cloud_toberegistration, 20, 180, 20);
	viewer.addPointCloud(cloud_toberegistration, cloud_tr_color_h, "cloud_tr_v1", v1);

	// ICP aligned point cloud is red  经旋转平移后未降采样的点云设置为红色，放在窗口2，后面将随着迭代过程更新
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h(cloud_toberegistration, 180, 20, 20);
	viewer.addPointCloud(cloud_toberegistration, cloud_icp_color_h, "cloud_icp_v2", v2);

	// //ICP aligned point cloud is red  旋转平移并降采样的点云设置为红色，放在窗口3，用于icp的输入，并随着迭代过程更新位置
	viewer.addPointCloud(cloud_icp_toberegistration, cloud_icp_color_h, "cloud_icp_v3", v3);

	//Adding text descriptions in each viewport  添加文字信息
	viewer.addText("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);
	viewer.addText("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);

	/*icp迭代次数iterations设置为1时，算法执行一次迭代。算法内部是先执行一次迭代再判定当前迭代次数是否大于等于设定次数*/
	int iterations = 1;  // Default number of ICP iterations  icp迭代次数，设为1
	std::stringstream ss;
	ss << iterations;
	std::string iterations_cnt = "ICP iterations = " + ss.str();//显示算法已经迭代了多少次
	viewer.addText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);
	viewer.addText("Carlos Lee 202011", 10, 80, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "author", v2);

	//Set background color  设置背景颜色，上面已设定bckgr_gray_level=0.0，所以背景全黑
	viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
	viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);
	viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v3);

	//Set camera position and orientation  设置可视化窗口的初始视角
	viewer.setCameraPosition(-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
	viewer.setSize(1280, 1024);  // Visualiser window size  设置整个可视化窗口大小  

	 //Register keyboard callback :  //键盘回调函数，响应键盘输入
	viewer.registerKeyboardCallback(&keyboardEventOccurred, (void*)NULL);

	// The Iterative Closest Point algorithm  icp算法开始
	time.tic();
	pcl::IterativeClosestPoint<PointT, PointT> icp;
	icp.setMaximumIterations(iterations);//设置最大迭代次数，iterations在上面已经设置为0，即只使用一次icp算法
	icp.setInputSource(cloud_icp_toberegistration);//输入目标点云，是原读取的目标点云只经过降采样后的点云
	icp.setInputTarget(cloud_icp_in);//输入待配准点云，是原读取的目标点云经旋转平移并降采样后的点云
	icp.align(*cloud_icp_toberegistration);//经1次icp算法配准后的点云

	Eigen::Matrix4f icp_trans;
	icp_trans = icp.getFinalTransformation();//获取当前求解的旋转平移矩阵
	std::cout << endl;
	//使用得到的变换对未降采样的点云进行变换，以达到显示效果
	pcl::transformPointCloud(*cloud_toberegistration, *cloud_toberegistration, icp_trans);
	// We set this variable to 1 for the next time we will call .align () function  设置迭代次数为1，下一次使用键盘交互来完成一次迭代
	icp.setMaximumIterations(1);    
	std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc() << " ms" << std::endl;

	if (icp.hasConverged())//算法是否正常收敛
	{
		std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;//算法的mse（均方误差）
		std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;
		std::cout << endl << endl << endl;
	}
	else
	{
		//若不是则输出错误并停止
		PCL_ERROR("\nICP has not converged.\n");
		system("pause");
		return (-1);
	}
	//在2、3窗口更新上次经icp算法后的点云，这个地方注意看，之前是addpointcloud添加点云，这里是updatePointCloud是更新之前添加好的点云
	viewer.updatePointCloud(cloud_toberegistration, cloud_icp_color_h, "cloud_icp_v2");
	viewer.updatePointCloud(cloud_icp_toberegistration, cloud_icp_color_h, "cloud_icp_v3");

	//Display the visualiser  
	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
		// The user pressed "space" :  当键盘按下空格键，next_iteration变为true（见最上面的键盘回调函数代码），if里面的语句执行
		if (next_iteration)
		{
			// The Iterative Closest Point algorithm  
			time.tic();
			icp.align(*cloud_icp_toberegistration);//将上一次配准后的点云作为输入，进行下一次配准
			std::cout << "Applied 1 ICP iteration in " << time.toc() << " ms" << std::endl;

			icp_trans = icp.getFinalTransformation();
			std::cout << "trans matrix:\n " << /*icp_transdisplay**/icp_trans << endl;

			std::cout << endl;
			//使用创建的变换对过滤及未过滤的输入点云进行变换
			pcl::transformPointCloud(*cloud_toberegistration, *cloud_toberegistration, icp_trans);

			if (icp.hasConverged())
			{
				printf("\033[11A");  // Go up 11 lines in terminal output.  
				printf("\nICP has converged, score is %+.0e\n", icp.getFitnessScore());
				std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_in" << std::endl;
				std::cout << endl << endl << endl;
				//更新显示信息与点云
				ss.str("");
				ss << iterations;
				std::string iterations_cnt = "ICP iterations = " + ss.str();
				viewer.updateText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt");
				viewer.updatePointCloud(cloud_toberegistration, cloud_icp_color_h, "cloud_icp_v2");
				viewer.updatePointCloud(cloud_icp_toberegistration, cloud_icp_color_h, "cloud_icp_v3");
			}
			else
			{
				PCL_ERROR("\nICP has not converged.\n");
				system("pause");
				return (-1);
			}
		}
		next_iteration = false;//next_iteration设为false，等再次按下空格键时next_iteration=true再进行配准
	}
	system("pause");
	return (0);
}

完整工程在此

share_noel/PCL/icp/202011icp_registration-Carlos_Lee.zip
https://blog.csdn.net/qq_41102371/article/details/125646840
愿意用c币支持的朋友也可在此下载：
202011icp_registration-Carlos_Lee.zip
（上述下载链接中csdn与网盘的文件完全相同，只不过网盘免费下载）

参考及感谢

郭浩, 朱德海, 苏伟, 王可. 点云库PCL从入门到精通. 北京: 机械工业出版社; 2019.
点云配准一：配准基础及icp算法
vs2019配置pcl1.10.0+点云可视化示例
利用SVD求得两个对应点集合的旋转矩阵R和转移矩阵t的数学推导
由空间三对点求解两个坐标系之间的变换矩阵
【PCL学习】点云配准——ICP

完

边学边用，如有错漏，敬请指正
--------------------------------------------------------------------------------------------诺有缸的高飞鸟202011

诺有缸的高飞鸟
关注
  • 35
    点赞
  • 踩
  • 200
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 21
    评论
专栏目录
订阅专栏
PCL 点到面的ICP算法实现点云配准（C++详细过程版）
点云侠的博客
06-29 1550
[1] Low K L. Linear least-squares optimization for point-to-plane icp surface registration[J]. Chapel Hill, University of North Carolina, 2004, 4(10): 1-3 计算方法详细过程复现
PCL :实现点到面的ICP算法（附完整源码）
希望我的博客，能帮上你解决学习中工作中所遇到的问题
09-25 244
PCL :实现点到面的ICP算法（附完整源码）
PCL库源代码阅读—配准模块(Registration)—ICP算法
不知名的代码小白
05-23 693
IterativeClosestPoint类是PCL库中用来实现经典ICP算法的类，它实现了源点云、目标点云的设置，并通过computeTransformation函数实现了变换矩阵的估计。computeTransformation函数实现了迭代求解最优变换矩阵，通过correspondence_estimation_估计对应关系，通过convergence_criteria_判断收敛，通过transformation_estimation_求解变换矩阵。
PCL库源代码阅读—配准模块(Registration)—收敛标准(ConvergenceCriteria)
最新发布
不知名的代码小白
05-24 964
pcl库中收敛条件的判断是通过ConvergenceCriteria模块的相关文件实现的，具体实现文件为convergence_criteria.h，default_convergence_criteria.h，default_convergence_criteria.hpp。 收敛标准模块为点云配准提供不同的收敛标准，该模块作为具体配准算法的一部分来使用，收敛标准主要为： 1、迭代次数达到阈值；2、变换矩阵的变化值小于阈值；3、均方误差(MSE)的变化值小于阈值；
【点云配准】迭代最近点(ICP)配准算法
点云兔子的博客
09-23 2912
点云配准（Point Cloud Registration）指的是输入两幅点云(source)和(target) ，输出一个变换T使得T(Ps)和T(Pt)的重合程度尽可能高；Ps和Pt是源点云和目标点云中的对应点。变换T可以是刚性的(rigid)，也可以不是，一般只考虑刚性变换，即变换只包括旋转、平移。点云配准可以分为粗配准（Coarse Registration）和精配准（Fine Registration）两步。
ICP代码实现
qq_45152711的博客
05-24 486
ICP（Iterative Closest Point）是一种常用于点云配准的算法，其主要思想是迭代地计算两个点云之间的最优变换矩阵。在实际应用中，ICP算法可以用来进行三维重建、机器人导航等工作。首先，我们需要加载两个点云并对它们进行预处理。预处理包括对点云进行滤波、去除离群点等操作。接下来，我们需要实现ICP算法的主要步骤。
实现点云配准ICP算法以及个人代码理解
vhzfhf的博客
03-31 606
打开build中的sln文件，并打开bluid中的源码，修改第48行实验用的点云模型的绝对地址，运行。前两篇 1.配置好了PCL库；2.会使用cmake编译源文件并查看结果。写好两个文件后用之前说过的方法cmake编译，一共得到如下。得到如下结果，按下空格迭代一次，迭代25次后，基本配准。新建CMakeLists.txt文件，并复制以下代码。将源码复制到新建的main.cpp文件中。现在简单实现一下ICP算法，并理解代码。再下载实验用的点云模型，地址。
基于PCL开源库ndt+icp算法实现点云配准实例测试代码
03-27
利用PCL开源库编写代码NDT+ICP算法实现点云高精度配准，包括粗配准+精配准，并计算配准误差！基于PCL库版本1.9！
点云配准算法四点法+icp代码，C++ ,PCL库
04-19
点云配准算法四点法代码，需要读取pcd文件，如果要读取ply文件需要自己改代码，或者将ply文件转换为pcd文件
icp.zip_ICP算法_PCL点云配准_icp_点云算法_点云配准
07-14
3维点云的配准基本算法，基于pcl库的icp算法程序
基于PCL开源库fpfh+icp算法实现点云配准
03-27
利用PCL开源库编写代码FPFH+ICP算法实现点云高精度配准，并计算配准误差！基于PCL库版本1.9！
icp点云配准代码python
11-20
简单的icp配准方法，python写的，只能实现简单的配准，用于做对比试验。简单的icp配准方法，python写的，只能实现简单的配准，用于做对比试验。
icp的源代码
11-03
著名的迭代就近点算法（icp）的源代码，在20世纪80年代中期，很多学者开始对点集数据的配准进行了大量研究。1987年，Horn[1]、Arun[2]等人用四元数法提出点集对点集配准方法。这种点集与点集坐标系匹配算法通过实践证明是一个解决复杂配准问题的关键方法。1992年，计算机视觉研究者Besl和Mckay[3]介绍了一种高层次的基于自由形态曲面的配准方法，也称为迭代就近点法ICP（Iterative Closest Point）。以点集对点集（PSTPS）配准方法为基础，他们阐述了一种曲面拟合算法，该算法是基于四元数的点集到点集配准方法。从测量点集中确定其对应的就近点点集后，运用Faugera和Hebert提出的方法计算新的就近点点集。用该方法进行迭代计算，直到残差平方和所构成的目标函数值不变，结束迭代过程。ICP配准法主要用于解决基于自由形态曲面的配准问题。
ICP算法源码，调试通过
04-06
ICP算法，即Iterative Closest Points Algorithm。基本思想是：根据某种几何特性对数据进行匹配，并设这些匹配点为假想的对应点，然后根据这种对应关系求解运动参数。再利用这些运动参数对数据进行变换。并利用同一几何特征，确定新的对应关系，重复上述过程。
ICP配准程序
06-27
ICP配准程序
ICP算法代码-有图形显示界面
12-13
一个非常好的学习ICP算法示例工程。Matlab代码齐全，含有测试数据，可以显示迭代后配准效果。对于学习图形、图像配准非常有帮助。
ICP点云配准算法源码
12-18
ICP（Iterative Closest Point迭代最近点）算法是一种点集对点集配准方法。该源码通过CMake+VTK，使用C++实现.
ICP算法实现(MATLAB)
热门推荐
dayuhaitang1的博客
03-06 1万+
这几天因为正在学习点云数据，所以就学习了一下ICP算法。 文章目录一、算法步骤二、实现代码及效果三、总结 一、算法步骤 （1）在目标点云P中取点集pi∈P； （2）找出源点云Q中的对应点集qi∈Q，使得||qi-pi||=min； （3）计算旋转矩阵R和平移矩阵t，使得误差函数最小； （4）对pi使用上一步求得的旋转矩阵R和平移矩阵t进行旋转和平移变换，的到新的 对应点集pi’={pi’=Rpi+...
Open3D 中的点到面的 ICP 配准算法
bug_api163的博客
09-28 108
ICP（Iterative Closest Point）是一种常用的点云配准算法，它通过迭代的方式不断优化点云之间的对应关系，最终实现点云的配准。Open3D 是一个开源的库，提供了丰富的点云处理功能，其中就包括了点到面的 ICP 配准算法。综上所述，Open3D 中的点到面的 ICP 配准算法提供了一种高效、准确的点云配准方法，方便了点云处理和三维重建的应用。以上是本文对 Open3D 中的点到面的 ICP 配准算法的介绍，希望能对读者在点云配准领域的研究和应用有所帮助。函数进行点云配准，其中。
用PCL写个fpfh+icp的点云配准算法
05-24
好的，下面是用PCL实现fpfh+icp的点云配准算法的代码。 首先，我们需要包含PCL库的头文件和一些必要的命名空间： ```cpp #include <iostream> #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/registration/icp.h> #include <pcl/features/normal_3d.h> #include <pcl/features/fpfh.h> ``` 然后，我们定义一些类型和变量： ```cpp typedef pcl::PointXYZ PointT; typedef pcl::PointCloud<PointT> PointCloudT; PointCloudT::Ptr source, target; // 源点云和目标点云 PointCloudT::Ptr source_keypoints, target_keypoints; // 源点云和目标点云的关键点 pcl::PointCloud<pcl::Normal>::Ptr source_normals, target_normals; // 源点云和目标点云的法线 pcl::FPFHEstimation<PointT, pcl::Normal, pcl::FPFHSignature33>::Ptr fpfh_estimator; // FPFH 估计器 pcl::IterativeClosestPoint<PointT, PointT> icp; // ICP 配准器 ``` 接着，我们加载源点云和目标点云，并对它们进行下采样： ```cpp source.reset(new PointCloudT); target.reset(new PointCloudT); // 加载源点云和目标点云 pcl::io::loadPCDFile("source.pcd", *source); pcl::io::loadPCDFile("target.pcd", *target); // 对源点云和目标点云进行下采样 pcl::VoxelGrid<PointT> sor; sor.setLeafSize(0.01f, 0.01f, 0.01f); sor.setInputCloud(source); sor.filter(*source); sor.setInputCloud(target); sor.filter(*target); ``` 然后，我们对源点云和目标点云计算法线和关键点，并计算FPFH特征描述子： ```cpp // 计算源点云和目标点云的法线 pcl::NormalEstimation<PointT, pcl::Normal> ne; pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>()); ne.setSearchMethod(tree); ne.setInputCloud(source); ne.setKSearch(20); source_normals.reset(new pcl::PointCloud<pcl::Normal>); ne.compute(*source_normals); ne.setInputCloud(target); target_normals.reset(new pcl::PointCloud<pcl::Normal>); ne.compute(*target_normals); // 计算源点云和目标点云的关键点 pcl::UniformSampling<PointT> uniform_sampling; uniform_sampling.setInputCloud(source); uniform_sampling.setRadiusSearch(0.02f); source_keypoints.reset(new PointCloudT); uniform_sampling.compute(*source_keypoints); uniform_sampling.setInputCloud(target); target_keypoints.reset(new PointCloudT); uniform_sampling.compute(*target_keypoints); // 计算源点云和目标点云的 FPFH 特征描述子 fpfh_estimator.reset(new pcl::FPFHEstimation<PointT, pcl::Normal, pcl::FPFHSignature33>); fpfh_estimator->setInputCloud(source_keypoints); fpfh_estimator->setInputNormals(source_normals); fpfh_estimator->setSearchMethod(tree); fpfh_estimator->setRadiusSearch(0.05f); pcl::PointCloud<pcl::FPFHSignature33>::Ptr source_features(new pcl::PointCloud<pcl::FPFHSignature33>); fpfh_estimator->compute(*source_features); fpfh_estimator->setInputCloud(target_keypoints); fpfh_estimator->setInputNormals(target_normals); pcl::PointCloud<pcl::FPFHSignature33>::Ptr target_features(new pcl::PointCloud<pcl::FPFHSignature33>); fpfh_estimator->compute(*target_features); ``` 最后，我们使用ICP进行点云配准： ```cpp // 设置ICP参数 icp.setInputSource(source_keypoints); icp.setInputTarget(target_keypoints); icp.setMaxCorrespondenceDistance(0.1f); icp.setMaximumIterations(50); icp.setTransformationEpsilon(1e-8); icp.setEuclideanFitnessEpsilon(1e-6); // 设置ICP的匹配特征 pcl::PointCloud<pcl::PointNormal>::Ptr aligned_source(new pcl::PointCloud<pcl::PointNormal>); pcl::concatenateFields(*source_keypoints, *source_normals, *aligned_source); pcl::PointCloud<pcl::PointNormal>::Ptr aligned_target(new pcl::PointCloud<pcl::PointNormal>); pcl::concatenateFields(*target_keypoints, *target_normals, *aligned_target); icp.setSourceFeatures(source_features); icp.setTargetFeatures(target_features); icp.align(*aligned_source); // 输出配准结果 std::cout << "ICP has converged with score: " << icp.getFitnessScore() << std::endl; std::cout << "Transformation matrix:" << std::endl << icp.getFinalTransformation() << std::endl; ``` 以上就是用PCL实现fpfh+icp的点云配准算法的完整代码。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 21
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

诺有缸的高飞鸟

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值