RGB-D SLAM学习总结(4)

第四讲 点云的拼接

---

在这一讲里，使用上一讲的旋转和平移量拼接点云，形成更大的点云图。

---

话不多说，直接上代码：

在slamBase.h里添加两个方法：一个是将之前输出的相机运动的旋转量和平移量转换成转换矩阵，另一个用于拼接点云。

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

#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/voxel_grid.h>

#include <opencv2/core/eigen.hpp>

// rvec, tvec --> T
Eigen::Isometry3d cvMat2Eigen(cv::Mat& rvec, cv::Mat& tvec);

// 拼接点云
PointCloud::Ptr joinPointCloud(PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera);

#include <pcl/filters/voxel_grid.h>这个头文件是原博客里没有特别说明的，记得加进去哦~用于点云滤波，设置分辨率啥的。

对应的slamBase.cpp：

// rvec, tvec --> T
Eigen::Isometry3d cvMat2Eigen(cv::Mat& rvec, cv::Mat& tvec)
{
	cv::Mat R;
	cv::Rodrigues(rvec, R);
	Eigen::Matrix3d r;
	//cv::cv2eigen(R, r);
	for(int i=0; i<3; i++)
		for(int j=0; j<3; j++)
			r(i,j) = R.at<double>(i,j);
	
	Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
	
	Eigen::AngleAxisd angle(r);
	//Eigen::Translation<double, 3> trans(tvec.at<double>(0,0), tvec.at<double>(0,1), tvec.at<double>(0,2));
	T = angle;
	T(0,3) = tvec.at<double>(0,0);
	T(1,3) = tvec.at<double>(1,0);
	T(2,3) = tvec.at<double>(2,0);
	return T;
}

// joinPointCloud
// 输入：原始点云， 新来的帧以及它的位姿
// 输出：将新来的帧加到原始帧后的图像
PointCloud::Ptr joinPointCloud(PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera)
{
	PointCloud::Ptr newCloud = image2PointCloud(newFrame.rgb, newFrame.depth, camera);
	// 合并点云
	PointCloud::Ptr output (new PointCloud());
	pcl::transformPointCloud(*original, *output, T.matrix());
	*newCloud += *output;
	// Voxel grid 滤波降采样
	static pcl::VoxelGrid<PointT> voxel;
	static ParameterReader pd;
	double gridsize = atof(pd.getData("voxel_grid").c_str());
	voxel.setLeafSize(gridsize, gridsize, gridsize);
	voxel.setInputCloud(newCloud);
	PointCloud::Ptr tmp(new PointCloud());
	voxel.filter(*tmp);
	return tmp;
}

还有一点就是在CMakeLists加入visualization filters模块，这个在原博客里也没有提及：

FIND_PACKAGE(PCL REQUIRED COMPONENTS common io visualization filters)

至此，我们就可以实现一个简单的VO啦~

---

代码补充说明：

pcl::VoxelGrid<PointT>是头文件<pcl/filters/voxel_grid.h>中定义的类。其中：

setLeafSize(lx, ly, lz)设置xyz方向的点云分辨率；

setInputCloud(cloud)传入要显示的点云；

filter(newcloud)对点云进行下采样，滤除离群点，结果返回到newcloud