C++实现ICP点云匹配

最新推荐文章于 2023-10-30 20:00:21 发布

JoannaJuanCV

最新推荐文章于 2023-10-30 20:00:21 发布

阅读量6k

点赞数 5

CC 4.0 BY-SA版权

分类专栏： PCL学习

本文链接：https://blog.youkuaiyun.com/zfjBIT/article/details/93896111

PCL学习专栏收录该内容

80 篇文章 ¥19.90 ¥99.00

订阅专栏

超级会员免费看

本文介绍了使用C++实现ICP（Iterative Closest Point）算法进行点云匹配的过程，包括读取源点云、构造目标点云、初始化变换参数、迭代优化直至满足停止条件。在实际应用中，ICP算法可能陷入局部极值，需要进一步的优化策略来提高匹配精度。

代码流程说明：

1. 读取源点云SC；

2. 构造目标点云TC；

3. 初始化旋转矩阵和平移矩阵；

4. 根据旋转矩阵和平移矩阵，变换目标点云，得到新的目标点云TC_1；

5. 查找近邻点对，得到重排序的源点云SC_1；

6. 分别去中心得到SC_2,TC_2：

7. 对应点对矩阵和，SVD分解得到新的旋转矩阵和平移矩阵；

重复4-7步骤，直到对应点对位置差小于预设阈值；

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/eigen>

using namespace cv;
using namespace std;

void calNearestPointPairs(Eigen::Matrix3f R, Eigen::Vector3f t, vector<Point3f>& pts1, vector<Point3f>& pts2, vector<Point3f>& outPoints, vector<float> &error)
{
	outPoints.clear();
	error.clear();

	int num = pts1.size();
	//对目标点云进行变换

	for (int i = 0; i < num; i++)
	{
		float x = Eigen::Vector3f(R(0, 0), R(0,

了解本专栏