ORB_SLAM : semi dense code

2016.9.20 update：添加论文中部分公式推导。
2016.9.9 update：完善部分代码
2016.9.7：初始博客

单目摄像头的Semi Dense 听起来很美，比如有人想用这个结合机器人导航，有人用它和三维物体识别相结合，用来同时提升定位和识别率。最近对单目semi dense也有了一些想法，想先按着ORB_SLAM作者论文实现下semi dense,练练手:《Probabilistic Semi Dense Mapping from Highly Accurate Feature-Based Method Monocular SLAM》. 之前，武汉大学王老师实验室找到了github 上Loeing的code，但是bug真心多: epipolar search有问题，depth 计算有问题，程序流程有问题等等。

在他的基础上，我接着做了完善，现在程序已经完成到论文III-D部分了，论文III-E部分为关键帧之间深度的checking and smoothing还没加入到整个系统.
目前的效果如下：
这里写图片描述

还要完善的部分：
１．把颜色添加到显示(这个比较容易)
２．论文III-E添加到系统里，这部分很必要: 目前重建出的outlier还很多，需要帧间互检。
３．整个semi dense的数据结构最好修改下。因为要不断遍历已经计算好depth的像素，现在代码用向量结构体保存depth map, 发现这种形式特慢，直接借用cv::Mat数据结构快了好多，如depth保存到cv::mat, depth的标准差depth_sigma也保存到Mat，比用结构体struct{float depth; float sigma}d, 向量结构体vector< d >保存的形式快很多。

代码已上传到my github: semi dense code，直接改的ORB_SLAM2，程序的运行方式是ORB_SLAM2的运行方式，没作改动。不过我把字典的加载方式改成了bin,不再是那个比较慢的txt的方式。切换到examples/monocular文件夹下，执行:

./mono_semidense ../../Vocabulary/ORBvoc.bin TUM2.yaml /home/hyj/datasets/freiburg2_desk

欢迎大家来一起修改和完善它, 最近苦逼的项目又多了起来，有点力不从心啊啊啊。

———————————————-2016.09.09更新————————————————————-
1.颜色附着。（完成）
2.论文III-D部分对那些极线匹配失败但是梯度大的像素点用周围成功点的平均深度进行赋值。（已完成）
3. III-E 关键帧之间的检测去噪点已完成，对深度点优化没完成。
4. 目前程序里定义了宏定义InterKeyFrameChecking，用来确定使不使用论文III-E部分。
目前效果如图：
这里写图片描述
从这个角度看上去不错，其实整个系统跑完还是很渣。极线匹配有很大问题，同时，多帧之间的点云重影太厉害，不知道位姿换算是不是出问题了，有待进一步代码检查。

———————————————-2016.09.20更新————————————————————-
论文中极线搜索部分，作者构造了极线匹配的损失函数公式(3):
这里写图片描述
通过不断沿着极线进行单位步进搜索可以找到一个初始的像素 $u_0$ ，使得（3）式最小。注意，由于搜索的时候是单位步进，这时候找到的 $u_0$ 还不是亚像素级精度，只能说 $u_0$ 非常靠近真值了。要求最小值，就需要对（3）式求导令其导数等于0。求导得（4）式：
这里写图片描述
将 $u_0$ 代入4式，由于不是最优值，此时的导数还不等于0。那么我们假设 $u_0 + \delta u$ 就是真值，此时偏导数等于0，就能求出 $u^*$ . 计算公式如下：

到这里就能就能得到论文中的公式(6)了。
公式7是计算 $u^*$ 的协方差，这个由误差传播理论就很容易计算了。因为 $u^*$ 是由最小化像素残差和梯度残差得到的，所以他的协方差计算是由这两者的协方差传播过来的：
这里写图片描述
于是我们就得到了公式7.

论文里面其他公式都不难，如果有问题，欢迎讨论。

（转载请注明作者和出处： http://blog.csdn.net/heyijia0327 未经允许请勿用于商业用途）

ORB_SLAM : semi dense code

猜你喜欢