ORB-SLAM2稠密点云重建:RGBD室内[2]
接上一篇。
步骤8：从rgb图和d图初始化点云，然后滤波（可选）
cout << "Initing...\n";
//初始化点云
vector pcVec;
for (int i = 0; i < keyVec.size(); i++) {
FRAME tframe;
tframe.rgb = cv::imread(rgbPathVec[i]);
tframe.depth = cv::imread(dPathVec[i], -1);
pcVec.push_back(image2PointCloud(tframe.rgb, tframe.depth, camera));
}
cout << "Filtering...\n";
//滤波
bool bFilter = true;
if (bFilter) {
for (auto& pciter : pcVec) {
PointCloud::Ptr cloud_filtered(new PointCloud());
pcl::VoxelGrid sor;
sor.setInputCloud(pciter);
sor.setLeafSize(0.01f, 0.01f, 0.01f);
sor.filter(*cloud_filtered);
pciter = cloud_filtered;
PointCloud::Ptr cloud_filtered2(new PointCloud());
pcl::StatisticalOutlierRemoval sor2;
sor2.setInputCloud(cloud_filtered);
sor2.setMeanK(50);
sor2.setStddevMulThresh(1.0);
sor2.filter(*cloud_filtered2);
pciter = cloud_filtered2;
}
} 初始化点云使用了高博的image2PointCloud函数，滤波使用了PCL官方教程上的体素滤波和离散值滤波，之前都有提过链接。
现在,pcVec中有了每个滤过波的关键帧的点云了。
步骤9：使用关键帧文件信息进行点云拼接
这一部分最会出坑的地方是对于输出信息的理解，还有四元数到旋转矩阵的转化公式，位移的正负。
从代码上可以看出，ORB-SLAM2的RGBD输出文件每行有8个数字，我以我跑的room出来的关键帧文件第一行为例进行解释：
1305031910.765238 0.0001796 -0.0002834 0.0001877 -0.0000384 -0.0001034 -0.0001286 1.0000000第一个数字是时间戳，在此也就是rgb文件名；后面3个数字是世界位置的负数，注意是负数！这是一个大坑。如果没有闭环检测，应该是0,0,0，但这里有，对第一个点进行了调整，所以不是了；再后面4个数字是表示世界旋转，同理，没有闭环检测应该是0,0,0,1。这里要注意的是每行都是世界坐标与旋转（没有闭环检测的话，世界原点是第一帧），我被其源码注释搞糊涂了，以为是每帧相对上一帧的变换，运行出来发现并不是。
接下来是四元数转换的坑，我就不赘述，使用下面的代码就可以了。
思路就是关键帧既然给了我们世界位置的负数，四元数。我们对pcVec中每个点云，位置直接加上世界位置的负数，旋转使用四元数对应的旋转矩阵的逆，就能将所有点云对齐到同一个原点。代码：
for(int i=0;i(3, 3)<<
2*(x*x+w*w)-1,2*(x*y+z*w),2*(x*z-y*w),
2*(x*y-z*w), 2*(y*y+w*w)-1, 2*(y*z+x*w),
2*(x*z+y*w), 2*(y*z-x*w), 2*(z*z+w*w)-1
);
R = R.inv();
Eigen::Matrix3d r;
cv::cv2eigen(R, r);
// 将平移向量和旋转矩阵转换成变换矩阵
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
Eigen::AngleAxisd angle(r);
cout << "translation" << endl;
T = angle;
T(0, 3) = keyVec[i].tx;
T(1, 3) = keyVec[i].ty;
T(2, 3) = keyVec[i].tz;
PointCloud::Ptr toutput(new PointCloud());
pcl::transformPointCloud(*pcVec[i], *toutput, T.matrix());
pcVec[i] = toutput;
}
cout << "trans over\n";
步骤10：组合点云，保存，使用PCL展示
在编码的过程中我也尝试了PCL的一些registration算法（比如常见的ICP），最终还是不使用了。有以下几个原因：
- 闭环检测了的室内ORB-SLAM2的RGBD位姿结果足够精确，registration没有意义
- registration的这些算法耗时长，不能保证效果，有些算法还需要不错的初始猜测参数
我虽然放在了代码里，但是没有用。allOutput是最终组合起来的大点云，进行了保存和展示。代码：
int algoFlag = 0;
bool bNDT = false;
int ndtindex = 0;
bool bPCH = false;
if (algoFlag==0) {
for (int i = 0; i < pcVec.size(); i++) {
*allOutput += *pcVec[i];
}
}
else if (algoFlag == 1) {
allOutput = pcVec[0];
for (int i = 0; i < pcVec.size() - 1; i++) {
PairwiseICP(pcVec[i + 1], allOutput, allOutput);
}
}
else if(algoFlag==2){
//记录每相邻两个点云ICP的矩阵
vector icpMatVec;
vector> funcVec;
vector taskVec;
for (int i = 0; i < pcVec.size()-1; i++) {
if (bPCH) {
funcVec.push_back([&]() {icpMatVec.push_back(XCPairwisePCH(pcVec[i], pcVec[i + 1])); });
}
else if(bNDT&&i==ndtindex){
funcVec.push_back([&]() {icpMatVec.push_back(XCPairwiseNDT(pcVec[i], pcVec[i + 1])); });
}
else {
funcVec.push_back([&]() {
icpMatVec.push_back(XCPairwiseICP(pcVec[i], pcVec[i + 1]));
/*double t1 = icpMatVec[i](0, 3);
double t2 = icpMatVec[i](1, 3);
double t3 = icpMatVec[i](2, 3);
double r1 = icpMatVec[i](0, 0);
double r2 = icpMatVec[i](0, 1);
double r3 = icpMatVec[i](0, 2);
double r4 = icpMatVec[i](1, 0);
double r5 = icpMatVec[i](1, 1);
double r6 = icpMatVec[i](1, 2);
double r7 = icpMatVec[i](2, 0);
double r8 = icpMatVec[i](2, 1);
double r9 = icpMatVec[i](2, 2);
cout << i+1<<"-"<
至此JoinPC这个主函数就全部填充完毕。
好了，基于ORB-SLAM2的RGBD室内重建至此就完成了。由于公司不能上传图片与文件，最终效果截图与完整代码将在介绍完“ORB-SLAM2稠密点云重建：双目室外”后的一篇独立博文中给出。