ORB-SLAM2稠密点云重建：双目室外[2]

接上一篇.

上一篇中，看起来我们基本完成了重建。但是有很多问题：

1. 问题1：如果你真正跑起来（00数据集），你会发现点云没有正确拼接。这是点云化的部分有问题。
2. 问题2：跑起来，你会发现很乱，除了问题1，还有是因为y方向剪裁不够，即还有很多天空像天花板一样贴在了顶上，所以从上面看是看不到道路的。
3. 问题3：乱的原因除了问题1，2，还有是因为z方向剪裁仍然不够。实际操作发现超过一小段距离点云就开始出现“裂缝”，也是从此距离开始，其实往后大段的点云都不能用。

接下来具体分析原因，并解决。

问题1.

在上一篇中，看我们对点云位置进行偏移，对齐原点：

double tscale = 1;  
  
        T = angle;  
        T(0, 3) = keyVec[i].tx / tscale;  
        T(1, 3) = keyVec[i].ty / tscale;  
        T(2, 3) = keyVec[i].tz / tscale;  

这个tscale应该改成5。为什么？因为在我们在image2PointCloudInverse中点云化图片的时候：

pix = cvGet2D(img, m, n);
			PointT p;
			// 计算这个点的空间坐标
			p.z = camera.fx*0.3861448/pix.val[0];
			p.x = (n - camera.cx) * p.z / camera.fx;
			p.y = (m - camera.cy) * p.z / camera.fy;

可以看出x,y尺度与z一致，fx,baseline的单位都是米，但是disparity不对了，它是由视差图得来的，而视差图是由相机拍的左右图得来的。如果你看过高博的文章或仔细看过TUM的RGBD的calibration，你会发现其中有一个scale。双目也有这个问题。00数据集双目相机经过elas得到的视差图的scale也是5000，即5米，那么disparity的单位就是5m，又Z=f*B/d,那么我们的点云现在的单位是0.2m。由于位移变换，文件的单位总是1m，所以如果要正确对齐点云，就要在位移时/5变成0.2m单位的。

所以解决办法:令tscale=5;

问题2，3。

问题2，当然是对y进行剪裁，我截取到树高的一半，这样从顶上也可以看见道路了。我也对x进行了更窄的截取，因为两边的实在太不准了。对于问题3，我对开始裂缝的z就舍去了（真的是舍去了一大段，视觉上90%也不夸张）。现在：

//减裁
	for (auto& pciter : pcVec) {
		PointCloud::Ptr cloud_filtered3(new PointCloud());
		//z剪裁
		pcl::PassThrough<pcl::PointXYZRGBA> zpass;
		zpass.setInputCloud(pciter);
		zpass.setFilterFieldName("z");
		zpass.setFilterLimits(0, 2.5);
		zpass.filter(*cloud_filtered3);
		pciter = cloud_filtered3;

		PointCloud::Ptr cloud_filtered4(new PointCloud());
		//x剪裁
		pcl::PassThrough<pcl::PointXYZRGBA> xpass;
		xpass.setInputCloud(cloud_filtered3);
		xpass.setFilterFieldName("x");
		xpass.setFilterLimits(-1000.0, 1000.0);
		xpass.filter(*cloud_filtered4);

		//y剪裁
		PointCloud::Ptr cloud_filtered5(new PointCloud());
		pcl::PassThrough<pcl::PointXYZRGBA> ypass;
		ypass.setInputCloud(cloud_filtered4);
		ypass.setFilterFieldName("y");
		ypass.setFilterLimits(-0.6, 10.0);
		ypass.filter(*cloud_filtered5);

		pciter = cloud_filtered5;
	}

如果细心你可以发现我为pcl viewer绑定了一个鼠标点击响应函数，shift+左键输出当前点坐标，这在确定剪裁范围的时候很有用。具体代码可以见后我后面单独一篇的截图与完整代码博文。

最后

      轨迹比较准确（ORB-SLAM2的准确度），可惜只能重建道路和道路边上一点的景物，对于道路周边比如房子，还有远处景物比如天空，那就对不起了，早就舍弃了，实在太不准了。也许以后基于物体识别的slam处理这些会比较好。

      值得一提的是，电脑是16G内存，基本重建个600帧就到顶了。各位如果有得出关键帧的方法还请指教。

      完整代码与截图会在之后一篇独立博文中给出。

 

注意（补充）！！！：

本文关于问题1的理解有误（结果碰巧凑对）。0.386那个原来是bf，而且关于scale=5000那个解释可能不对。

而且视差也照理说是pix[0]/255*camera_inx。

不过说实话，虽然原理好像改过来了，但结果还是不正确，到目前还没有解决。由于之前的结果又凑得正确，可能一段时间不会去解决。。

如果解决了，将在下一篇：ORB-SLAM2稠密点云重建：双目室外[3]中解决。