VINS-Mono原理推导与代码解析（三）后端非线性优化

欢喜h~

已于 2023-11-07 11:21:42 修改

阅读量245

点赞数

分类专栏： VINS-Mono原理与代码解析文章标签：自动驾驶

于 2023-11-06 14:43:05 首次发布

本文链接：https://blog.csdn.net/weixin_50911531/article/details/134241779

版权

VINS-Mono原理与代码解析专栏收录该内容

4 篇文章 0 订阅

订阅专栏

本专栏参考崔华坤的《VINS论文推导及代码解析》的PDF，按照这个框架，对VINS-Mono的原理进行详解，并附加代码解析~

总体内容如下：

VINS-Mono原理推导与代码解析（一）总体框架

VINS-Mono原理推导与代码解析（二）IMU预积分

VINS-Mono原理推导与代码解析（三）后端非线性优化

VINS-Mono原理推导与代码解析（四）前端视觉处理

VINS-Mono原理推导与代码解析（五）初始化

VINS-Mono原理推导与代码解析（六）边缘化Maginalization和FEJ

VINS-Mono原理推导与代码解析（七）闭环检测与优化

VINS-Mono原理推导与代码解析（八）其他补充~

1.状态向量

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

$X=[x_0,x_1,\cdotp\cdotp\cdotp x_n,x_c^b,\lambda_0,\lambda_1,\cdotp\cdotp\cdotp\lambda_m]$

$x_k=[p_{b_k}^w,v_{b_k}^w,q_{b_k}^w,b_a,b_g]$

$x_c^b=[p_c^b,q_c^b]$

2.目标函数

$\min_X\left\{\left\|r_p-J_pX\right\|^2+\sum_{k\in B}\left\|r_B\left(\hat{z}_{b_{k+1}}^{b_k},X\right)\right\|_{P_{b_{k+1}}^{b_k}}^2+\Sigma_{(l,j)\in C}\left\|r_C\left(\hat{z}_l^{c_j},X\right)\right\|_{P_l^{c_j}}^2\right\}$

（19）

三个残差项分别是边缘化的先验信息、IMU测量残差、视觉的重投影残差。这三个残差都是用码是距离来表示的。

根据《十四讲》中高斯-牛顿法，若要计算目标函数的最小值，可以理解为，当优化变量有一个增量后，目标函数值最小，以IMU残差为例，可写成如下形式：

$\min_{\delta X}\left\|r_B\left(\hat{z}_{b_{k+1}}^{b_k},X+\Delta X\right)\right\|_{P_{b_{k+1}}^{b_k}}^2=\min_{\delta X}\left\|r_B\left(\hat{z}_{b_{k+1}}^{b_k},X\right)+J_{b_{k+1}}^{b_k}\Delta X\right\|_{P_{b_{k+1}}^{b_k}}^2$

其中 $J_{b_{k+1}}^{b_k}$ 为误差项 $r_B$ 关于所有状态向量（即优化变量）X的Jacobian，将上式展开并令关于 $\Delta X$ 的导数为0，可得增量 $\Delta X$ 的计算公式：

$\begin{aligned}{J_{b_{k+1}}^{b_k}}^TP_{b_{k+1}}^{b_k}&^{-1}J_{b_{k+1}}^{b_k}\Delta X=-{J_{b_{k+1}}^{b_k}}^TP_{b_{k+1}}^{b_k}&^{-1}r_B\end{aligned}$

那么，对于公式（19）的整体目标函数的整体增量方程可写成：

$(H_p+\sum {J_{b_{k+1}}^{b_k}}^T{P_{b_{k+1}}^{b_k}}^{-1} {J_{b_{k+1}}^{b_k}} + \sum {J_l^{C_j}}^T{P_l^{C_j}}^{-1}{J_l^{C_j}})\Delta X\\=b_p+\sum {J_{b_{k+1}}^{b_k}}^T{P_{b_{k+1}}^{b_k}}^{-1} r_B + \sum {J_l^{C_j}}^T{P_l^{C_j}}^{-1}r_C$

上式中， $P_{b_{k+1}}^{b_k}$ 为IMU预积分噪声项的协方差， $P_l^{C_j}$ 为visual观测的噪声协方差。当IMU的噪声协方差 $P_{b_{k+1}}^{b_k}$ 越大时，其信息矩阵 ${P_{b_{k+1}}^{b_k}}^{-1}$ 将越小，意味着该IMU观测越不可信，换句话说，因IMU噪声较大，越不可信IMU预积分数据，而更加相信visual观测。这里的IMU和visual协方差的绝对值没有意义，考虑的是两者的相对性。

将上式继续化简为：

$(\Lambda_p+\Lambda_B+\Lambda_C)\Delta X=b_p+b_B+b_C$

其中， $\Lambda_p\text{,}\Lambda_B$ 和 $\Lambda_C$ 为Hessian矩阵，上述方程称之为增量方程。

上面的Jacobian虽然是误差项r关于状态向量X的一阶导，但是在具体求解时，通常采用扰动的方式进行计算，如下所示，其中 $\delta X$ 是状态向量的微小扰动，并非上面求解的增量 $\Delta X$ ：

$J=\frac{\partial\gamma}{\partial X}=\lim_{\delta X\to0}\frac{\gamma(X\oplus\delta X)-\gamma(X)}{\delta X}$

3.IMU约束

（1）残差

IMU残差为两帧之间的PVQ和bias的变化量的差

$r_B^{15\times1}(\hat{z}_{b_{k+1}}^{b_k},X)=\begin{bmatrix} \delta\alpha_{b_{k+1}}^{b_k} \\ \delta\theta_{b_{k+1}}^{b_k} \\ \delta\beta_{b_{k+1}}^{b_k} \\ \delta b_a \\ \delta b_g\end{bmatrix} = \begin{bmatrix} R_w^{b_k}\left(p_{b_{k+1}}^w-p_{b_k}^w-v_{b_k}^w\Delta t_k+\frac12g^w\Delta t_k^2\right)-\alpha_{b_{k+1}}^{b_k} \\ \left.2\left[\gamma_{b_{k+1}}^{b_k}\right.^{-1}\otimes q_{b_k}^{w_k-1}\otimes q_{b_{k+1}}^w\right]_{xyz} \\ R_w^{b_k}(v_{b_{k+1}}^w-v_{b_k}^w+g^w\Delta t_k)-\beta_{b_{k+1}}^{b_k} \\ b_{a_{b_{k+1}}}-b_{a_{b_k}} \\ b_{\omega b_{k+1}}-b_{\omega b_k}\end{bmatrix}$

（20）

其中各增量关于bias的Jacobian可从公式（16）的大Jacobian中的相应位置获得。

${J_{k+1}}^{15\times15}=FJ_k$ （16）

然后看看代码中对IMU残差定义的实现，在vins_estimator/src/factor/integration_base.h的evaluate中

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;

    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;

    // 残差
    residuals.block<3,1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - P_i - 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）优化变量

$[p_{b_k}^w,q_{b_k}^w],~[v_{b_k}^w,b_{a_k},b_{\omega_k}],~[p_{b_{k+1}}^w,q_{b_{k+1}}^w],~[v_{b_{k+1}}^w,b_{a_{k+1}},b_{\omega_{k+1}}]$

（3）Jacobian

计算Jacobian时，残差对应的求偏导对象为上面的优化变量，但是计算时采用扰动方式计算，即扰动为 $[\delta p_{b_k}^w,\delta\theta_{b_k}^w],~[\delta v_{b_k}^w,\delta b_{a_k},\delta b_{\omega_k}],~[\delta p_{b_{k+1}}^w,\delta\theta_{b_{k+1}}^w],~[\delta v_{b_{k+1}}^w,\delta b_{a_{k+1}},\delta b_{\omega_{k+1}}]$ 。

$J[0]^{15\times7}=\left[\frac{\partial r_B}{\partial p_{b_k}^w},\frac{\partial r_B}{\partial q_{b_k}^w}\right]==\begin{bmatrix}-R_w^{b_k}&\left[R_w^{b_k}\left(p_{b_{k+1}}^w-p_{b_k}^w-v_{b_k}^w\Delta t_k+\frac12g^w\Delta t_k^2\right)\right]^{\land}\\0&-\mathcal{L}[{q_{b_{k+1}}^w}^{-1}\otimes q_{b_k}^w]\mathcal{R}\left[\gamma_{b_{k+1}}^{b_k}\right]\\0&\left[R_w^{b_k}(v_{b_{k+1}}^w-v_{b_k}^w+g^w\Delta t_k)\right]^{\land}\\0&0\\0&0\end{bmatrix}$

$J[1]^{15\times9}=\left[\frac{\partial r_B}{\partial v_{b_k}^w},\frac{\partial r_B}{\partial b_{a_k}},\frac{\partial r_B}{\partial b_{w_k}}\right]=\begin{bmatrix}-R_W^{b_k}\Delta t&-J_{b_a}^\alpha&-J_{b_\omega}^\alpha\\0&0&-\mathcal{L}\left[q_{b_{k+1}}^w{}^{-1}\otimes q_{b_k}^w\otimes\gamma_{b_{k+1}}^{b_k}\right]J_{b_\omega}^\gamma\\-R_W^{b_k}&-J_{b_a}^\beta&-J_{b_\omega}^\beta\\0&-I&0\\0&0&-I\end{bmatrix}$

$J[2]^{15\times7}=\left[\frac{\partial r_B}{\partial p_{b_{k+1}}^w},\frac{\partial r_B}{\partial q_{b_{k+1}}^w}\right]=\begin{bmatrix}R_w^{b_k}&0\\0&\mathcal{L}\left[{\gamma_{D_{k+1}}^{b_k}}^{-1}\otimes{q_{b_k}^{w}}^{-1}\otimes{q_{D_{k+1}}^{w}}\right]\\0&0\\0&0\\0&0\end{bmatrix}$

$J[3]^{15\times9}=\left[\frac{\partial r_B}{\partial v_{b_{k+1}}^w},\frac{\partial r_B}{\partial b_{a_{k+1}}},\frac{\partial r_B}{\partial b_{w_{k+1}}}\right]=\begin{bmatrix}0&0&0\\0&0&0\\R_w^{b_k}&0&0\\0&I&0\\0&0&I\end{bmatrix}$

然后看看代码实现部分~在vins_estimator/src/factor/imu_factor.h中

if(jacobians[0])
{
    Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMahjor>> jacobian_pose_i(jacobians[0]);
    jacobian_pose_i.setZero();

    jacobian_pose_i.block<3,3>(O_P, O_P) = -Qi.inverse().toRotationMatirx();
    jacobian_pose.i.block<3,3>(O_P, O_R) = Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt * Pj - Pi - Vi * sum_dt));
#if 0
    jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix();
#else
    Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
    jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();
#endif
    
    jacobian_pose_i.block<3,3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));
    
    jacobian_pose_i = sqrt_info * jacobian_pose_i;
}
if(jacobians[1])
{
    Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_i(jacobians[1]);
    jacobian_speedbias_i.setZero();
    jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt;
    jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba;
    jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg;

#if 0
    jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg;
#else
    jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * pre_integration->delta_q).bottomRightCorner<3, 3>() * dq_dbg;
#endif

    jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix();
    jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba;
    jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg;

    jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity();

    jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity();

    jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i;
}
if(jacobians[2])
{
    Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[2]);
    jacobian_pose_j.setZero();

    jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix();

#if 0
    jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity();
#else
    Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
    jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>();
#endif

    jacobian_pose_j = sqrt_info * jacobian_pose_j;
}
if(jacobians[3])
{
    Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_j(jacobians[3]);
    jacobian_speedbias_j.setZero();

    jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix();

    jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity();

    jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity();

    jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j;
}

代码中residual还乘以一个信息矩阵sqrt_info，这是因为真正的优化项是Mahalanobis距离： $d=r^TP^{-1}r$ ，P是协方差，又因为Ceres只接受最小二乘优化，也就是 $min(e^Te)$ ，所有把 $P^{-1}$ 做LLT分解，即 $LL^T=P^{-1}$ ，则有：

$d=r^TLL^Tr=(L^Tr)^T(L^Tr)$

令 $r'=L^Tr$ 作为新的优化误差，这样就能用Ceres求解了。Mahalanobis距离其实相当于残差加权，协方差大的加权小，协方差小的加权大，着重优化那些比较确定的残差。

（4）协方差

$P_{k+1}{}^{15\times15}=FP_kF^T+VQV^T$ （17）

IMU协方差P为公式（17）推导的IMU预积分中迭代出来的IMU增量误差的协方差。

4.视觉约束

（1）残差

视觉残差是重投影误差，对于第l个路标点P，将P从第一次观看到它的第i个相机坐标系，转换到当前的第j个相机坐标系下的像素坐标系，可定义视觉误差项为：

$r_C\left(\hat{z}_l^{C_j},X\right)=\begin{bmatrix}\vec{b}_1\\\vec{b}_2\end{bmatrix}\left(\frac{P_l^{c_j}}{\left\|P_l^{c_j}\right\|}-\bar{P}_l^{c_j}\right)$ （25）

其中， $\bar{P}_l^{c_j}$ 为第l个路标点在第j个相机归一化相机坐标系中的观察到的坐标：

$\left.\bar{P}_l^{c_j}=\pi_c^{-1}\left(\begin{bmatrix}\hat{u}_l^{c_j}\\\hat{v}_l^{c_j}\end{bmatrix}\right.\right)$ （26）

${P}_l^{c_j}$ 是估计第l个路标点在第j个相机归一化相机坐标系中可能坐标：

$P_l^{c_j}=R_b^c\left\{R_w^{b_j}\left[R_{b_i}^w\left(R_c^b\frac{1}{\lambda_l}\bar{P}_l^{c_i}+p_c^b\right)+p_{b_i}^w-p_{b_j}^w\right]-p_c^b\right\}$ （27）

（2）优化变量

$[p_{b_i}^w,q_{b_i}^w],~\left[p_{b_j}^w,q_{b_j}^w\right],~[p_c^b,q_c^b],~\lambda_l$

（3）Jacobian

根据视觉残差公式，可以得到相对于各优化变量的Jacobian。

$J[0]^{3\times7}=\left[\frac{\partial r_C}{\partial p_{b_i}^{w}},\frac{\partial r_C}{\partial q_{b_i}^{w}}\right]=\left[R_{b}^{c}R_{w}^{b_j}\quad-R_{b}^{c}R_{w}^{b_j}R_{b_i}^{w}\left(R_{c}^{b}\frac{1}{\lambda_{l}}\bar{P}_{l}^{c_i}+p_{c}^{b}\right)^{\wedge}\right]$

$J[1]^{3\times7}=\left[\frac{\partial r_{C}}{\partial p_{b_{j}}^{w}},\frac{\partial r_{C}}{\partial q_{b_{j}}^{w}}\right]=\left[-R_{b}^{c}R_{w}^{b_{j}}\quad R_{b}^{c}\left\{R_{w}^{b_{j}}\left[R_{b_{i}}^{b}\left(R_{c}^{b}\frac{\overline{P}_{l}^{c_{i}}}{\lambda_{l}}+p_{c}^{b}\right)+p_{b_{i}}^{w}-p_{b_{j}}^{w}\right]\right\}^{\wedge}\right]$

$J[2]^{3\times7}=\left[\frac{\partial r_C}{\partial p_c^b},\frac{\partial r_C}{\partial q_c^b}\right]\\=\begin{bmatrix}R_b^c\left(R_w^{b_j}R_{b_i}^{\prime\prime}-I_{3\times3}\right)\\-R_b^cR_w^{b_j}R_{b_i}^{b_j}R_c^{b}\left(\frac{\bar{F}^{c_i}}{\lambda_l}\right)^{\wedge}+\left(R_b^cR_w^{b_j}R_{b_l}^{w}R_c^{b}\frac{\bar{F}_l^{c_i}}{\lambda_l}\right)^{\wedge}+\left\{R_b^c\left[R_w^{b_j}\left(R_{b_i}^{b_j}p_c^{b}+p_{b_i}^{w}-p_{b_j}^{w}\right)-p_c^{b}\right]\right\}^{\wedge}\end{bmatrix}^T$

$J[3]^{3\times1}=\frac{\partial r_{C}}{\partial\lambda_{l}}=-R_{b}^{c}R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b}\frac{\bar{P}_{l}^{c_{i}}}{{\lambda_{l}}^{2}}$

代码实现在vins_estimator/src/factor/projection_factor.cpp中

if(jacobians[0])
{
    Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);

    Eigen::Matrix<double, 3, 6> jaco_i;
    jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();
    jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i);

    jacobian_pose_i.leftCols<6>() = reduce * jaco_i;
    jacobian_pose_i.rightCols<1>().setZero();
}
if(jacobians[1])
{
    Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[1]);

    Eigen::Matrix<double, 3, 6> jaco_j;
    jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose();
    jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j);

    jacobian_pose_j.leftCols<6>() = reduce * jaco_j;
    jacobian_pose_j.rightCols<1>().setZero();
}
if(jacobians[2])
{
    Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_ex_pose(jacobians[2]);
    Eigen::Matrix<double, 3, 6> jaco_ex;
    jaco_ex.leftCols<3>() = ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity());
    Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric;
    jaco_ex.rightCols<3>() = -tmp_r * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(tmp_r * pts_camera_i) +
                             Utility::skewSymmetric(ric.transpose() * (Rj.transpose() * (Ri * tic + Pi - Pj) - tic));
    jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex;
    jacobian_ex_pose.rightCols<1>().setZero();
}
if(jacobians[3])
{
    Eigen::Map<Eigen::Vector2d> jacobian_feature(jacobians[3]);
#if 1
    jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i * -1.0 / (inv_dep_i * inv_dep_i);
#else
    jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i;
#endif
}

（4）协方差

视觉约束的噪声协方差与标定相机内参时的重投影误差有关，这里取1.5个像素，对应到归一化相机平面上的协方差矩阵需除以焦距f，则信息矩阵等于协方差矩阵的逆，为：

$\Omega_{\nu is}=\Sigma_{\nu is}^{-1}=\left(\frac{1.5}{f}I_{2\times2}\right)^{-1}=\frac f{1.5}I_{2\times2}$

欢喜h~

关注

0
点赞
踩
6

收藏

觉得还不错? 一键收藏
0
评论
VINS-Mono原理推导与代码解析（三）后端非线性优化

本专栏参考崔华坤的《VINS论文推导及代码解析》的PDF，按照这个框架，对VINS-Mono的原理进行详解，并附加代码解析~VINS-Mono原理推导与代码解析（三）后端非线性优化VINS-Mono原理推导与代码解析（四）前端视觉处理VINS-Mono原理推导与代码解析（五）初始化VINS-Mono原理推导与代码解析（六）边缘化Maginalization和FEJVINS-Mono原理推导与代码解析（七）闭环检测与优化VINS-Mono原理推导与代码解析（八）其他补充~
复制链接

扫一扫

专栏目录