DPI-SLAM：融合面特征以及IMU信息的SLAM系统 学习笔记

全文主要使用因子图方式进行优化

具体内容为三个线程：

1.融合IMU的视觉位姿估计：

首先通过IMU预积分计算出两帧间的相对位姿：

在因子图中对应的误差方程是：

由于在预积分计算过程中假设偏置不变，所以出于对精度的考虑，选择参考帧间隔进行IMU预积分计算。

之后通过IMU预积分算出来的位姿对相邻帧进行位姿的初始化预测，使用Fast Dense RGB-D Odometry进行位姿的进一步计算。通过IMU预测的帧间位移为：

用于估算位姿的误差函数均由两部分组成：

IPP：计算重投影误差

1）面的提取过程：

在对深度图进行平滑之后，将降噪的深度图分为10*10像素的网格。在每个网格中采样部分像素点并计算在含有这些点局部面上的法向量，如果有超过阈值的点数接近平行，用这些点和法向量生成一个面模型，并在同一网格中寻找内点，并用所有内点对平面模型进行改进。

2）平面的匹配：

当帧中面的光心通过利用IMU计算出的帧间位姿投影在了参考帧中包含面的网格中，则认为两个面是匹配的。

3）利用重投影误差构造误差函数：

当有m个对面被发现匹配后，即可构造关于位姿的重投影误差：

Pyramid：

在计算光度误差时，为了减小光照变化的影响，使用拉普拉斯图对光度误差进行计算：

Jr是有效像素点i的光度在拉普拉斯下采样图像（图像尺寸和分辨率变小）中关于6自由度（旋转和平移）的雅可比阵，ri是有效像素点在Fn和Rr的拉普拉斯图中对应像素点的光度残差。

SRO：

SRO仅用少量具有很高色彩梯度的像素点进行残差计算。首先选择具有高梯度的几个像素点，之后选择这些点周围小块内具有可用深度的点进行计算。在点快选好之后即可以进行位姿的计算：

其中Jr是选择像素点i的光度在灰度图像中关于6自由度（旋转和平移）的雅可比阵，

ri是选择像素点在Fn和Rr的灰度图像中对应像素点的光度残差。

    由于我们使用的是IMU状态，所以所有求出来的RGBD相机坐标下的转移阵要转换到IMU坐标系下：

2.局部深度融合：

    融合帧被用来与当前的深度图进行融合，融合帧的深度测量被投影到关键帧的深度图中，之后用平均法（应该是最后对同一个点会测量出很多深度）融合到深度地图中。为了防止不正确的深度融合，只有在关键帧对应深度测量的差值在一个较小阈值范围内的深度测量才会被融合。在新的关键帧被选择出来之前，融合帧会被不断选择并融入到深度地图中，有新的关键帧被选出来之后将从已有的深度图中分割平面（点簇算法）并初始化新的局部深度地图。

3.全局建图：

通过寻找法向量和原点距离相似的点簇进行融合来提取面特征，对平面包含的点簇应用线性模型进行匹配，求取平面参数：

方程可以用

进行求解

关键帧间平面的匹配：

在新的关键帧中有面特征被提取之后，之前的十个关键帧会被当作数据融合的候选帧并利用优化后的位姿投影到当前帧，并开始寻找匹配的路标。两个面匹配的准则有：

1．  法线的方向差小于阈值

2．  平面到原点的距离之差小于阈值

3．  计算代价函数：

其中设定阈值。

每一个面都可以作为一个在单位球面的投影空间上的一个齐次向量。可以将过参数的面用一个三维向量进行建模（类似于四元数），通过指数映射可以得到两者间的关系并进行更新：

平面观测会被加入到关键帧中对准过的平面和IMU间作为因子，紧耦合的路标面以及IMU会被全局优化，在优化之前面的信息也要被转移到IMU坐标系下：

在面间添加结构约束：

在室内环境下面之间的约束关系主要有平行和垂直两种，在极小间隔内观测到的路标面和被考虑为有结构约束的候选面对。首先计算

是两个面的base pose（即第一次观测到面的IMU状态）大于阈值时两面视为平行，当小于阈值时两个平面视为垂直。当确定两个面之间的结构约束之后，两个面之间的结构约束误差为：

垂直：

平移：