LIO-livox - 激光IMU初始化模块分析_详解lio-livox中

概述

LIO-LIVOX中激光IMU的初始化的分析梳理。
LIO-LIVOX中激光IMU初始化和ORBSLAM3的视觉惯性初始化类似，都是将初始化问题转换为一个MAP问题求解，
ORBSLAM3相关内容参考ORB_SLAM3 算法框架解析。
总的来说，ORBSLAM3的初始化模块可分为：
1、Vision-only MAP Estimation
2、Inertial-only MAP Estimation
3、Visual-Inertial MAP Estimation
对于激光惯性系统，由于不需要估计特征点的位置，因此，只需要前两步即可：
1、Lidar-only MAP Estimation
2、Inertial-only MAP Estimation

TryMAPInitialization()

因子图如上图所示。
优化状态为：$[[v_0,...v_n],R_{wb},b_a,b_{\omega }]$
$[v_0,...v_n]$是滑窗中n个关键帧imu的速度。
$R_{wb}$为滑窗第0帧IMU坐标相对与world系(重力)的旋转。
注意滑动窗口n个帧的局部位姿$[R_{b_0{b}_i},t_{b_0{b}_i}]$ 不会被优化，而是作为MAP的固定约束，而其值由激光里程计给出。

所有的用于初始化的数据(激光里程计数据，IMU预积分等)保存在lidarFrameList中
boost::shared_ptr<std::list< Estimator::LidarFrame >> lidarFrameList;

计算初始的先验旋转(world->local)

  // 求解IMU加速度的比例因子
  Eigen::Vector3d average_acc = -lidarFrameList->begin()->imuIntegrator.GetAverageAcc();
  double info_g = std::fabs(9.805 - average_acc.norm());
  average_acc = average_acc * 9.805 / average_acc.norm();

  // calculate the initial gravity direction
  double para_quat[4];   // world -> local 旋转
  para_quat[0] = 1;
  para_quat[1] = 0;
  para_quat[2] = 0;
  para_quat[3] = 0;
  // 基于ceres求解初始姿态 
  ceres::LocalParameterization *quatParam = new ceres::QuaternionParameterization();
  ceres::Problem problem_quat;
  problem_quat.AddParameterBlock(para_quat, 4, quatParam);
  problem_quat.AddResidualBlock(Cost_Initial_G::Create(average_acc),
                                nullptr,
                                para_quat);
  ceres::Solver::Options options_quat;
  ceres::Solver::Summary summary_quat;
  ceres::Solve(options_quat, &problem_quat, &summary_quat);

  Eigen::Quaterniond q_wg(para_quat[0], para_quat[1], para_quat[2], para_quat[3]);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

构建最小二乘问题并采用ceres求解$$R_{bw}=\underset{R_{bw}}{\mathrm{argmin}}\Vert R_{bw}{G}_w-A_b{\Vert }_2$$

疑问：这貌似是个非凸优化问题吧？ 当这样当姿态倾斜比较大时，直接优化能求解的出来??

求解MAP优化

step1: 构建先验约束因子
认为IMU bias保持不变，只有速度V在变化。
IMU bias先验设为0，旋转设置为上一步求解的结果。

// 速度、P、bias先验均为0  
  Eigen::Vector3d prior_r = Eigen::Vector3d::Zero();
  Eigen::Vector3d prior_ba = Eigen::Vector3d::Zero();
  Eigen::Vector3d prior_bg = Eigen::Vector3d::Zero();
  std::vector<Eigen::Vector3d> prior_v;
  int v_size = lidarFrameList->size();
  for(int i = 0; i < v_size; i++) {
    prior_v.push_back(Eigen::Vector3d::Zero());    // 滑窗内全部速度状态设为0  
  }
    // 旋转先验
  Sophus::SO3d SO3_R_wg(q_wg.toRotationMatrix());
  prior_r = SO3_R_wg.log();    // world -> local 旋转
1
2
3
4
5
6
7
8
9
10
11
12

重新计算每一帧IMU的速度先验信息，说明不需要保持静止!

 for (int i = 1; i < v_size; i++){
   auto iter = lidarFrameList->begin();
   auto iter_next = lidarFrameList->begin();
   std::advance(iter, i-1);    // 将迭代器 iter 后退 i-1 步骤
   std::advance(iter_next, i);
   // 通过 lidar的位置 计算imu的位置  => 计算imu的速度
   Eigen::Vector3d velo_imu = (iter_next->P - iter->P + iter_next->Q*exPlb - iter->Q*exPlb) 
                                                               / (iter_next->timeStamp - iter->timeStamp);
   prior_v[i] = velo_imu;
 }
1
2
3
4
5
6
7
8
9
10

构建优化问题，并设置与先验约束有关的优化状态，添加先验残差factor。

  ceres::Problem::Options problem_options;
  ceres::Problem problem(problem_options);
  problem.AddParameterBlock(para_r, 3);   // 旋转
  problem.AddParameterBlock(para_ba, 3);  // bias
  problem.AddParameterBlock(para_bg, 3);
  for(int i = 0; i < v_size; i++) {
    problem.AddParameterBlock(para_v[i], 3);  // 每一帧的速度
  }
  
  // add CostFunction  添加先验factor
  problem.AddResidualBlock(Cost_Initialization_Prior_R::Create(prior_r, sqrt_information_r),
                           nullptr,
                           para_r);  // 先验旋转约束   
  
  problem.AddResidualBlock(Cost_Initialization_Prior_bv::Create(prior_ba, sqrt_information_ba),
                           nullptr,
                           para_ba);  // 先验 bias约束 
  problem.AddResidualBlock(Cost_Initialization_Prior_bv::Create(prior_bg, sqrt_information_bg),
                           nullptr,
                           para_bg);// 先验 bias约束  
  // 先验速度约束 
  for(int i = 0; i < v_size; i++) {
    problem.AddResidualBlock(Cost_Initialization_Prior_bv::Create(prior_v[i], sqrt_information_v),
                             nullptr,
                             para_v[i]);
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

step1: 添加IMU预积分约束因子，并求解优化

// 为滑动窗口中每两帧之间添加预积分约束
  for(int i = 1; i < v_size; i++) {
    auto iter = lidarFrameList->begin();
    auto iter_next = lidarFrameList->begin();
    std::advance(iter, i-1);
    std::advance(iter_next, i);
    // 根据激光的位置求解 第i帧  imu的位姿  
    Eigen::Vector3d pi = iter->P + iter->Q * exPlb;   
    Sophus::SO3d SO3_Ri(iter->Q*exRlb); // R * Rlb = Rwb  
    Eigen::Vector3d ri = SO3_Ri.log();
    // 根据激光的位置求解 j = i + 1, 第 j 帧 imu的位姿  
    Eigen::Vector3d pj = iter_next->P + iter_next->Q*exPlb;
    Sophus::SO3d SO3_Rj(iter_next->Q*exRlb);
    Eigen::Vector3d rj = SO3_Rj.log();

    problem.AddResidualBlock(Cost_Initialization_IMU::Create(iter_next->imuIntegrator,
                                                                   ri,   //  Rwb  
                                                                   rj, 
                                                                   pj-pi,
                                                                   Eigen::LLT<Eigen::Matrix<double, 9, 9>>
                                                                    (iter_next->imuIntegrator.GetCovariance().block<9,9>(0,0).inverse())
                                                                    .matrixL().transpose()),
                             nullptr,
                             para_r,         
                             para_v[i-1],
                             para_v[i],
                             para_ba,
                             para_bg);
  }
  // 求解优化
  ceres::Solver::Options options;
  options.minimizer_progress_to_stdout = false;
  options.linear_solver_type = ceres::DENSE_QR;
  options.num_threads = 6;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

对于第i帧和第j帧IMU，Cost_Initialization_IMU Factor的残差计算如下：

r I i I j 9 × 1 ( R W b 0 , b a , b ω , V b i b 0 , V b j b 0 ) = = ( R b 0 b i ( P b j b 0 − P b i b 0 − V b i b 0 Δ t i j + 0.5 R W b 0 g W Δ t i j 2 ) − ( α I j I i + ∂ α I j I i ∂ δ b a δ b a + ∂ α I j I i ∂ δ b ω δ b ω ) [ ( θ I j I i E x p ( ∂ θ I j I i ∂ δ b ω δ b ω ) ) − 1 ⊗ q b 0 b i ⊗ q b j b 0 ] x y z R b 0 b i ( V b j b 0 − V b i b 0 + R W b 0 g W Δ t i j ) − ( β I j I i + ∂ β I j I i ∂ δ b a δ b a + ∂ β I j I i ∂ δ b ω δ b ω ) ) r_{IiIj}^{9\times1}(R^{b_0}_W,b_a, b_{\omega},V^{b_0}_{b_i},V^{b_0}_{b_j})=\\=

$$\begin{pmatrix} R^{b_i}_{b_0}(P^{b_0}_{b_j}-P^{b_0}_{b_i}-V^{b_0}_{b_i}\Delta t_{ij}+0.5R^{b_0}_Wg^W\Delta t_{ij}^2)-(\alpha^{I_i}_{I_j}+\frac{\partial{\alpha^{I_i}_{I_j}}}{\partial{\delta{b_a}}}\delta{b_a}+\frac{\partial{\alpha^{I_i}_{I_j}}}{\partial{\delta{b_{\omega}}}}\delta{b_{\omega}} ) \\ [(\theta^{I_i}_{I_j} Exp(\frac{\partial{\theta^{I_i}_{I_j} }}{\partial{\delta b_{\omega}}}\delta b_{\omega}))^{-1}\otimes q^{b_i}_{b_0}\otimes q^{b_0}_{b_j}]_{xyz}\\R^{b_i}_{b_0}(V^{b_0}_{b_j}-V^{b_0}_{b_i}+R^{b_0}_Wg^W\Delta t_{ij})-(\beta^{I_i}_{I_j}+\frac{\partial{\beta^{I_i}_{I_j}}}{\partial{\delta{b_a}}}\delta{b_a}+\frac{\partial{\beta^{I_i}_{I_j}}}{\partial{\delta{b_{\omega}}}}\delta{b_{\omega}} ) \end{pmatrix}$$ rIiIj9×1​(RWb0​​,ba​,bω​,Vbi​b0​​,Vbj​b0​​)==⎝ ⎛​Rb0​bi​​(Pbj​b0​​−Pbi​b0​​−Vbi​b0​​Δtij​+0.5RWb0​​gWΔtij2​)−(αIj​Ii​​+∂δba​∂αIj​Ii​​​δba​+∂δbω​∂αIj​Ii​​​δbω​)[(θIj​Ii​​Exp(∂δbω​∂θIj​Ii​​​δbω​))−1⊗qb0​bi​​⊗qbj​b0​​]xyz​Rb0​bi​​(Vbj​b0​​−Vbi​b0​​+RWb0​​gWΔtij​)−(βIj​Ii​​+∂δba​∂βIj​Ii​​​δba​+∂δbω​∂βIj​Ii​​​δbω​)​⎠ ⎞​

其中，$P_{b_j}^{b_0},P_{b_i}^{b_0},R_{b_0}^{b_i},R_{b_0}^{b_j}$是激光里程计计算出来的位姿，作为已知量传入到残差中，不参与优化。
优化是采用ceres自动求导完成的，因此只需要将上述残差写入factor的operator()重载即可：

 /**
  * @brief: 自动求导  - 计算残差 的仿函数
  * @param rwg_ world - > 滑动窗口第0帧的旋转
  * @param vi_ 第i帧速度
  * @param vj_ 第j帧速度
  * @param ba_ 加速度bias
  * @param bg_ 角速度bias
  * @param[out] residual 输出结果  残差
  * @return {*}
  */	
	template <typename T>
	bool operator()(const T *rwg_, const T *vi_, const T *vj_, const T *ba_, const T *bg_, T *residual) const {
		Eigen::Matrix<T, 3, 1> G_I{T(0), T(0), T(-9.805)};
		
		Eigen::Map<const Eigen::Matrix<T, 3, 1>> ba(ba_);
		Eigen::Map<const Eigen::Matrix<T, 3, 1>> bg(bg_);
		Eigen::Matrix<T, 3, 1> dbg = bg - imu_measure.GetBiasGyr().cast<T>();
		Eigen::Matrix<T, 3, 1> dba = ba - imu_measure.GetBiasAcc().cast<T>();
		// imu相对与w的旋转 
		Sophus::SO3<T> SO3_Ri = Sophus::SO3<T>::exp(ri.cast<T>());
		Sophus::SO3<T> SO3_Rj = Sophus::SO3<T>::exp(rj.cast<T>());

		Eigen::Matrix<T, 3, 1> dP = dp.cast<T>();

		Eigen::Map<const Eigen::Matrix<T, 3, 1>> rwg(rwg_);
		Sophus::SO3<T> SO3_Rwg = Sophus::SO3<T>::exp(rwg);

		Eigen::Map<const Eigen::Matrix<T, 3, 1>> vi(vi_);
		Eigen::Matrix<T, 3, 1> Vi = vi;
		Eigen::Map<const Eigen::Matrix<T, 3, 1>> vj(vj_);
		Eigen::Matrix<T, 3, 1> Vj = vj;

		Eigen::Map<Eigen::Matrix<T, 9, 1> > eResiduals(residual);
		eResiduals = Eigen::Matrix<T, 9, 1>::Zero();

		T dTij = T(imu_measure.GetDeltaTime());
		T dT2 = dTij*dTij;
		Eigen::Matrix<T, 3, 1> dPij = imu_measure.GetDeltaP().cast<T>();   // imu 预积分量   
		Eigen::Matrix<T, 3, 1> dVij = imu_measure.GetDeltaV().cast<T>();   // imu 预积分量   
		Sophus::SO3<T> dRij = Sophus::SO3<T>(imu_measure.GetDeltaQ().cast<T>());   // imu 预积分量    
		Sophus::SO3<T> RiT = SO3_Ri.inverse();     // Rbw
		// 预积分位置残差
		// Rbw * (pj - pi - Vwi * t - Rlw * gw *t^2 * 0.5 )
		Eigen::Matrix<T, 3, 1> rPij = RiT*(dP - Vi*dTij - SO3_Rwg*G_I*dT2*T(0.5)) -
						(dPij + imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_P, IMUIntegrator::O_BG).cast<T>()*dbg +
						imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_P, IMUIntegrator::O_BA).cast<T>()*dba);
		// 预积分姿态残差
		Sophus::SO3<T> dR_dbg = Sophus::SO3<T>::exp(
						imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_R, IMUIntegrator::O_BG).cast<T>()*dbg);
		Sophus::SO3<T> rRij = (dRij * dR_dbg).inverse() * RiT * SO3_Rj;
		Eigen::Matrix<T, 3, 1> rPhiij = rRij.log();
		// 预积分速度残差
		Eigen::Matrix<T, 3, 1> rVij = RiT*(Vj - Vi - SO3_Rwg*G_I*dTij) -
						(dVij + imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_V, IMUIntegrator::O_BG).cast<T>()*dbg +
										imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_V, IMUIntegrator::O_BA).cast<T>()*dba);

		eResiduals.template segment<3>(0) = rPij;
		eResiduals.template segment<3>(3) = rPhiij;
		eResiduals.template segment<3>(6) = rVij;
		// 左乘信息矩阵
		eResiduals.applyOnTheLeft(sqrt_information.template cast<T>());

		return true;
	}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64

由于对于ceres只能接受残差$r$,因此，若考虑协方差阵$\Sigma$:
$$\Vert r{\Vert }_{\Sigma }^2=r^T{\Sigma }^{-1}r=(\sqrt{{\Sigma }^{-1}}r{)}^T\sqrt{{\Sigma }^{-1}}r$$
因此构造一个新的残差$r^{\prime }=\sqrt{{\Sigma }^{-1}}r$.

总结

这种一种简洁的紧耦合初始化方法，但是需要提前已知IMU与激光的外参，并且也并没有提供时间戳标定，后续可以想想如何加入外参初始化与时间戳对齐的功能…