RGB-D SLAM程序框架总结4.点云拼接

点云拼接方法及误差分析

最新推荐文章于 2025-05-17 11:43:32 发布

原创最新推荐文章于 2025-05-17 11:43:32 发布 · 890 阅读

3 ·

CC 4.0 BY-SA版权

一起动手做rgb-D 专栏收录该内容

9 篇文章

订阅专栏

本文介绍点云拼接技术，先估计图像相对运动，用旋转矩阵将第一帧点云转换到第二帧坐标系下进行拼接。详细说明了转换矩阵、点云转换及相加的代码实现，还提到点云显示处理。最后指出随着拼接点云增多，误差逐渐增大，并给出参考文献。

本节代码见git@github.com:jlm345/RGB-D.git
点云拼接是针对相邻帧的，先估计图像的相对运动，然后使用旋转矩阵把第一帧对应的点云转换到第二帧点云的坐标系下，使得两帧点云处在同一个坐标系，然后进行拼接形成融合了两个点云的新点云
首先是按照上一步分产生点云，然后估计相机运动，得出旋转矩阵和平移矩阵，通过这两个矩阵把第一帧的坐标系转换到与第二帧一致的坐标系下，细节如下

把solvePnPRansac得到的旋转向量rvec和平移向量tvec转换到Eigen模块对应的李群SE(3)，代码段如下，这个转换过程需要使用到函数cv::cv2eigen(),包含在头文件#include <opencv2/core/eigen.hpp>//for cv::cv2eigen

//由旋转向量rvec和平移向量tvec得到SE（3）T
	//T是SE(3)特殊欧式群里的元素
	Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
	cv::Mat R;
	cv::Rodrigues(rvec,R);
	Eigen::Matrix3d r;
	cv::cv2eigen(R,r);
	Eigen::AngleAxisd angle(r);
	T=angle;
	T(0,3) =tvec.at<double>(0,0); 
    T(1,3) = tvec.at<double>(0,1); 
    T(2,3) = tvec.at<double>(0,2);
	cout<<"T= "<<T.matrix()<<endl;

得到矩阵后把第一帧的点云coud1转换到第二帧相机坐标系下的点云代码如下

PointCloud::Ptr output(new PointCloud);
pcl::transformPointCloud(*cloud1,*output,T.matrix());

然后把转换了的点云与第二帧图像的点云相加如下，得到的output就是相应的点云指针

*output+=*cloud2;
pcl::io::savePCDFile("/data/result.pcd",*output);

使用上述函数需要包含eigen相关文件

//Eigen!
#include<Eigen/Core>
#include<Eigen/Geometry>

在CMakeLists.txt中也要加上下面一行

include_directories( "/usr/include/eigen3")

拼接了两幅图像的点云的图像如下
在这里插入图片描述直接得到的点云是一幅颠倒的结构，要么进行手动旋转，要么对需要显示的点云进行以下处理（只是为了显示方便），注意到此处的取反轴是y和z，与上一节的数学计算公式取反的轴不相同，具体原因未知

for(size_t i=0;i<output->points.size();i++)
	{
		output->points[i].y=-output->points[i].y;
		output->points[i].z=-output->points[i].z;
	}

这样就可以得到方便易看的点云了，而且因为是拼接未完成后再进行取反处理，对拼接过程也不会有影响

像这样一直拼接每一幅图像对应地点云就是一个Visual Odometry了（加上一定的筛选条件）,下面是拼接了前30帧，和前一100帧的点云图，可以发现随着拼接的点云越来越多，误差逐渐增大
30帧拼接的点云
在这里插入图片描述 100帧拼接的点云

参考文献https://www.cnblogs.com/gaoxiang12/p/4633316.html系列教程
毛星云《OpenCV3编程入门》