fastlivo2-ekf-voxelmap函数总结

最新推荐文章于 2025-06-17 14:18:40 发布

原创最新推荐文章于 2025-06-17 14:18:40 发布 · 1.2k 阅读

CC 4.0 BY-SA版权

文章标签：

fastlivo中：obtained by fusing the last LiDAR or image measurement
(see Section IV-E). We denote the state and covariance
propagated until t k as x̂ k and P̂ k , respectively. Notice that
we do not assume LiDAR scans and images are received at
the same time. The arrival of either a LiDAR scan or image
will cause an update of the state as detailed in Section IV-E.

🧠 核心要点提炼：

概念	说明
xˉk−1,Pˉk−1\bar{x}_{k-1}, \bar{P}_{k-1}xˉk−1,Pˉk−1	上一帧（LiDAR 或图像）观测融合后得到的状态和协方差
x^k,P^k\hat{x}_k, \hat{P}_kx^k,P^k	从 tk−1t_{k-1}tk−1 到 tkt_ktk 期间用 IMU 连续预测得到的先验
IMU 测量 uiu_iui	在 tk−1t_{k-1}tk−1 和 tkt_ktk 之间接收的惯性数据
LiDAR or 图像到达	无论哪一个先到，都会触发一次 EKF 更新
更新位置	详见论文 Section IV-E（即 EKF 优化主循环的部分）

状态预测（公式2）和协方差（公式3）是从时间 tk−1t_{k-1}tk−1 开始进行传播的，此时最近一次接收到 LiDAR 或图像测量，
传播一直持续到时间 tkt_ktk，此时当前帧的 LiDAR 或图像测量到达，
在这两个时间点之间，会不断接收 IMU 测量 uiu_iui，并用它们进行预测传播。

在公式（2）和（3）中的初始状态 xˉk−1\bar{x}_{k-1}xˉk−1 和协方差 Pˉk−1\bar{P}_{k-1}Pˉk−1，是通过上一帧 LiDAR 或图像观测融合得到的（详见论文第 IV-E 节）。

我们将传播到 tkt_ktk 时刻的状态和协方差分别记作 x^k\hat{x}_kx^k 和 P^k\hat{P}_kP^k。

注意，我们并不要求 LiDAR 扫描和图像帧同时到达。
无论是 LiDAR 扫描或图像帧的到达，都会触发一次状态更新，详细过程见第 IV-E 节。

总结：

特性	Fast-LIVO	Fast-LIVO2
状态更新方式	谁先来谁更新（交替）	顺序更新（sequential fusion）
每轮优化可融合的残差	一种（LiDAR 或图像）	多种（LiDAR + 图像）
多模态信息融合程度	松耦合	紧耦合 ✅
精度和一致性	较好	更好 ✅
实现复杂度	中等	高 ✅

🧭 总结一句话：

✅ Fast-LIVO2 使用的是顺序更新机制，在每一帧更新中，将视觉残差和雷达残差在同一次 EKF 优化循环中依次融合，形成真正的紧耦合联合优化框架。

这也是 Fast-LIVO → Fast-LIVO2 最核心的系统性进化之一。

void VoxelMapManager::StateEstimation(StatesGroup &state_propagat)
{
  cross_mat_list_.clear();
  cross_mat_list_.reserve(feats_down_size_);
  body_cov_list_.clear();
  body_cov_list_.reserve(feats_down_size_);

  // build_residual_time = 0.0;
  // ekf_time = 0.0;
  // double t0 = omp_get_wtime();

  for (size_t i = 0; i < feats_down_body_->size(); i++)
  {
    //遍历降采样后的点云数据 feats_down_body_；
//取出每个点的坐标并转换为 Eigen 向量 V3D 类型（Vector3d）；
V3D point_this(feats_down_body_->points[i].x, feats_down_body_->points[i].y, feats_down_body_->points[i].z);
    if (point_this[2] == 0) { point_this[2] = 0.001; }
    M3D var;
    calcBodyCov(point_this, config_setting_.dept_err_, config_setting_.beam_err_, var);
    body_cov_list_.push_back(var);
    point_this = extR_ * point_this + extT_;
    M3D point_crossmat;
    point_crossmat << SKEW_SYM_MATRX(point_this);
    cross_mat_list_.push_back(point_crossmat);
  }

✅ 整体作用：

遍历降采样后的激光点云数据 feats_down_body_，为每个点计算其在传感器坐标系下的协方差 body_cov_list_ 和其世界坐标的反对称矩阵（交叉乘矩阵） cross_mat_list_，作为后续联合更新的一部分准备数据。

vector<pointWithVar>().swap(pv_list_);
  pv_list_.resize(feats_down_size_);

  int rematch_num = 0;
  MD(DIM_STATE, DIM_STATE) G, H_T_H, I_STATE;
  G.setZero();
  H_T_H.setZero();
  I_STATE.setIdentity();

  bool flg_EKF_inited, flg_EKF_converged, EKF_stop_flg = 0;

✅ 小结这段的作用：

这部分是 EKF 联合更新流程的准备阶段，它主要完成了：

清理并初始化点结构列表 pv_list_；
初始化矩阵：
- 状态协方差更新项 G；
- 雅可比项累加器 H_T_H；
- 单位矩阵 I_STATE；
设置若干 EKF 状态控制变量，用于后续的联合迭代更新；

for (int iterCount = 0; iterCount < config_setting_.max_iterations_; iterCount++)
  {
    double total_residual = 0.0;
    pcl::PointCloud<pcl::PointXYZI>::Ptr world_lidar(new pcl::PointCloud<pcl::PointXYZI>);
    TransformLidar(state_.rot_end, state_.pos_end, feats_down_body_, world_lidar);
    M3D rot_var = state_.cov.block<3, 3>(0, 0);
    M3D t_var = state_.cov.block<3, 3>(3, 3);
    for (size_t i = 0; i < feats_down_body_->size(); i++)
    {
      pointWithVar &pv = pv_list_[i];
      pv.point_b << feats_down_body_->points[i].x, feats_down_body_->points[i].y, feats_down_body_->points[i].z;
      pv.point_w << world_lidar->points[i].x, world_lidar->points[i].y, world_lidar->points[i].z;

      M3D cov = body_cov_list_[i];
      M3D point_crossmat = cross_mat_list_[i];
      cov = state_.rot_end * cov * state_.rot_end.transpose() + (-point_crossmat) * rot_var * (-point_crossmat.transpose()) + t_var;
      pv.var = cov;
      pv.body_var = body_cov_list_[i];
    }
    ptpl_list_.clear();

    // double t1 = omp_get_wtime();

    BuildResidualListOMP(pv_list_, ptpl_list_);

    // build_residual_time += omp_get_wtime() - t1;

🧠 总结本轮迭代主要任务：

把 body 坐标下的点转换为 world 坐标；
计算每个点在 world 坐标下的测量协方差（考虑传感器误差 + 姿态/平移误差）；
构建匹配残差列表，为后续 EKF 的雅可比和卡尔曼增益计算做准备；


    for (int i = 0; i < ptpl_list_.size(); i++)
    {
      total_residual += fabs(ptpl_list_[i].dis_to_plane_);
    }
    effct_feat_num_ = ptpl_list_.size();
    cout << "[ LIO ] Raw feature num: " << feats_undistort_->size() << ", downsampled feature num:" << feats_down_size_ 
         << " effective feature num: " << effct_feat_num_ << " average residual: " << total_residual / effct_feat_num_ << endl;

✅ 总结当前这段代码的作用：

统计所有参与 EKF 更新的点对的残差；
计算有效点数量；
输出优化质量（是否收敛，误差是否变小）；
为判断是否继续迭代，或提前停止提供依据；

   /*** Computation of Measuremnt Jacobian matrix H and measurents covarience
     * ***/
    MatrixXd Hsub(effct_feat_num_, 6);
    MatrixXd Hsub_T_R_inv(6, effct_feat_num_);
    VectorXd R_inv(effct_feat_num_);
    VectorXd meas_vec(effct_feat_num_);
    meas_vec.setZero();
    for (int i = 0; i < effct_feat_num_; i++)
    {
      auto &ptpl = ptpl_list_[i];
      V3D point_this(ptpl.point_b_);
      point_this = extR_ * point_this + extT_;
      V3D point_body(ptpl.point_b_);
      M3D point_crossmat;
      point_crossmat << SKEW_SYM_MATRX(point_this);

      /*** get the normal vector of closest surface/corner ***/

      V3D point_world = state_propagat.rot_end * point_this + state_propagat.pos_end;
      Eigen::Matrix<double, 1, 6> J_nq;
      J_nq.block<1, 3>(0, 0) = point_world - ptpl_list_[i].center_;
      J_nq.block<1, 3>(0, 3) = -ptpl_list_[i].normal_;

      M3D var;
      

      double sigma_l = J_nq * ptpl_list_[i].plane_var_ * J_nq.transpose();

      R_inv(i) = 1.0 / (0.001 + sigma_l + ptpl_list_[i].normal_.transpose() * var * ptpl_list_[i].normal_);
     
      /*** calculate the Measuremnt Jacobian matrix H ***/
      V3D A(point_crossmat * state_.rot_end.transpose() * ptpl_list_[i].normal_);
      Hsub.row(i) << VEC_FROM_ARRAY(A), ptpl_list_[i].normal_[0], ptpl_list_[i].normal_[1], ptpl_list_[i].normal_[2];
      Hsub_T_R_inv.col(i) << A[0] * R_inv(i), A[1] * R_inv(i), A[2] * R_inv(i), ptpl_list_[i].normal_[0] * R_inv(i),
          ptpl_list_[i].normal_[1] * R_inv(i), ptpl_list_[i].normal_[2] * R_inv(i);
      meas_vec(i) = -ptpl_list_[i].dis_to_plane_;
    }

变量名	含义
`Hsub`	测量雅可比矩阵 HHH，每行对状态变量偏导（6维：3旋转+3平移）
`Hsub_T_R_inv`	HTR−1H^T R^{-1}HTR−1，用于高效构造信息矩阵
`R_inv`	每个残差的测量协方差逆，用于残差加权
`meas_vec`	残差向量 zzz，表示每个点到局部平面的距离误差

✅ 总结

这一段做的事就是：
🔧 为联合 EKF 更新构造观测矩阵 H、残差向量 z、以及测量协方差逆 R−1R^{-1}R−1，用来后续计算 Kalman 增益与状态修正。

如果我们用数学形式概括这一段代码，它就是构建了如下三项：

H∈Rn×6\mathbf{H} \in \mathbb{R}^{n \times 6}H∈Rn×6：观测雅可比矩阵
z∈Rn\mathbf{z} \in \mathbb{R}^{n}z∈Rn：残差向量
Ri−1R_i^{-1}Ri−1：每个点观测误差的权重

逐个点处理，开始构造每一行的 HiH_iHi、ziz_izi、RiR_iRi。

    EKF_stop_flg = false;
    flg_EKF_converged = false;
    /*** Iterative Kalman Filter Update ***/
    MatrixXd K(DIM_STATE, effct_feat_num_);
    
    auto &&HTz = Hsub_T_R_inv * meas_vec;
    // fout_dbg<<"HTz: "<<HTz<<endl;
    H_T_H.block<6, 6>(0, 0) = Hsub_T_R_inv * Hsub;
   
    MD(DIM_STATE, DIM_STATE) &&K_1 = (H_T_H.block<DIM_STATE, DIM_STATE>(0, 0) + state_.cov.block<DIM_STATE, DIM_STATE>(0, 0).inverse()).inverse();
    G.block<DIM_STATE, 6>(0, 0) = K_1.block<DIM_STATE, 6>(0, 0) * H_T_H.block<6, 6>(0, 0);
    auto vec = state_propagat - state_;
    VD(DIM_STATE)
    solution = K_1.block<DIM_STATE, 6>(0, 0) * HTz + vec.block<DIM_STATE, 1>(0, 0) - G.block<DIM_STATE, 6>(0, 0) * vec.block<6, 1>(0, 0);
    int minRow, minCol;
    state_ += solution;
    auto rot_add = solution.block<3, 1>(0, 0);
    auto t_add = solution.block<3, 1>(3, 0);

🧠 总结这段做了什么？

步骤	说明
1️⃣	构建测量项残差 HTR−1zH^T R^{-1} zHTR−1z 和信息矩阵 HTR−1HH^T R^{-1} HHTR−1H
2️⃣	结合先验协方差 PPP，构造融合形式 K1K_1K1
3️⃣	计算最终联合状态更新量（融合观测与预测）
4️⃣	更新状态变量 `state_`
5️⃣	提取旋转/平移修正用于判断收敛或后续输出

    if ((rot_add.norm() * 57.3 < 0.01) && (t_add.norm() * 100 < 0.015)) { flg_EKF_converged = true; }
    V3D euler_cur = state_.rot_end.eulerAngles(2, 1, 0);

    /*** Rematch Judgement ***/

    if (flg_EKF_converged || ((rematch_num == 0) && (iterCount == (config_setting_.max_iterations_ - 2)))) { rematch_num++; }

    /*** Convergence Judgements and Covariance Update ***/
    if (!EKF_stop_flg && (rematch_num >= 2 || (iterCount == config_setting_.max_iterations_ - 1)))
    {
      /*** Covariance Update ***/
      // _state.cov = (I_STATE - G) * _state.cov;
      state_.cov.block<DIM_STATE, DIM_STATE>(0, 0) =
          (I_STATE.block<DIM_STATE, DIM_STATE>(0, 0) - G.block<DIM_STATE, DIM_STATE>(0, 0)) * state_.cov.block<DIM_STATE, DIM_STATE>(0, 0);
      // total_distance += (_state.pos_end - position_last).norm();
      position_last_ = state_.pos_end;
      geoQuat_ = tf::createQuaternionMsgFromRollPitchYaw(euler_cur(0), euler_cur(1), euler_cur(2));

      // VD(DIM_STATE) K_sum  = K.rowwise().sum();
      // VD(DIM_STATE) P_diag = _state.cov.diagonal();
      EKF_stop_flg = true;
    }
    if (EKF_stop_flg) break;
  }

  // double t2 = omp_get_wtime();
  // scan_count++;
  // ekf_time = t2 - t0 - build_residual_time;

  // ave_build_residual_time = ave_build_residual_time * (scan_count - 1) / scan_count + build_residual_time / scan_count;
  // ave_ekf_time = ave_ekf_time * (scan_count - 1) / scan_count + ekf_time / scan_count;

  // cout << "[ Mapping ] ekf_time: " << ekf_time << "s, build_residual_time: " << build_residual_time << "s" << endl;
  // cout << "[ Mapping ] ave_ekf_time: " << ave_ekf_time << "s, ave_build_residual_time: " << ave_build_residual_time << "s" << endl;
}

🧠 最终总结这一部分做了什么？

步骤	作用
✅ 判断收敛	根据姿态和平移变化是否小于阈值
🔄 Rematch 控制	如果还没收敛，则可能触发 rematch
📉 协方差更新	根据信息增益公式更新状态不确定性
💾 保存状态	保存位姿、更新 last position、构造 TF 输出用的四元数
🛑 终止条件	达到收敛 / 次数限制 / rematch 次数限制时结束迭代

for iterCount in [0, max_iterations):

    // 1. 点云转换：将 body 点云转换到世界坐标系
    world_lidar = TransformLidar(state_.rot_end, state_.pos_end, feats_down_body_)

    // 2. 为每个点计算测量协方差，并转换为世界坐标系下的结构
    for each point i in feats_down_body_:
        point_b = feats_down_body_[i]
        point_w = TransformPoint(state_, extR_, extT_, point_b)
        cov = propagateCov(point_b, state_.cov, body_cov_list_[i])
        pv_list_[i] = { point_b, point_w, cov }

    // 3. 构建匹配的点-平面对
    ptpl_list_ = BuildResidualListOMP(pv_list_)

    // 4. 统计总残差与有效特征点数
    total_residual = sum(|dis_to_plane| for ptpl in ptpl_list_)
    effct_feat_num = ptpl_list_.size()

    // 5. 构建残差向量 z，观测雅可比矩阵 H，以及测量协方差逆 R⁻¹
    for i in [0, effct_feat_num):
        ptpl = ptpl_list_[i]
        point_this = TransformToIMU(ptpl.point_b, extR_, extT_)
        point_crossmat = skew(point_this)
        point_world = TransformToWorld(state_propagat, point_this)
        J_nq = ComputeJnp(point_world, ptpl.center, ptpl.normal)
        sigma_l = J_nq * ptpl.plane_var * J_nqᵀ
        R_inv[i] = 1 / (sigma_l + normalᵀ * cov * normal)
        H[i] = [crossmat * Rᵀ * normal, normal]
        H_T_R_inv[:, i] = H[i] * R_inv[i]
        z[i] = -ptpl.dis_to_plane

    // 6. 联合状态更新（FastLIO-style EKF）
    HTz = H_T_R_inv * z
    HTH = H_T_R_inv * H
    K1 = (HTH + P⁻¹).inverse()
    G = K1 * HTH
    dx = K1 * HTz + (x_p - x) - G * (x_p - x)
    state_ += dx

    // 7. 判断收敛：增量是否足够小
    if (rotation_delta(dx) < 0.01° and translation_delta(dx) < 1.5cm):
        flg_EKF_converged = true

    // 8. 控制 Rematch（如果刚收敛或接近尾声）
    if (flg_EKF_converged or (rematch_num == 0 and iterCount == max_iterations - 2)):
        rematch_num += 1

    // 9. 达到终止条件，更新协方差并退出迭代
    if (!EKF_stop_flg and (rematch_num >= 2 or iterCount == max_iterations - 1)):
        state_.cov = (I - G) * state_.cov
        position_last_ = state_.pos_end
        geoQuat_ = ConvertToQuaternion(state_.rot_end)
        EKF_stop_flg = true

    if (EKF_stop_flg):
        break