SLAM会议笔记（二）Real-time DEMO

Real-time Depth Enhanced Monocular Odometry

Abstract

利用图像和稀疏的深度图做运动估计，同时利用三角量测和运动估计得到位置深度的突出图像特征点的深度，使用光束平差法(BA)对运动估计做修正。

SYSTEM OVERVIEW

帧间运动估计：

对于有深度信息的图像特征点，运动描述如下，只使用两帧中的前一帧的深度信息，无深度信息的点归一化表示为 ，其中Z轴坐标为1。


联立三行等式消去深度值得到等式


对于无深度信息的特征点表示如下，联立得到等式：


根据罗德里格斯公式，旋转矩阵R可以由向量 表示，所以式（3）（4）（6）可以表示为

作为非线性优化的损失函数，使用非线性最小二乘LM法求解。

特征点深度提取：

将深度图存储在一个2D KD-tree中，对于每一个特征点，在KD-tree中找到三个与之最接近的点，将特征点投影到三个最近点构成的平面上得到特征点的深度。

光束平差法：

调用开源的iSAM库可以方便的处理有无可用深度信息的特征点。以一个较低的频率运行，每5帧中选一帧，每40帧中选8帧进行批处理优化。