C++实现ICP点云匹配

最新推荐文章于 2024-08-18 03:50:37 发布

JoannaJuanCV

最新推荐文章于 2024-08-18 03:50:37 发布

阅读量5.7k

点赞数 5

分类专栏： PCL学习

本文链接：https://blog.csdn.net/zfjbit/article/details/93896111

版权

PCL学习专栏收录该内容

80 篇文章 134 订阅 ¥19.90 ¥99.00

订阅专栏

超级会员免费看

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

摘要由CSDN通过智能技术生成

代码流程说明：

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