第四講 點雲的拼接

在這一講裡，使用上一講的旋轉和平移量拼接點雲，形成更大的點雲圖。

話不多說，直接上程式碼：

在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