后端优化 | VINS-Mono 论文公式推导与代码解析分讲

3. 后端优化(紧耦合)

VIO 紧耦合方案的主要思路就是通过将基于视觉构造的残差项和基于IMU构造的残差项放在一起构造成一个联合优化的问题，整个优化问题的最优解即可认为是比较准确的状态估计。

为了限制优化变量的数目，VINS-Mono 采用了滑动窗口的形式，滑动窗口中的全状态量：

滑动窗口内 n+1 个所有相机的状态(包括位置、朝向、速度、加速度计 bias 和陀螺仪 bias)
Camera 到 IMU 的外参
m+1 个 3D 点的逆深度

优化过程中的误差状态量

进而得到系统优化的代价函数（Minimize residuals from all sensors）

其中三个残差项依次是
边缘化的先验信息
IMU 测量残差
视觉的观测残差

三种残差都是用 马氏距离（与量纲无关）来表示的。

Motion-only visual-inertial bundle adjustment: Optimize position, velocity, rotation in a smaller windows, assuming all other quantities are fixed

3.1 IMU 测量残差
（1）IMU 测量残差
上面的IMU预积分（估计值 - 测量值），得到 IMU 测量残差

/**

[evaluate 计算IMU测量模型的残差]
@param Pi，Qi，Vi，Bai，Bgi [前一次预积分结果]
@param Pj，Qj，Vj，Baj，Bgj [后一次预积分结果]
*/
Eigen::Matrix<double, 15, 1> evaluate(
const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi,
const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj)
{
Eigen::Matrix<double, 15, 1> residuals;

Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);

Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);

Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);
Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);

Eigen::Vector3d dba = Bai - linearized_ba;
Eigen::Vector3d dbg = Bgi - linearized_bg;

// IMU预积分的结果,消除掉acc bias和gyro bias的影响, 对应IMU model中的\hat{\alpha},\hat{\beta},\hat{\gamma}
Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;

// IMU项residual计算,输入参数是状态的估计值, 上面correct_delta_*是预积分值, 二者求’diff’得到residual
residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;
residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();
residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;

return residuals;
}
（2）协方差矩阵
此处用到的协方差矩阵为前面 IMU 预积分计算出的协方差矩阵。
残差的后处理对应代码：
// 在优化迭代的过程中, 预积分值是不变的, 输入的状态值会被不断的更新, 然后不断的调用evaluate()计算更新后的IMU残差
Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
Pj, Qj, Vj, Baj, Bgj);

Eigen::Matrix<double, 15, 15> sqrt_info =
Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
//sqrt_info.setIdentity();

residual = sqrt_info * residual; // 为了保证 IMU 和视觉參差项在尺度上保持一致，一般会采用与量纲无关的马氏距离

这里残差 residual 乘以 sqrt_info，这是因为真正的优化项其实是 Mahalanobis 距离:d= , 其中是协方差。Mahalanobis距离其实相当于一个残差加权，协方差大的加权小，协方差小的加权大，着重优化那些比较确定的残差。
而 ceres 只接受最小二乘优化，也就是 min ,所以把 做 LLT分解，即，则 d= ，令 ′=( )，作为新的优化误差，所以 sqrt_info 等于
（3）雅克比矩阵
高斯迭代优化过程中会用到IMU测量残差对状态量的雅克比矩阵，但此处我们是对误差状态量求偏导，下面对四部分误差状态量求取雅克比矩阵。

雅克比矩阵计算的对应代码在class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>中的Evaluate()函数中。

3.2 视觉(td) 测量残差
视觉测量残差即 特征点的重投影误差，视觉残差和雅克比矩阵计算的对应代码在 ProjectionFactor::Evaluate 函数中。
（1）切平面重投影误差（Spherical camera model）

// 将第i frame下的3D点转到第j frame坐标系下
Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i; // pt in ith camera frame, 归一化平面
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; // pt in ith body frame
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; // pt in world frame
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); // pt in jth body frame
Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); // pt in jth camera frame

（2）像素重投影误差（Pinhole camera model）

Eigen::MapEigen::Vector2d residual(residuals);
#ifdef UNIT_SPHERE_ERROR
// 把归一化平面上的重投影误差投影到Unit sphere上的好处就是可以支持所有类型的相机 why
// 求取切平面上的误差
residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else
// 求取归一化平面上的误差
double dep_j = pts_camera_j.z();
residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif
residual = sqrt_info * residual; // 转成与量纲无关的马氏距离
（3）协方差矩阵
固定的协方差矩阵，归一化平面的标准差为，即像素标准差为 1.5。
ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
（4）雅克比矩阵
下面关于误差状态量对相机测量残差求偏导，得到高斯迭代优化过程中的雅克比矩阵。

（5）Vision measurement residual for temporal calibration
视觉残差和雅克比矩阵计算的对应代码在 ProjectionTdFactor::Evaluate 函数中。

// TR / ROW * row_i 是相机 rolling 到这一行时所用的时间
Eigen::Vector3d pts_i_td, pts_j_td;
pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i;
pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j;

Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i;
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);

Eigen::MapEigen::Vector2d residual(residuals);
#ifdef UNIT_SPHERE_ERROR
residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized());
#else
double dep_j = pts_camera_j.z();
residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>();
#endif
residual = sqrt_info * residual;
添加对 imu-camera 时间戳不完全同步和 Rolling Shutter 相机的支持：通过前端光流计算得到每个角点在归一化的速度，根据 imu-camera 时间戳的时间同步误差和Rolling Shutter相机做一次rolling的时间，对角点的归一化坐标进行调整
3.3 Temporal Calibration
Timestamps

Time Synchronization

Temporal Calibration
calibrate the fixed latency occurred during time stamping
change the IMU pre-integration interval to the interval between two image timestamps
linear incorporation of IMU measurements to obtain the IMU reading at image time stamping estimates states(position, orientation, etc.) at image time stamping

3.4 边缘化(Marginalization)
SLAM is tracking a noraml distribution through a large state space

滑窗（Sliding Window）限制了关键帧的数量，防止pose和feature的个数不会随时间不断增加，使得优化问题始终在一个有限的复杂度内，不会随时间不断增长。

Marginalization

然而，将pose移出windows时，有些约束会被丢弃掉，这样势必会导致求解的精度下降，而且当MAV进行一些退化运动(如: 匀速运动)时，没有历史信息做约束的话是无法求解的。所以，在移出位姿或特征的时候，需要将相关联的约束转变成一个约束项作为prior放到优化问题中，这就是marginalization要做的事情。
边缘化的过程就是将滑窗内的某些较旧或者不满足要求的视觉帧剔除的过程，所以边缘化也被描述为将联合概率分布分解为边缘概率分布和条件概率分布的过程(就是利用shur补减少优化参数的过程)。
直接进行边缘化而不加入先验条件的后果：
无故地移除这些 pose 和 feature 会丢弃帧间约束，会降低了优化器的精度，所以在移除pose 和 feature 的时候需要将相关联的约束转变为一个先验的约束条件作为prior放到优化问题中。
在边缘化的过程中，不加先验的边缘化会导致系统尺度的缺失，尤其是系统在进行退化运动时(如无人机的悬停和恒速运动)。一般来说只有两个轴向的加速度不为0的时候，才能保证尺度可观，而退化运动对于无人机或者机器人来说是不可避免的。所以在系统处于退化运动的时候，要加入先验信息保证尺度的可观性。
VINS-Mono 中为了处理一些悬停的case，引入了一个 two-way marginalization：
MARGIN_OLD：如果次新帧是关键帧，则丢弃滑动窗口内最老的图像帧，同时对与该图像帧关联的约束项进行边缘化处理。这里需要注意的是，如果该关键帧是观察到某个地图点的第一帧，则需要把该地图点的深度转移到后面的图像帧中去。
MARGIN_NEW：如果次新帧不是关键帧，则丢弃当前帧的前一帧。因为判定当前帧不是关键帧的条件就是当前帧与前一帧视差很小，也就是说当前帧和前一帧很相似，这种情况下直接丢弃前一帧，然后用当前帧代替前一帧。为什么这里可以不对前一帧进行边缘化，而是直接丢弃，原因就是当前帧和前一帧很相似，因此当前帧与地图点之间的约束和前一帧与地图点之间的约束是很接近的，直接丢弃并不会造成整个约束关系丢失信息。这里需要注意的是，要把当前帧和前一帧之间的 IMU 预积分转换为当前帧和前二帧之间的 IMU 预积分。
在悬停等运动较小的情况下，会频繁的 MARGIN_NEW ，这样也就保留了那些比较旧但是视差比较大的 pose 。这种情况如果一直 MARGIN_OLD 的话，视觉约束不够强，状态估计会受 IMU 积分误差影响，具有较大的累积误差。

Schur Complement
Marginalization via Schur complement on information matrix

First Estimate Jacobin

后端优化 | VINS-Mono 论文公式推导与代码解析分讲相关推荐

[从零手写VIO|第五节]——后端优化实践——单目BA求解代码解析
长篇警告⚠⚠⚠ 目录 solver 全流程回顾 Solver三要素 Solver求解中的疑问核心问题代码解析 1. TestMonoBA.cpp 2. 后端部分: 2.1 顶点 2.2 边(残差) ...
单目标跟踪算法：Siamese RPN论文解读和代码解析
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者:周威 | 来源:知乎 https://zhuanlan.zhihu.com/p/16198364 ...
PointNet论文解读和代码解析
目录一.论文动机现有的问题: 作者的思路及面临的问题: 二.论文方法如何解决点云无序性问题?作者提出了三种想法. 针对点云的刚体运动不变性三.网络结构四.代码阅读五.Reference(两 ...
Inception V3论文解读和代码解析
目录论文解读代码解析小结论文解读在介绍inception V2时提到过,inception V3的论文依据是Rethinking the Inception Architecture for ...
Longformer论文解读和代码解析
前言这篇博文记录了longformer论文的主要思想.代码实现和结果复现方面的一些工作,相关链接如下: 原longformer论文地址 github上原作者公开的代码 huggingface上原作者 ...
含多类型充电桩的电动汽车充电站优化配置方法论文复现——附代码
目录摘要: 研究背景: 电动汽车充电行为及负荷的表征方法: 连续时域的离散化及相关简化策略: 电动汽车负荷建模方法: 电力用户分类与负荷建模: 电动汽车充电站优化配置模型: 目标函数: 约束条件: ...
【单目3D目标检测】MonoDLE论文精读与代码解析
文章目录 Preface Abstract Contributions Diagnostic Experiments Pipeline Revisiting Center Detection Trai ...
YOLOv7来临：论文解读附代码解析
前言: 是一份关于YOLOv7的论文解读,首发于[GiantPandaCV]公众号,写的不是很好,望大佬们包涵! 2022年7月,YOLOv7来临, 论文链接:https://arxiv.org/ab ...
【单目3D目标检测】MonoFlex论文精读与代码解析
文章目录 Preface Abstract Contributions Pipeline Problem Definition Decoupled Representations of Objects ...

后端优化 | VINS-Mono 论文公式推导与代码解析分讲

后端优化 | VINS-Mono 论文公式推导与代码解析分讲相关推荐

最新文章

热门文章