VINS通过ceres的解析求导的方式进行的,主要分成三个部分:

  • 定义待优化的参数块,类似于g2o中的顶点;
  • 通过残差约束来添加残差块,类似于g2o的边
  • 进行求解

定义待优化的参数块

数据预处理

首先回顾待优化的变量:

状态向量共包括滑动窗口内的 $n+1$ 个所有 IMU 状态$\mathbf{x}_k,k\in[0,n]$(包括位置、朝向、速度、加速度计 $bias$ 和陀螺仪 $bias$)、 Camera 到 IMU 的外参 $\mathbf{x}^b_c$、 $m+1$ 个 3D 点的逆深度$\lambda_l,l\in[0,m]$:

$$ \begin{align*} \mathcal{X}&=[\mathbf{x}_0,\mathbf{x}_1,···,\mathbf{x}_n,\mathbf{x}^b_c,\lambda_0,\lambda_1,···,\lambda_m]\\ \mathbf{x}_k&=[\mathbf{p}^w_{b_k},\mathbf{v}^w_{b_k},\mathbf{q}^w_{b_k},\mathbf{b}_a,\mathbf{b}_g],k\in[0,n]\\ \mathbf{x}^b_c&=[\mathbf{p}^b_c,\mathbf{q}^b_c] \end{align*}\tag{1} $$

$n$ 是关键帧的总数,$m$ 是滑动窗口中的特征总数。 $λ_l$ 是第 $l$ 个特征与其第一次观察的逆深度。·


参数块1:滑窗中位姿(包括位置和姿态)

vins_estimator\src\estimator.cpp\void Estimator::optimization()

// 参数块1:滑窗中位姿(包括位置和姿态),共11帧
loss_function = new ceres::CauchyLoss(1.0); // 使用柯西核函数
for (int i = 0; i < WINDOW_SIZE + 1; i++)
{
    // 由于姿态不满足加法,也就是李群没有加法运算,因此需要自己定义它的加法
    ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
    // 将滑窗中的所有参数块加到优化问题里
    problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
    problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
}

参数块的具体定义内容如下:

vins_estimator\src\estimator.h\

//定义参数块
double para_Pose[WINDOW_SIZE + 1][SIZE_POSE];           // 位置和姿态当作一个参数块 SIZE_POSE=7 (位置3个 姿态使用四元数表示 4个)
double para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS]; // 速度、零偏当作一个参数块 SIZE_SPEEDBIAS=9
double para_Feature[NUM_OF_F][SIZE_FEATURE];            // 维护特征点的逆深度 SIZE_FEATURE=1
double para_Ex_Pose[NUM_OF_CAM][SIZE_POSE];             // 外参
double para_Retrive_Pose[SIZE_POSE];                    // 回环检测相关
double para_Td[1][1];                                   // 时间戳校正
double para_Tr[1][1];

另外,由于姿态不满足加法,也就是李群没有加法运算,因此需要自己定义它的加法,定义位置:

vins_estimator\src\factor\pose_local_parameterization.h;实现:vins_estimator\src\factor\pose_local_parameterization.cpp

参数块2:相机和IMU的外参时间戳的同步

// 参数块2:相机和IMU的外参
for (int i = 0; i < NUM_OF_CAM; i++)
{
    // 由于姿态不满足加法,也就是李群没有加法运算,因此需要自己定义它的加法
    ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
    // 将滑窗中的所有参数块加到优化问题里
    problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
    if (!ESTIMATE_EXTRINSIC)
    {
        ROS_DEBUG("fix extinsic param");
        // 如果不需要优化外参就调用SetParameterBlockConstant将其设置为 fix
        problem.SetParameterBlockConstant(para_Ex_Pose[i]);
    }
    else
        ROS_DEBUG("estimate extinsic param");
}
// 传感器的时间同步
if (ESTIMATE_TD)
{
    problem.AddParameterBlock(para_Td[0], 1);
    // problem.SetParameterBlockConstant(para_Td[0]);
}

由于ceres操作的对象是double数组,因此还需要将eigen转为double,包括位姿、陀螺仪的零偏、旋转外参、逆深度以及时间戳同步:

vins_estimator\src\estimator.cpp\void Estimator::vector2double()

// eigen -> double
vector2double();

构建IMU预积分残差块

IMU预积分约束的定义

预积分约束定义在类 IMUFactor中;

vins_estimator\src\estimator.cpp\void Estimator::optimization()

//残差块1:imu预积分约束
for (int i = 0; i < WINDOW_SIZE; i++)
{
    int j = i + 1;
    if (pre_integrations[j]->sum_dt > 10.0) // 如果预积分时间过长,说明这个约束没有参考价值了;
        continue;
    IMUFactor *imu_factor = new IMUFactor(pre_integrations[j]); // 定义 costfunction
    problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
}

由于VINS在后端优化中使用的是解析求导, 在这种情况下,需要定义残差的计算方式,又需要定义残差对各个参数块的雅可比。为此,如果您在编译时知道参数和残差的大小,请定义 CostFunctionSizedCostFunction 的子类。另外,需要重载Evaluate 函数,用来计算残差和雅可比。

  • 重载Evaluate 函数

vins_estimator\src\factor\imu_factor.h\virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const{}

/**
 * @brief 使用ceres解析求导,必须重载 Evaluate 函数
 * 
 * @param[in] parameters 这是一个二维数组,每个参数块都是一个double数组,而一个观测会对多个参数形成束
 * @param[in] residuals 残差的计算结果,是一个一维数组,残差就是该观测量和约束的状态量通过某种关系形成的
 * @param[in] jacobians 残差对参数块的雅可比矩阵,这也是一个二维数组,对任意一个参数块的雅可比都是一个一维数组 
 * @return true 
 * @return false 
 */
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const{}

这些在自定义的CostFuction函数中,对应代码为:vins_estimator\src\factor\imu_factor.h\class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>

这里需要对之前推导imu帧间约束以及雅可比的求解做回顾,详细的推导过程:VINS-Mono-后端非线性优化_IMU约束公式详细推导

残差构建

待优化变量

vins_estimator\src\factor\imu_factor.h\virtual bool Evaluate(...) const{}

virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    // 便于后续计算,把参数块转换成eigen
    // imu预积分的约束是相邻两帧之间的约束
    //G_ 待优化变量
    // 第i帧:Pi Qi Vi Bai Bgi
    Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
    Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
    Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]);
    Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]);
    Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]);

    // 第j帧:Pi Qi Vi Bai Bgi
    Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]);
    Eigen::Quaterniond Qj(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);
    Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]);
    Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]);
    Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]);
}

构建残差

vins_estimator\src\factor\integration_base.h\Eigen::Matrix<double, 15, 1> evaluate(...)

$$ \mathbf{r}_{\mathcal{B} }(\hat{\mathbf{z} }^{b_k}_{b_{k+1} },\mathcal{X})= \begin{bmatrix} \delta {\alpha}^{b_k}_{b_{k+1} }\\ \delta {\theta}^{b_k}_{b_{k+1} }\\ \delta {\beta}^{b_k}_{b_{k+1} }\\ \delta {b}_{a}\\\delta {b}_{g} \end{bmatrix}= \begin{align*} \begin{bmatrix} \mathbf{R}^{b_k}_w(\mathbf{p}^w_{b_{k+1} }-\mathbf{p}^w_{b_k}+ \frac{1}{2}g^w\Delta t_k^2-\mathbf{v}^w_{b_k}\Delta t_k)-\hat{\alpha}^{b_k}_{b_{k+1} }\\ 2[{\mathbf{q}^w_{b_k}}^{-1}\otimes\mathbf{q}^w_{b_{k+1} } \otimes(\hat{\gamma}^{b_k}_{b_{k+1} })^{-1}]_{xyz}\\ \mathbf{R}^{b_k}_w(\mathbf{v}^w_{b_{k+1} }-\mathbf{v}^w_{b_k}+g^w\Delta t_k)-\hat{\beta}^{b_k}_{b_{k+1} }\\ \mathbf{b}_{ab_{k+1} }-\mathbf{b}_{ab_k}\\ \mathbf{b}_{wb_{k+1} }-\mathbf{b}_{wb_k}\\ \end{bmatrix} \end{align*}\tag{2} $$

对应代码如下:

vins_estimator\src\factor\integration_base.h\Eigen::Matrix<double, 15, 1> evaluate(...)

Eigen::Matrix<double, 15, 1> residuals;
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;

其中 Qi表示 $\mathbf{R}^w_{b_k}$, Qj表示 $\mathbf{R}^w_{b_{k+1} }$,corrected_delta_p表示 $\hat{\alpha}^{b_k}_{b_{k+1} }$,corrected_delta_v表示$\hat{\beta}^{b_k}_{b_{k+1} }$,corrected_delta_v表示 $\hat{\gamma}^{b_k}_{b_{k+1} }$;

corrected_delta_p等表示的是修正的零偏,由于在进行预积分的时候,我们使用的是给定的零偏进行建模,而零偏也是待优化的变量,为了避免在更新零偏的时候重新进行预积分操作,论文中,使用 $\alpha^{b_k}_{b_{k+1} },\beta^{b_k}_{b_{k+1} },\gamma^{b_k}_{b_{k+1} }$ 关于偏差的一阶近似进行补偿,可以写成:

$$ \begin{align*} \alpha^{b_k}_{b_{k+1} }&\approx \hat{\alpha}^{b_k}_{b_{k+1} }+\mathbf{J}^{\alpha}_{b_a}\delta b_{a_k}+\mathbf{J}^{\alpha}_{b_w}\delta b_{w_k} \\ \beta^{b_k}_{b_{k+1} }&\approx \hat{\beta}^{b_k}_{b_{k+1} }+\mathbf{J}^{\beta}_{b_a}\delta b_{a_k}+\mathbf{J}^{\beta}_{b_w}\delta b_{w_k} \\ \gamma^{b_k}_{b_{k+1} }&\approx \hat{\gamma}^{b_k}_{b_{k+1} }\otimes \begin{bmatrix}1\\\frac{1}{2}\mathbf{J}^{\gamma}_{b_w}\delta b_{w_k} \end{bmatrix} \end{align*}\tag{3} $$

对应代码:vins_estimator\src\factor\integration_base.h\Eigen::Matrix<double, 15, 1> evaluate(...)

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;

在定义完残差之后,还需要对得到的残差进一步处理:因为ceres没有g2o设置信息矩阵的接口,因此置信度直接乘在残差上,这里通过LLT分解,相当于将信息矩阵开根号;

实际上我们得到的残差为 $e$,我们最终求解误差的形式为最小二乘一个马氏距离,即 $e^TPe$,我们对信息矩阵 $P$ 进行 $LLT$ 分解,可以得到 $e^TPe=e^T(LL^T)e=(L^Te)^T(L^Te)$,因此可以得到一个新的残差 $e^\prime=L^Te$;

vins_estimator\src\factor\imu_factor.h\virtual bool Evaluate(...) const{}

// 计算并得到残差
Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
                                     Pj, Qj, Vj, Baj, Bgj);
// 因为ceres没有g2o设置信息矩阵的接口,因此置信度直接乘在残差上,这里通过LLT分解,相当于将信息矩阵开根号
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;

定义雅可比的计算

重载Evaluate 函数,用来计算残差和雅可比。因此,雅可比的计算位于Evaluate 函数中:vins_estimator\src\factor\imu_factor.h\virtual bool Evaluate(...) const{}

$$ \begin{align*} \mathbf{J}&= \begin{bmatrix} \mathbf{J}[0]&\mathbf{J}[1]\\\mathbf{J}[2]&\mathbf{J}[3] \end{bmatrix}\\ &=\begin{bmatrix} [\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{p}^w_{b_k} } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{q}^w_{b_k} }] &[\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{v}^w_{b_k} } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{b}_{a_k} } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{b}_{w_k} }]\\ [\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{p}^w_{b_{k+1} } } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{q}^w_{b_{k+1} } }] &[\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{v}^w_{b_{k+1} } } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{b}_{a_{k+1} } } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{b}_{w_{k+1} } }] \end{bmatrix} \end{align*} $$

  • $\mathbf{J}[0]$

$$ \begin{align*} \mathbf{J}[0]^{15\times7} &= [\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{p}^w_{b_k} } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{q}^w_{b_k} }]\\ &=\begin{bmatrix} \frac{\partial \delta {\alpha}^{b_k}_{b_{k+1} } }{\partial \mathbf{p}^w_{b_k} } &\frac{\partial \delta {\alpha}^{b_k}_{b_{k+1} } }{\partial \mathbf{q}^w_{b_k} }\\ 0&\frac{\partial \delta {\theta}^{b_k}_{b_{k+1} } }{\partial \mathbf{q}^w_{b_k} }\\ 0&\frac{\partial \delta {\beta}^{b_k}_{b_{k+1} } }{\partial \mathbf{q}^w_{b_k} }\\ 0&0\\0&0\\ \end{bmatrix}= \begin{bmatrix} \mathrm{block<3,3>(O\_P, O\_P)}&\mathrm{block<3, 3>(O\_P, O\_R)}\\ 0&\mathrm{block<3,3>(O\_R, O\_R)}\\ 0&\mathrm{block<3,3>(O\_V, O\_R)}\\0&0\\ 0&0\\ \end{bmatrix} \\ &=\begin{bmatrix} -\mathbf{R}^{b_k}_w& \left[\mathbf{R}^{b_k}_w\left(\mathbf{p}^w_{b_{k+1} }-\mathbf{p}^w_{b_k}-\mathbf{v}^w_{b_k}\Delta t_k+ \frac{1}{2}g^w\Delta t_k^2\right) \right]^{\land}\\ 0&-\mathcal{R} \left[{\mathbf{q}_{b_k}^w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} } \right]_{3\times3} \mathcal{L} \left[\hat{\gamma}^{b_k}_{b_{k+1} }\right]_{3\times3}\\ 0&\left[\mathbf{R}^{b_k}_w(\mathbf{v}^w_{b_{k+1} }-\mathbf{v}^w_{b_k}+g^w\Delta t_k)\right]^{\land}\\ 0&0\\ 0&0\\ \end{bmatrix} \end{align*} $$

对应代码:

vins_estimator\src\factor\imu_factor.h\virtual bool Evaluate(...) const{}

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

    jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();
    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));
     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;
    ...
}

其中,Utility::skewSymmetric()表用于求解矩阵的反对称矩阵;Utility::Qleft()表示求解四元数的做成左乘矩阵,Utility::Qright()表示四元数的右乘矩阵;其函数是实现位于:vins_estimator\src\utility\utility.h内;

  • $\mathbf{J}[1]$

$$ \begin{align*} \mathbf{J}[1]^{15\times9} &= [\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{v}^w_{b_k} } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{b}_{a_k} } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{b}_{w_k} }]\\ &= \begin{bmatrix} \frac{\partial \delta {\alpha}^{b_k}_{b_{k+1} } }{\partial \mathbf{v}^w_{b_k} }& \frac{\partial \delta {\alpha}^{b_k}_{b_{k+1} } }{\partial \mathbf{b}_{a_k} }& \frac{\partial \delta {\alpha}^{b_k}_{b_{k+1} } }{\partial \mathbf{b}_{w_k} }\\ 0&0&\frac{\partial \delta {\theta}^{b_k}_{b_{k+1} } }{\partial \mathbf{b}_{w_k} }\\ \frac{\partial \delta {\beta}^{b_k}_{b_{k+1} } }{\partial \mathbf{v}^w_{b_k} }& \frac{\partial \delta {\beta}^{b_k}_{b_{k+1} } }{\partial \mathbf{b}_{a_k} }& \frac{\partial \delta {\beta}^{b_k}_{b_{k+1} } }{\partial \mathbf{b}_{w_k} }\\ 0&\frac{\partial \delta {b}_{a} }{\partial \mathbf{b}_{w_k} }&0\\ 0&0&\frac{\partial \delta {b}_{g} }{\partial \mathbf{b}_{w_k} } \end{bmatrix} \\ &=\begin{bmatrix} -\mathbf{R}^{b_k}_w\Delta t&-\mathbf{J}^{\alpha}_{b_a}&-\mathbf{J}^{\alpha}_{b_w}\\ 0&0&-\mathcal{L}\left[{\mathbf{q}_{b_k}^w}^{-1}\otimes\mathbf{q}^w_{b_{k+1} } \otimes\hat{\gamma}^{b_k}_{b_{k+1} } \right]_{3\times3}\mathbf{J}^{\gamma}_{b_w}\\ -\mathbf{R}^{b_k}_w&-\mathbf{J}^{\beta}_{b_a}&-\mathbf{J}^{\beta}_{b_w}\\ 0&-\mathbf{I}&0\\ 0&0&-\mathbf{I} \end{bmatrix} \end{align*} $$

对应代码:

vins_estimator\src\factor\imu_factor.h\virtual bool Evaluate(...) const{}

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;
     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;

    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;
  • $\mathbf{J}[2]$

$$ \mathbf{J}[2]^{15\times7}= [\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{p}^w_{b_{k+1} } } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{q}^w_{b_{k+1} } }] =\begin{bmatrix} \mathbf{R}^{b_k}_w&0\\ 0&\mathcal{L}\left[{\mathbf{q}^{b_k}_w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} }\otimes\hat{\gamma}^{b_k}_{b_{k+1} }\right]_{3\times3}\\ 0&0\\ 0&0\\ 0&0\\ \end{bmatrix} $$

对应代码:

vins_estimator\src\factor\imu_factor.h\virtual bool Evaluate(...) const{}

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();
    jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity();

    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>();

    jacobian_pose_j = sqrt_info * jacobian_pose_j;
}
  • $\mathbf{J}[3]$

$$ \mathbf{J}[3]^{15\times9}= [\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{v}^w_{b_{k+1} } } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{b}_{a_{k+1} } } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{b}_{w_{k+1} } }] =\begin{bmatrix} 0&0&0\\ 0&0&0\\ \mathbf{R}^{b_k}_w&0&0\\ 0&\mathbf{I}&0\\ 0&0&\mathbf{I}\\ \end{bmatrix} $$

对应代码:

vins_estimator\src\factor\imu_factor.h\virtual bool Evaluate(...) const{}

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;
}
🧐 本文作者:
😉 本文链接:https://lukeyalvin.site/archives/84.html
😊 版权说明:本博客所有文章除特别声明外,均默认采用 CC BY-NC-SA 4.0 许可协议。
最后修改:2022 年 07 月 27 日
赏杯咖啡