VINS-FUSION 优化-先验因子(边缘化)

标签：para Pose 边缘化 FUSION 因子先验 VINS 优化

一、边缘化

VINS中的边缘化策略，将滑出窗外的帧与滑窗内的帧的约束使用边缘化的形式保存为先验误差因子进行后续非线性优化，以保留约束信息。

VINS-Fusion优化约束包括：a.视觉误差因子约束，b.IMU预积分约束，c.边缘化先验因子约束

文章主要讲述边缘化先验因子约束如何产生。

VINS-Fusion中，边缘化如图示：

二、边缘化原理

优化问题通用表达形式：

$HX = b$

其中， $H = J^{T}J$ , $b=J^{T}e$ , $e=(J^{T})^{+ }b, ()^{+ }$ 为伪逆，e为残差， $J$ 为残差对状态的Jacobian。

将其拆解成以下形式：

$\begin{bmatrix} H_{mm} & H_{mr}\\ H_{rm} & H_{rr} \end{bmatrix} \begin{bmatrix} X_{m}\\ X_{r} \end{bmatrix} = \begin{bmatrix} b_{m}\\ b_{r} \end{bmatrix}$

拆解成上述形式后，通过变化将状态量 $X_{m}$ 删掉，并把它的约束保留下来。

在滑窗模式中，这个 $X_{m}$ 即为要边缘化的量。

舒尔补原理

给定矩阵

$M = \begin{bmatrix} A & B\\ C & D \end{bmatrix}$

通过下述变换，变成上三角矩阵，如下：

$\begin{bmatrix} I & 0\\ -CA^{-1} & I \end{bmatrix} \begin{bmatrix} A & B\\ C & D \end{bmatrix} = \begin{bmatrix} A & B\\ 0 & \Delta A \end{bmatrix}$

其中， $\Delta A = D - CA^{-1}B$ 称为A关于M的舒尔补。

将舒尔补原理应用到上述拆解形式的优化问题上，则：

$\begin{bmatrix} I & 0\\ -H_{rm}H_{mm}^{-1} & I \end{bmatrix} \begin{bmatrix} H_{mm} & H_{mr}\\ H_{rm} & H_{rr} \end{bmatrix} \begin{bmatrix} X_{m}\\ X_{r} \end{bmatrix} = \begin{bmatrix} I & 0\\ -H_{rm}H_{mm}^{-1} & I \end{bmatrix}\begin{bmatrix} b_{m}\\ b_{r} \end{bmatrix}$

进一步化简，得到：

$\begin{bmatrix} H_{mm} & H_{mr}\\ 0 & H_{rr} - H_{rm}H_{mm}^{-1}H_{mr} \end{bmatrix} \begin{bmatrix} X_{m}\\ X_{r} \end{bmatrix} = \begin{bmatrix} b_{m}\\ b_{r} - H_{rm}H_{mm}^{-1}b_{m}\end{bmatrix}$

于是通过矩阵第二行等式，可得：

$(H_{rr} - H_{rm}H_{mm}^{-1}H_{mr})X_{r} = b_{r} - H_{rm}H_{mm}^{-1}b_{m}$

通过观察上述形式，等式中不包含变量 $X_{m}$ ，即完成了将 $X_{m}$ 删掉的目的。

将 $(H_{rr} - H_{rm}H_{mm}^{-1}H_{mr})X_{r}$ 记为 $H_{0}$ ， $b_{r} - H_{rm}H_{mm}^{-1}b_{m}$ 记为 $b_{0}$ 。

得到边缘化后待求解的增量方程如下：

$H_{0}\delta x = b_{0}$ （1）

需根据这个增量方程恢复出边缘化先验误差因子，即 $e_{p}$ 。

随着优化迭代进行，状态量被不断更新。但边缘化时，被边缘化的状态值固定，舒尔补时使用的Jacobian为当时泰勒展开使用该固定值(x线性化点)求得的Jacobian。在VINS中，线性化处的参数值x0保存为parameter_block_data，此即为EFJ。

但(1)式中， $b = J^{T}e$ ，其中e随着优化状态更新而变化。

优化状态的更新：

$x = Exp(\delta x)\bigotimes x_{0}$

此时b的更新可表示为：

$b=b_{0}+\frac{\partial b}{\partial x}\mid _{x_{0}}\delta x$

$=b_{0}+J^{T}\frac{\partial e}{\partial x}\mid _{x_{0}}\delta x$

$=b_{0}+J^{T}J\delta x$

$=b_{0}+H\delta x$

则，边缘化后先验误差因子：

$e_{p} = (J^{T})^{+ }b$

$=(J^{T})^{+ }(b_{0} + H\delta x)$

$=(J^{T})^{+ }b_{0} + (J^{T})^{+ }J^{T}J\delta x$

$=(J^{T})^{+ }b_{0} + J\delta x$

至此，边缘化的步骤可总结为：

Marginalize_old
$Hx = b$ 优化参数 $x$ ，由ceres完成优化迭代, Evaluate如下： $d_{x}=x-x_{0}, e=e_{0}+J_{0}d_{x}, J=J_{0}$	上一次Marginalize，更新得到线性化点 $x_{0}$ ，及其 $J_{0}$ 、 $e_{0}$
MarginalizationInfo *marginalization_info = new MarginalizationInfo() 创建边缘化管理类MarginalizationInfo，添加AddResidualBlockInfo	上一次先验残差及待边缘化变量索引Marginalization_factor
	最老帧，即0-1帧预积分残差IMUFactor
	第一次观测为第0帧的所有路标点对应的视觉观测ProjectionFactor
preMarginalize()，更新 $J$ 、 $e$ ，并将参数 $x$ 存入parameter_block_data，Evaluate如下： $d_{x}=x-x_{0}, e=e_{0}+J_{0}d_{x}, J=J_{0}$	上一步参数 $x$ 优化
Marginalize()，多线程更新 $A$ 、 $b$ $A=J^{T}J$ , $b=J^{T}e$ 舒尔补更新线性化点 $x_{0}$ 及其 $J_{0}$ 、 $e_{0}$ ： $H_{0} = (H_{rr} - H_{rm}H_{mm}^{-1}H_{mr})X_{r}$ $b_{0}=b_{r} - H_{rm}H_{mm}^{-1}b_{m}$ $H_{0}$ 分解得到 $J_{0}$ ， $e_{0}=(J^{T})^{+ }b_{0}$ parameter_block_data，得到 $x_{0}$	由preMarginalize更新得到parameter_block_data、 $J$ 、 $e$

三、先验因子约束优化

1.添加边缘化先验因子约束，完成优化

在runOptimization()优化函数中，使用ceres完成先验因子优化，参照文章“Ceres优化库详细解析”。

添加先验因子约束

构建边缘化因子类MarginalizationFactor。

MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_margnlztn_manager_);

class MarginalizationFactor : public ceres::CostFunction {
   public:
    // 将边缘化的结果转化为边缘化因子
    MarginalizationFactor(MarginalizationManager* _marginalization_info);

    // ceres接口，计算残差和雅可比，提供给ceres做优化
    virtual bool Evaluate(double const* const* _Parameters, double* _Residuals, double** _Jacobians) const;

    MarginalizationManager* marginalization_info_;
};

MarginalizationFactor类继承SizedCostFunction，并在类IMUFactor中实现CostFunction::Evaluate()，

MarginalizationManager，边缘化管理类。主要完成：收集被边缘化帧及其相关联帧、因子(imu预积分、特征重投影因子)，执行边缘化操作，获得边缘化因子。

ceres完成优化。

2.执行边缘化，产生边缘化先验因子

上述完成边缘化因子的前提是已经完成一次边缘化，如何执行边缘化操作，获得边缘化因子？

(1)边缘化最老帧

a.与最老帧关联的IMU预积分处理

if(USE_IMU) {
	if (pre_integrations_[1]->sum_dt < 10.0) {
		IMUFactor* cost_func = new IMUFactor(pre_integrations_[1]);
		ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(
			cost_func, NULL/*loss func*/,
			vector<double*>{para_Pose_[0], para_Speed_Bias_[0], 
							para_Pose_[1], para_Speed_Bias_[1]},
			vector<int>{0, 1}/*drop set*/);
		curr_margnlztn_manager->addResidualBlockInfo(residual_block_info);
	}
}

添加残差块，值得注意的是参数块：{para_Pose_[0], para_Speed_Bias_[0], para_Pose_[1], para_Speed_Bias_[1]}, 和标记边缘化掉的参数块的索引drop_set，如上vector<int>{0, 1}/*drop set*/，丢弃掉的参数为para_Pose_[0], para_Speed_Bias_[0]。

b.与最老帧关联的特征点(被最老帧观测到的特征点)

这一部分与加入视觉误差因子一致，唯一区别就是需标记要边缘化的参数，如ProjectionTwoFrameOneCamFactor 优化的参数为{para_Pose_[frame_i], para_Pose_[frame_j], para_Ex_Pose_[0], para_Features_[feature_index], para_Td_[0]}；要边缘化的参数为para_Pose_[frame_i], para_Features_[feature_index]；标记为vector<int>{0, 3}/*drop set*/。

其它类似。

c.计算残差和雅克比(preMarginalize)

计算残差对应的Jacobian，并将各参数块数据放在parameter_block_data中。

计算残差和雅克比：

it->Evaluate()

再将数据放到parameter_block_data中。

d.边缘化(marginlize)

多线程更新 $A$ 、 $b$ ，舒尔补更新线性化点 $x_{0}$ 及其 $J_{0}$ 、 $e_{0}$ 。

e.重置状态量索引

std::unordered_map<long, double*> addr_shift;
for (int i = 0; i <= WINDOW_SIZE; i++) {
	if (i == WINDOW_SIZE - 1) {
		continue;
	}
	else if (i == WINDOW_SIZE) {
		addr_shift[reinterpret_cast<long>(para_Pose_[i])] = para_Pose_[i - 1];
		if(USE_IMU)
			addr_shift[reinterpret_cast<long>(para_Speed_Bias_[i])] = para_Speed_Bias_[i - 1];
	}
	else {
		addr_shift[reinterpret_cast<long>(para_Pose_[i])] = para_Pose_[i];
		if(USE_IMU)
			addr_shift[reinterpret_cast<long>(para_Speed_Bias_[i])] = para_Speed_Bias_[i];
	}
}
for (int i = 0; i < NUM_OF_CAM; i++) {
	addr_shift[reinterpret_cast<long>(para_Ex_Pose_[i])] = para_Ex_Pose_[i];
}
addr_shift[reinterpret_cast<long>(para_Td_[0])] = para_Td_[0];

last_marginalization_info = marginalization_info;
last_marginalization_parameter_blocks = parameter_blocks;

(2)边缘化次新帧

待更新。

至此，边缘化先验信息存储到last_marginalization_info 和last_marginalization_parameter_blocks ，在optimization之后进行slidewindow()操作。

四、滑窗slideWindow

滑窗过程：在滑动窗口中，总是保持窗口总体大小不变，当新进来一帧，通过函数addFeatureCheckParallax判断，如果属于关键帧，则去除窗口中最老帧，置标志MARGIN_OLD；如果不属于关键帧，则去除窗口中次新帧，置标志MARGIN_SECOND_NEW。通过该种策略完成滑窗过程。

根据文章“VINS-FUSION 基于视差Parallax关键帧”，判断新进帧是否为关键帧。

1.去掉最老帧

当边缘化标志flag_marglize_type_为MARGIN_OLD时：

(1)将优化变量左移一

a.将最老帧直接去掉，滑窗内最老帧位姿保留后，滑窗内帧优化变量左移一，预积分左移一，及每个特征点的共视帧索引左移一，深度左移一。

深度左移一说明如下：

//相机坐标系坐标 = 归一化相机平面坐标uv_i * 深度
Eigen::Vector3d pts_i = uv_i * it->estimated_depth;

//世界坐标系下特征点坐标p_w = R_wc * p_c
Eigen::Vector3d w_pts_i = marg_R * pts_i + marg_P;

//次老帧相机坐标系坐标 = (R_wc)^T * ((w_pts_i - new_P))
Eigen::Vector3d pts_j = new_R.transpose() * (w_pts_i - new_P);

//pts_j(2)即相机坐标系下坐标z_{c}，及特征点深度
double dep_j = pts_j(2);

if (dep_j > 0)
	it->estimated_depth = dep_j;
else
	it->estimated_depth = INIT_DEPTH;

通过以上方式，将滑窗中最老的特征点深度信息转移给次老帧。

slideWindowOld()

//将特征点滑窗内共视帧索引减一

f_manager.removeBack()

//移除了最老帧，更新滑窗帧index，并将最老帧持有特征的深度信息转移给次老帧

f_manager.removeBackShiftDepth(marg_R, marg_P, new_R, new_P)

2.去掉次新帧

将最新帧替换为次新帧，次新帧直接去掉。将最新帧优化变量赋值给次新帧，并重新预积分。

主要函数

slideWindowNew()

f_manager.removeFront(frame_count)去除特征点与次新帧的共视关系

五、参考

1. 滑窗优化、边缘化、舒尔补、FEJ及fill-in问题_滑动窗口fej算法是啥-CSDN博客

2. VINS-MONO边缘化策略_边缘化先验误差-CSDN博客

3. 白话VINS-Mono之边缘化（五） - 知乎

标签：para,Pose,边缘化,FUSION,因子,先验,VINS,优化
From： https://blog.csdn.net/u010196944/article/details/136204629