首先工具函数：将cv的旋转矢量与位移矢量转换为变换矩阵，类型为Eigen::Isometry3d；
src/slamBase.cpp
复制代码
1 // cvMat2Eigen
2 Eigen::Isometry3d cvMat2Eigen( cv::Mat& rvec, cv::Mat& tvec )
3 {
4 cv::Mat R;
5 cv::Rodrigues( rvec, R );
6 Eigen::Matrix3d r;
7 cv::cv2eigen(R, r);
8
9 // 将平移向量和旋转矩阵转换成变换矩阵
10 Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
11
12 Eigen::AngleAxisd angle(r);
13 Eigen::Translation<double,3> trans(tvec.at(0,0), tvec.at(0,1), tvec.at(0,2));
14 T = angle;
15 T(0,3) = tvec.at(0,0);
16 T(1,3) = tvec.at(0,1);
17 T(2,3) = tvec.at(0,2);
18 return T;
19 }
复制代码
另一个函数：将新的帧合并到旧的点云里：
复制代码
1 // joinPointCloud
2 // 输入：原始点云，新来的帧以及它的位姿
3 // 输出：将新来帧加到原始帧后的图像
4 PointCloud::Ptr joinPointCloud( PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera )
5 {
6 PointCloud::Ptr newCloud = image2PointCloud( newFrame.rgb, newFrame.depth, camera );
7
8 // 合并点云
9 PointCloud::Ptr output (new PointCloud());
10 pcl::transformPointCloud( original, output, T.matrix() );
11 newCloud += output;
12
13 // Voxel grid 滤波降采样
14 static pcl::VoxelGrid voxel;
15 static ParameterReader pd;
16 double gridsize = atof( pd.getData("voxel_grid").c_str() );
17 voxel.setLeafSize( gridsize, gridsize, gridsize );
18 voxel.setInputCloud( newCloud );
19 PointCloud::Ptr tmp( new PointCloud() );
20 voxel.filter( *tmp );
21 return tmp;
22 }
复制代码
另外，在parameters.txt中，我们增加了几个参数，以便调节程序的性能：
复制代码
part 5
数据相关
起始与终止索引
start_index=1
end_index=700
数据所在目录
rgb_dir=../data/rgb_png/
rgb_extension=.png
depth_dir=../data/depth_png/
depth_extension=.png
点云分辨率
voxel_grid=0.02
是否实时可视化
visualize_pointcloud=yes
最小匹配数量
min_good_match=10
最小内点
min_inliers=5
最大运动误差
max_norm=0.3
