DEMO-lidar介绍

DEMO-lidar介绍

论文：《Real-time Depth Enhanced Monocular Odometry》
Real-time Depth Enhanced Monocular Odometry概述
DEMO-lidar源码解析
demo-rgbd解读
http://wiki.ros.org/demo_lidar

DEMO-lidar：Depth Enhanced Monocular Odometry (Demo)，是一个组合深度信息的单目视觉里程计。主要由三个主线程：

1. “feature tracking” 特征跟踪：提取并跟踪匹配角点(Harris corners)特征，使用Kanade Lucas Tomasi (KLT) 完成特征提取。
2. “visual odometry” 视觉里程计，基于帧与帧之间的特征点匹配关系，计算帧与帧之间的相机运动。其中附加上深度信息的特征点(通过深度图或通过估计相机运动的三角估计方法得到深度信息)解决6自由度运动估计，没有深度信息的特征点有助于解决方向估计。
3. “bundle adjustment” 光束法平差，BA优化，就是通过特征点投影在图像上的像素，来优化运动估计。

如果有IMU数据，转向测量值可由角速率和加速度积分得到，可用于优化滚转以及俯仰角，VO来优化偏航角和平移。

硬件：单目+（imu）+ lidar/rgbd
以下为算法框架图：

论文标识指代：

k指第k个关键帧。
C为系统坐标系，采用右手坐标系，x朝左，y朝上，z朝前。其实，使用z朝前，可以正好对应相机模型的深度信息。

大写I为一帧的特征点集合，i指代其中的特征点$i$i，$i \in I$i∈I。
设第k帧图像中，坐标系为$C^k$Ck，特征点i具有深度信息，则设其坐标为$X_i^k=[x_i^k, y_i^k, z_i^k]$Xik​=[xik​,yik​,zik​].
如果特征点没有深度信息，则使用归一化坐标$\overline{X}_i^k$Xik​来表示特征点i坐标，即$\overline X_i^k=[\overline x_i^k, \overline y_i^k, 1]$Xik​=[xik​,y​ik​,1]。

运动估计公式推导：

设两帧k-1与k帧之间的一对匹配点$X_i^{k-1}$Xik−1​
与$X_i^k$Xik​。则有$X_i^k=RX_i^{k-1}+T$Xik​=RXik−1​+T
1）假设$X_i^{k-1}$Xik−1​深度信息已知，但由于第k帧的运动估计尚未进行，因此，没有得到$X_i^k$Xik​的深度信息。因此，将$X_i^{k-1}$Xik−1​按归一化坐标表示，
即，$\overline X_i^k=[\overline x_i^k, \overline y_i^k, 1]$Xik​=[xik​,y​ik​,1]。
令$z_i^k$zik​为$X_i^k$Xik​深度信息。
则：
$z_i^k\overline X_i^k=RX_i^{k-1}+T$zik​Xik​=RXik−1​+T
因为$z_i^k$zik​未知，使用初等行变换消去$z_i^k$zik​。

将等式两侧，第一行 减去 第三行乘以$\overline x_i^k$xik​，得到论文公式(3):

将等式两侧，第二行 减去 第三行乘以$\overline y_i^k$y​ik​，得到论文公式(4):

其中$R_h及T_h$Rh​及Th​的下表h表示$R$R矩阵的第h行。

2）假设$X_i^{k-1}$Xik−1​深度信息未知，也使用归一化坐标$X_i^{k-1}$Xik−1​表示，
即，$\overline X_i^{k-1}=[\overline x_i^{k-1}, \overline y_i^{k-1}, 1]$Xik−1​=[xik−1​,y​ik−1​,1]。
得到匹配特征点之间的归一化坐标关系如下：
$z_i^k\overline X_i^k=z_i^{k-1} R\overline X_i^{k-1}+T$zik​Xik​=zik−1​RXik−1​+T
仍利用初等行变换，消去$z_i^k与z_i^{k-1}$zik​与zik−1​。
第一行 减去 第三行乘以$\overline x_i^k$xik​，
第二行 减去 第三行乘以$\overline y_i^k$y​ik​，
第一行乘以$\overline y_i^k$y​ik​ 减去 第二行乘以$\overline y_i^k$y​ik​，最后整理可得三个约束方程，论文公式(6)：

利用LM算法得到最优值。

运动估计算法流程

输入：$\overline X_i^k$Xik​(当前帧无深度信息)，$X_i^{k-1}$Xik−1​(上一帧有深度信息的点云)或$\overline X_i^{k-1}$Xik−1​(上一帧有深度信息的点云)
输出：$\theta, T$θ,T(旋转及平移变换估计)

根据上帧是否有深度信息，选择对应的约束方程，并迭代优化得到变换值。

深度特征点关联

深度点云降采样，以角度间隔均匀分布特征点(离相机越近，则点越密)。且以球坐标表示，即弦长、方位角(左右)、极角(上下)，变换到$C^{k-1}$Ck−1下。
以2D KD-tree存储特征点。设$i\in C^{k-1}$i∈Ck−1，在$C^{k-1}$Ck−1的kdtree中找到与i相近的3个点，记为：
则$X^{k-1}_i$Xik−1​的深度值通过解以下方程得到。

该方程与loam的点到面类似，只是没有分母项，意思是约束点i落到三个j点构成的平面上。

BA(Bundle Adjustment)后端优化

相对于前端的运动估计，BA优化采用批量优化，估计频率是前端odom的1/5。感觉优化思路类似，都是优化投影误差方程来得到运动估计，只是BA为批量优化，处理点云的点云为累积点云，而非单帧的点云。
约束方程：就是通过优化相同特征点在不同帧的图像中获得的特征点的投影坐标(可理解为像素，归一化坐标)，来估计得到更加准确的定位估计。
约束方程如下：

其中：$\widetilde X_i^j$Xij​表示特征点i在j frame坐标系下的观测值。
BA使用开源的iSAM库实现，作者说iSAM可以支持相机参数自定义，并且能够很好的处理有无深度的特征点。