vins中紧耦合优化模型（状态量状态方程观测方程）

最新推荐文章于 2024-07-13 16:16:21 发布

火星机器人life

最新推荐文章于 2024-07-13 16:16:21 发布

阅读量1.4k

点赞数

分类专栏： SLAM 文章标签： vins代码详解

本文链接：https://blog.csdn.net/ktigerhero3/article/details/91042110

版权

SLAM 专栏收录该内容

46 篇文章 22 订阅

订阅专栏

问题：vins中误差方程包括哪些？
vins中整体误差方程包含视觉重投影误差、IMU测量误差和先验误差。
优化的factor有
imu_factor.h,projection_factor.h，marginalization_factor.h
问题：vin中优化的状态量是什么？

１.imu测量误差

在imu_factor.h中

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

15, 7, 9, 7, 9分别代表１５维残差，前一帧７维位姿，前一帧９维（线速度３维，加速度计的线加速度偏移为３维，陀螺仪的角加速度偏移为３维），当前帧７维位姿，当前帧９维（线速度３维，加速度计的线加速度偏移为３维，陀螺仪的角加速度偏移为３维）

1.1残差

残差为１５维：残差分别为位移为３，旋转为３维，线速度为３维，加速度计的线加速度偏移为３维，陀螺仪的角加速度偏移为３维
残差赋值代码如下

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;

1.2状态量

优化的第一个状态量位姿为７维，平移３维，旋转的四元数为４维
优化的第二个状态量为９维度，线速度为３维，加速度计的线加速度偏移为３维，陀螺仪的角加速度偏移为３维
优化赋值代码如下

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

2.重投影误差

在projection_factor.h中

class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1>

2, 7, 7, 7, 1分别代表２维重投影残差，前一帧７维位姿，当前帧７维位姿，7维imu和camera坐标系的外参，最后１维为特征点的逆深度（inv_dep_i，具体还未弄清楚）

2.1残差

残差为２维：为重投影像素误差
（与传统的针孔相机模型定义的重投影误差不同，论文中使用的是单位半球体的相机观测残差，是一个宽视野鱼眼或者全方位相机。）

1.2状态量

优化的状态量为：
前一帧７维位姿，当前帧７维位姿，7维imu和camera坐标系的外参，最后１维为特征点的逆深度（inv_dep_i，具体还未弄清楚）

３.先验误差

在marginalization_factor.h中

class MarginalizationFactor : public ceres::CostFunction

优化的状态量是变化的，根据不同的状态来选择优化不同的状态量。
VINS-MONO中,为了处理一些悬停的case, 引入了一个two-way marginalization, 简单来说就是:
-如果倒数第二帧是关键帧, 则将最旧的pose移出sliding window, 也就是MARGIN_OLD,

如果倒数第二帧不是关键帧, 则将倒数第二帧pose移出sliding window, 也就是MARGIN_NEW.
选取关键帧的策略是视差足够大。
如在MARGIN_OLD状态时优化的可以为７维或７维

 for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
 {
      if (last_marginalization_parameter_blocks[i] == para_Pose[0] ||
          last_marginalization_parameter_blocks[i] == para_SpeedBias[0])
          drop_set.push_back(i);
  }
// construct new marginlization_factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL, last_marginalization_parameter_blocks,drop_set);

MARGIN_OLD状态时优化也可以为７维加９维

ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(imu_factor, NULL,
                                                                           vector<double *>{para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]},
                                                                           vector<int>{0, 1});

参考文献：
https://blog.csdn.net/wangshuailpp/article/details/78461171
https://blog.csdn.net/q597967420/article/details/76099443