Vins-Mono 论文 && Coding 一 4(2). imu factor

最新推荐文章于 2023-02-14 15:36:46 发布

scofield_ly

最新推荐文章于 2023-02-14 15:36:46 发布

阅读量382

点赞数

分类专栏： # vio 文章标签： slam

本文链接：https://blog.csdn.net/weixin_40072161/article/details/113091217

版权

vio 专栏收录该内容

13 篇文章 1 订阅

订阅专栏

一、imu factor

1. 模型

$e= \begin{pmatrix} e(\alpha)\\ e(\beta)\\ e(\gamma)\\ e(b_a)\\ e(b_\omega) \end{pmatrix}= \begin{pmatrix} R^{b_k}_w(p^w_{b_{k+1}} -p^w_{b_k} + \frac{1}{2}g^w\Delta t_k^2 - v^w_{b_k} \Delta t_k) - \hat{\alpha}^{b_k}_{b_{k+1}} \\ R^{b_k}_w(v^w_{b_{k+1}} + g^w\Delta t_k - v^w_{b_k}) - \hat{\beta}^{b_k}_{b_{k+1}} \\ 2(\hat{\gamma}^{b_{k+1}}_{b_k} \otimes (q^{w^{-1}}_{b_k} \otimes q^w_{b_{k+1}}))_{[xyz]} \\ b_{ab_{k+1}} - b_{ab_k} \\ b_{\omega b_{k+1}} - b_{\omega b_k} \\ \end{pmatrix}$ (1)

其中 $\hat{\alpha}^{b_k}_{b_{k+1}}, \hat{\beta}^{b_k}_{b_{k+1}} , \hat{\gamma}^{b_k}_{b_{k+1}}$ 是 imu 预积分结果

$p^w_{b_k}, v^w_{b_k}, q^w_{b_k}(R^w_{b_k}), b_{ab_k}, b_{\omega b_k}$ 是 $k$ 时刻状态变量 (PVQ + Bias)

$p^w_{b_{k+1}}, v^w_{b_{k+1}}, q^w_{b_{k+1}}(R^w_{b_{k+1}}), b_{ab_{k+1}}, b_{\omega b_{k+1}}$ 是 $k+1$ 时刻状态变量 (PVQ + Bias)

2. orientation error

当旋转矢量比较小时，对应四元数可以近似为：

$q(u\cdot \theta)=\bigl(\begin{smallmatrix} cos(\tfrac{1}{2}\theta)\\ ucos(\tfrac{1}{2}sin\theta) \end{smallmatrix}\bigr)\approx \bigl(\begin{smallmatrix} 1 \\ \tfrac{1}{2}\theta \end{smallmatrix}\bigr)$ (2)

那么两个四元数的 error 可以表示为:

$\delta \theta ^{b_k}_{b_{k+1}}= 2(\hat{\gamma}^{b_{k+1}}_{b_k} \otimes (q^{w^{-1}}_{b_k} \otimes q^w_{b_{k+1}}))_{[xyz]}$ (3)

其中 $(quaternion)_{[xyz]}$ 代表取四元数的虚部

3. vins imu factor

vins_estimator/src/factor/imu_factor.h

class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>

(1). 求导方式

解析求导 (关于各种求导方式的区别，参考：ceres solver 三种求导方式)

(2). 模板参数

15: 约束项个数

7： $p^w_{b_k}, q^w_{b_k}$

9： $v^w_{b_k}, b_{ab_k}, b_{\omega b_k}$

7： $p^w_{b_{k+1}}, q^w_{b_{k+1}}$

9： $v^w_{b_{k+1}}, b_{ab_{k+1}}, b_{\omega b_{k+1}}$

(3). Evaluate 函数

virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const

<1>. 内存映射状态变量

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

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

<2>. 计算 residual

// 若 bias 更新量过大，则重新预积分，否则一阶近似修正
#if 0
 if ((Bai - pre_integration->linearized_ba).norm() > 0.10 || (Bgi - pre_integration->linearized_bg).norm() > 0.01) {
    pre_integration->repropagate(Bai, Bgi);
}
#endif

// 计算 imu error term
Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi, Pj, Qj, Vj, Baj, Bgj);
// covariance 加权
Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
residual = sqrt_info * residual;

预积分 covariance 加权

$e=r^TP^{-1}r=r^T(LL^T)r=(L^Tr)^T(L^Tr)={r}'^T{r}'$ (4)

<3>. 计算雅可比

参考 “三、雅可比推导”

二、 local parameterization

1. pose

(1). position

常规向量加

$p_1\leftarrow p_0+\delta p$ (5)

(2). quaternion

旋转矢量扰动，四元数右乘，模长归一化

$\delta q = \bigl(\begin{smallmatrix} 1\\ \frac{1}{2}\delta \theta \end{smallmatrix}\bigr), q_1\leftarrow q_0\otimes \delta q, q_1\leftarrow q_1/||q_1||$ (6)

考虑到增量四元数有可能不是单位四元数，所以更新的四元数需要归一化

(3). 代码实现

vins_estimator/src/factor/pose_local_parameterization.h

ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);

<1>. operator plus

bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const {
    Eigen::Map<const Eigen::Vector3d> _p(x);
    Eigen::Map<const Eigen::Quaterniond> _q(x + 3);

    Eigen::Map<const Eigen::Vector3d> dp(delta);
    Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map<const Eigen::Vector3d>(delta + 3));

    Eigen::Map<Eigen::Vector3d> p(x_plus_delta);
    Eigen::Map<Eigen::Quaterniond> q(x_plus_delta + 3);

    p = _p + dp;
    q = (_q * dq).normalized();
    return true;
}

<2>. ComputeJacobian

bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const {
    Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
    j.topRows<6>().setIdentity();
    j.bottomRows<1>().setZero();
    return true;
}

2. speed-bias

常规向量加

三、雅可比推导

1. 预积分 error state jacobian

参考： Vins-Mono 论文 && Coding 一 4. imu 预积分

(7)

double sum_dt = pre_integration->sum_dt;

// 预积分 delta pvq 关于 delta bias 的雅可比
Eigen::Matrix3d dp_dba = pre_integration->jacobian.template block<3, 3>(O_P, O_BA);
Eigen::Matrix3d dp_dbg = pre_integration->jacobian.template block<3, 3>(O_P, O_BG);

Eigen::Matrix3d dq_dbg = pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
Eigen::Matrix3d dv_dba = pre_integration->jacobian.template block<3, 3>(O_V, O_BA);
Eigen::Matrix3d dv_dbg = pre_integration->jacobian.template block<3, 3>(O_V, O_BG);

2. 预积分关于 k 时刻 pose 状态变量的雅可比

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

(1). 位置预积分 error

<1>. 关于 position

$\frac{de(\alpha)}{dp^w_{b_k}}=-R_w^{b_k}$ (8)

<2>. 关于 rotation

$e(\alpha)=R^{b_k}_w(p^w_{b_{k+1}} -p^w_{b_k} + \frac{1}{2}g^w\Delta t_k^2 - v^w_{b_k} \Delta t_k) - \hat{\alpha}^{b_k}_{b_{k+1}}$ (9)

令 $p^w_{b_{k+1}} -p^w_{b_k} + \frac{1}{2}g^w\Delta t_k^2 - v^w_{b_k} \Delta t_k=k$ ，那么有

$e(\alpha)=R^{b_k}_w\cdot k - \hat{\alpha}^{b_k}_{b_{k+1}}=f(R^{b_k}_w)$ (10)

考虑扰动：

$f(q^w_{b_k}\oplus\delta \theta )=(R^w_{b_k}\cdot exp((\delta \theta)_\times)))^{-1}\cdot k - \hat{\alpha}^{b_k}_{b_{k+1}}$

$=(exp((-\delta \theta)_\times))\cdot R^{b_k}_w)\cdot k- \hat{\alpha}^{b_k}_{b_{k+1}}$

$\approx( (I - (\delta \theta)_\times )\cdot R^{b_k}_w)\cdot k- \hat{\alpha}^{b_k}_{b_{k+1}}$

$= R^{b_k}_wk -\hat{\alpha}^{b_k}_{b_{k+1}} - (\delta \theta)_\times \cdot R^{b_k}_w\cdot k$

$=f(q^w_{b_k}) - (\delta \theta)_\times \cdot R^{b_k}_w\cdot k$

$=f(q^w_{b_k}) + (R^{b_k}_w\cdot k)_\times \cdot \delta \theta$ (11)

所以对应雅可比为

$\frac{de(\alpha)}{dq^w_{b_k}}=(R^{b_k}_w\cdot k)_\times$

$=(R^{b_k}_w\cdot (p^w_{b_{k+1}} -p^w_{b_k} + \frac{1}{2}g^w\Delta t_k^2 - v^w_{b_k} \Delta t_k))_\times$ (12)

(2). 速度预计分 error

<1>. 关于 rotation

$e(\beta)=R^{b_k}_w(v^w_{b_{k+1}} + g^w\Delta t_k - v^w_{b_k} ) - \hat{\beta}^{b_k}_{b_{k+1}}$ (13)

令 $v^w_{b_{k+1}} + g^w\Delta t_k - v^w_{b_k}=k$ ，那么有

$e(\beta)=R^{b_k}_w\cdot k - \hat{\beta}^{b_k}_{b_{k+1}}=f(R^{b_k}_w)$ (14)

参考(11)(12)，得到所以对应雅可比为

$\frac{de(\beta)}{dq^w_{b_k}}=(R^{b_k}_w\cdot k)_\times$

$=(R^{b_k}_w\cdot (v^w_{b_{k+1}} + g^w\Delta t_k - v^w_{b_k}))_\times$ (15)

(3). 旋转预积分 error

<1>. 关于 rotation

考虑扰动：

$e(q^w_{b_k}\otimes \delta q ) = 2(\hat{\gamma}^{b_{k}*}_{b_{k+1}} \otimes (q^w_{b_k}\otimes \begin{bmatrix} 1 \\ \frac{1}{2}\delta q\end{bmatrix})^* \otimes q^w_{b_{k+1}})_{[xyz]}$

$=-2((\hat{\gamma}^{b_{k}*}_{b_{k+1}} \otimes (q^w_{b_k}\otimes \begin{bmatrix} 1 \\ \frac{1}{2}\delta q\end{bmatrix})^* \otimes q^w_{b_{k+1}})^*)_{[xyz]}$

$=-2[q^{w^*}_{b_{k+1}}\otimes (q^w_{b_k}\otimes \begin{bmatrix} 1 \\ \frac{1}{2}\delta q\end{bmatrix}) \otimes \hat{\gamma}^{b_{k}}_{b_{k+1}}]_{[xyz]}$

$=-2 \begin{bmatrix} 0 & I \end{bmatrix}(q^{w^*}_{b_{k+1}}\otimes (q^w_{b_k}\otimes \begin{bmatrix} 1 \\ \frac{1}{2}\delta q\end{bmatrix}) \otimes \hat{\gamma}^{b_{k}}_{b_{k+1}})$

$=-2 \begin{bmatrix} 0 & I \end{bmatrix}(q^{w^*}_{b_{k+1}}\otimes q^w_{b_k})_L (\hat{\gamma}^{b_{k}}_{b_{k+1}} )_R\begin{bmatrix} 1 \\ \frac{1}{2}\delta q\end{bmatrix}$ (16)

所以，对应雅可比为：

$\frac{de(\gamma)}{dq^w_{b_k}}=-2 \begin{bmatrix} 0 & I \end{bmatrix}(q^{w^*}_{b_{k+1}}\otimes q^w_{b_k})_L (\hat{\gamma}^{b_{k}}_{b_{k+1}} )_R\begin{bmatrix} 0 \\ \frac{1}{2}I\end{bmatrix}$ (17)

3. 预积分关于 k 时刻 speed-bias 状态变量的雅可比

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

(1). 位置预积分 error

<1>. 关于 velocity

$\frac{de(\alpha)}{dv_{b_k}}=-R^{b_k}_w*\Delta t$ (18)

<2>. 关于 bias

$\frac{de(\alpha)}{db_{ab_k}}=\frac{de(\alpha)}{d \hat{\alpha}^{b_k}_{b_{k+1}}}\cdot \frac{d \hat{\alpha}^{b_k}_{b_{k+1}}}{db_{ab_k}}$

$=-\frac{d \hat{\alpha}^{b_k}_{b_{k+1}}}{db_{ab_k}}$ (19)

$\frac{de(\alpha)}{db_{\omega b_k}}=\frac{de(\alpha)}{d \hat{\alpha}^{b_k}_{b_{k+1}}}\cdot \frac{d \hat{\alpha}^{b_k}_{b_{k+1}}}{db_{\omega b_k}}$

$=-\frac{d \hat{\alpha}^{b_k}_{b_{k+1}}}{db_{\omega b_k}}$ (20)

(2). 速度预计分 error

<1>. 关于 velocity

$\frac{de(\beta)}{dv_{b_k}}=-R^{b_k}_w$ (21)

<2>. 关于 bias

$\frac{de(\beta)}{db_{ab_k}}=\frac{de(\beta)}{d \hat{\alpha}^{b_k}_{b_{k+1}}}\cdot \frac{d \hat{\alpha}^{b_k}_{b_{k+1}}}{db_{ab_k}}$

$=-\frac{d \hat{\beta}^{b_k}_{b_{k+1}}}{db_{ab_k}}$ (22)

$\frac{de(\beta)}{db_{\omega b_k}}=\frac{de(\beta)}{d \hat{\alpha}^{b_k}_{b_{k+1}}}\cdot \frac{d \hat{\alpha}^{b_k}_{b_{k+1}}}{db_{\omega b_k}}$

$=-\frac{d \hat{\beta}^{b_k}_{b_{k+1}}}{db_{\omega b_k}}$ (23)

(3). 旋转预积分 error

<1>. 关于 gyro bias

一阶近似旋转预积分

$e(b_{\omega b_k} + \delta \omega)=2((\hat{\gamma}^{b_k}_{b_{k+1}}\otimes\begin{bmatrix} 1 \\ \frac{1}{2} J^{\gamma}_{b_{\omega b_k}}\delta \omega\end{bmatrix} )^*\otimes q^{w^*}_{b_k}\otimes q^w_{b_{k+1}})_{[xyz]}$

$=-2(((\hat{\gamma}^{b_k}_{b_{k+1}}\otimes\begin{bmatrix} 1 \\ \frac{1}{2} J^{\gamma}_{b_{\omega b_k}}\delta \omega\end{bmatrix} )^*\otimes q^{w^*}_{b_k}\otimes q^w_{b_{k+1}})^*)_{[xyz]}$

$=-2(q^{w^*}_{b_{k+1}} \otimes q^w_{b_k} \otimes (\hat{\gamma}^{b_k}_{b_{k+1}}\otimes\begin{bmatrix} 1 \\ \frac{1}{2} J^{\gamma}_{b_{\omega b_k}}\delta \omega\end{bmatrix}) )_{[xyz]}$

$=-2\begin{bmatrix} 0 & I \end{bmatrix} [q^{w^*}_{b_{k+1}} \otimes q^w_{b_k} \otimes \hat{\gamma}^{b_k}_{b_{k+1}}]_L\begin{bmatrix} 1 \\ \frac{1}{2} J^{\gamma}_{b_{\omega b_k}}\delta \omega\end{bmatrix}$ (24)

所以，对应雅可比为：

$\frac{de(\gamma)}{db_{\omega b_{k}}}=-2\begin{bmatrix} 0 & I \end{bmatrix} [q^{w^*}_{b_{k+1}} \otimes q^w_{b_k} \otimes \hat{\gamma}^{b_k}_{b_{k+1}}]_L\begin{bmatrix} 0 \\ \frac{1}{2} J^{\gamma}_{b_{\omega b_k}} \end{bmatrix}$ (25)

(4). bias error

$\frac{de(b_a)}{db_{a b_k}}=\frac{d(b_{ab_{k+1}}-b_{ab_k})}{db_{a b_k}} = -I$ (26)

$\frac{de(b_\omega)}{db_{\omega b_k}}=\frac{d(b_{\omega b_{k+1}}-\omega_{ab_k})}{db_{\omega b_k}} = -I$ (27)

4. 预积分关于 k+1 时刻 pose 状态变量的雅可比

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

(1). 位置预积分 error

<1>. 关于 position

$\frac{de(\alpha)}{dp^w_{b_k}}= R_w^{b_k}$ (28)

(2). 角度预积分 error

<1>. 关于 rotation

考虑扰动：

$e(q^w_{b_{k+1}}\otimes \delta q ) = 2(\hat{\gamma}^{b_{k}*}_{b_{k+1}} \otimes q^{w^*}_{b_k} \otimes ( q^w_{b_{k+1}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}\delta q\end{bmatrix} ))_{[xyz]}$

$= 2([\hat{\gamma}^{b_{k}*}_{b_{k+1}} \otimes q^{w^*}_{b_k} \otimes q^w_{b_{k+1}} ]_L \begin{bmatrix} 1 \\ \frac{1}{2}\delta q\end{bmatrix} )_{[xyz]}$

$= 2 \begin{bmatrix} 0 & I \end{bmatrix} [\hat{\gamma}^{b_{k}*}_{b_{k+1}} \otimes q^{w^*}_{b_k} \otimes q^w_{b_{k+1}} ]_L \begin{bmatrix} 1 \\ \frac{1}{2}\delta q\end{bmatrix}$ (29)

所以，对应雅可比为：

$\frac{de(\gamma)}{dq^w_{b_{k+1}}}= 2 \begin{bmatrix} 0 & I \end{bmatrix} [\hat{\gamma}^{b_{k}*}_{b_{k+1}} \otimes q^{w^*}_{b_k} \otimes q^w_{b_{k+1}} ]_L \begin{bmatrix} 0 \\ \frac{1}{2}I\end{bmatrix}$ (30)

5. 预积分关于 k+1 时刻 speed-bias 状态变量的雅可比

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