代码浅析DLIO(四)---位姿更新

0. 简介

我们刚刚了解过DLIO的整个流程，我们发现相比于Point-LIO而言，这个方法更适合我们去学习理解，同时官方给出的结果来看DLIO的结果明显好于现在的主流方法，当然指的一提的是，这个DLIO是必须需要六轴IMU的，所以如果没有IMU的画，那只有DLO可以使用了。

1. getNextPose–通过IMU + S2M + GEO获取下一个姿态

下面的函数作用主要是获取下一个姿态的函数，主要是通过IMU、S2M和GEO三种方式获取。首先，检查新子地图是否准备就绪，如果准备好了并且子地图发生了变化，则将当前全局子地图设置为目标点云，并设置子图的kdtree以及将目标云的法线设置为子地图的法线。接着，使用全局IMU变换作为初始猜测，将当前子地图与全局地图对齐，并在全局坐标系中获取最终变换。然后，更新下一个全局位姿，并进行几何观察器更新。最终，该函数返回下一个姿态。

/**
 * @brief 通过IMU + S2M + GEO获取下一个姿态
 *
 */
void dlio::OdomNode::getNextPose() {

  // 检查新子地图是否准备好可供使用
  this->new_submap_is_ready =
      (this->submap_future.wait_for(std::chrono::seconds(0)) ==
       std::future_status::ready); //等待子地图准备好

  if (this->new_submap_is_ready &&
      this->submap_hasChanged) { //如果子地图准备好了,并且子地图发生了变化

    // 将当前全局子地图设置为目标点云
    this->gicp.registerInputTarget(this->submap_cloud);

    // 设置子图的kdtree,之前就是直接从target_kdtree_拿出来的,这个有必要嘛?
    this->gicp.target_kdtree_ = this->submap_kdtree;

    // 将目标云的法线设置为子地图的法线
    this->gicp.setTargetCovariances(this->submap_normals);

    this->submap_hasChanged = false;
  }

  // 使用全局IMU变换作为初始猜测，将当前子地图与全局地图对齐
  pcl::PointCloud<PointType>::Ptr aligned(
      boost::make_shared<pcl::PointCloud<PointType>>());
  this->gicp.align(*aligned); // 设置对齐后的地图

  // 在全局坐标系中获取最终变换
  this->T_corr =
      this->gicp.getFinalTransformation(); // 根据对齐后的地图来校准转换
  this->T = this->T_corr * this->T_prior;

  // 更新下一个全局位姿,现在源点云和目标点云都在全局坐标系中，所以变换是全局的
  this->propagateGICP();

  // 几何观察器更新
  this->updateState();
}

2. updateState–更新GEO信息

这段代码是通过GEO更新状态的，主要分为以下几个步骤：

1. 锁定线程以防止状态被PropagateState访问，保证线程安全。
2. 获取机器人当前的位置、四元数和时间差。
3. 通过拿到的四元数和预测的四元数构造误差的四元数，然后根据误差的四元数构建四元数校正量，对应公式7。
4. 将误差的四元数转换到机器人的body坐标系下。
5. 更新加速度偏差和陀螺仪偏差，同时限制偏差的最大值。
6. 更新机器人的速度和位置，以及四元数，同时存储前一个姿态、方向和速度。

void dlio::OdomNode::updateState() {

  // 锁定线程以防止状态被PropagateState访问
  std::lock_guard<std::mutex> lock(this->geo.mtx);

  Eigen::Vector3f pin = this->lidarPose.p;              // 位置
  Eigen::Quaternionf qin = this->lidarPose.q;           // 四元数
  double dt = this->scan_stamp - this->prev_scan_stamp; // 时间差

  Eigen::Quaternionf qe, qhat, qcorr;
  qhat = this->state.q;

  // 构造误差的四元数
  qe = qhat.conjugate() * qin; //通过拿到的四元数和预测的四元数构造误差的四元数

  double sgn = 1.;
  if (qe.w() < 0) { // 如果误差的四元数的w小于0
    sgn = -1;
  }

  // 构建四元数校正量,对应公式7
  qcorr.w() = 1 - abs(qe.w());  // 误差的四元数的w部分
  qcorr.vec() = sgn * qe.vec(); // 误差的四元数的向量部分
  qcorr = qhat * qcorr;         // 误差的四元数

  Eigen::Vector3f err = pin - this->state.p;
  Eigen::Vector3f err_body;

  err_body =
      qhat.conjugate()._transformVector(err); // 误差的四元数转换到body坐标系下

  double abias_max = this->geo_abias_max_;
  double gbias_max = this->geo_gbias_max_;

  // 更新加速度偏差
  this->state.b.accel -= dt * this->geo_Kab_ * err_body;
  this->state.b.accel =
      this->state.b.accel.array().min(abias_max).max(-abias_max);

  // 更新陀螺仪偏差
  this->state.b.gyro[0] -= dt * this->geo_Kgb_ * qe.w() * qe.x();
  this->state.b.gyro[1] -= dt * this->geo_Kgb_ * qe.w() * qe.y();
  this->state.b.gyro[2] -= dt * this->geo_Kgb_ * qe.w() * qe.z();
  this->state.b.gyro =
      this->state.b.gyro.array().min(gbias_max).max(-gbias_max);

  // 更新速度和位置
  this->state.p += dt * this->geo_Kp_ * err;
  this->state.v.lin.w += dt * this->geo_Kv_ * err;

  this->state.q.w() += dt * this->geo_Kq_ * qcorr.w();
  this->state.q.x() += dt * this->geo_Kq_ * qcorr.x();
  this->state.q.y() += dt * this->geo_Kq_ * qcorr.y();
  this->state.q.z() += dt * this->geo_Kq_ * qcorr.z();
  this->state.q.normalize();

  // 存储前一个姿态、方向和速度
  this->geo.prev_p = this->state.p;
  this->geo.prev_q = this->state.q;
  this->geo.prev_vel = this->state.v.lin.w;
}

3. updateKeyframes–更新关键帧

在做完位置更新后就会更新关键帧，通过计算当前姿态与轨迹中所有关键帧姿态的差异，然后根据距离和旋转角度是否超过一定阈值来判断是否需要更新关键帧。具体实现过程如下：

首先，遍历轨迹中所有关键帧，计算当前姿态与每个关键帧之间的距离，并记录最近的关键帧的索引和距离。同时，计算当前姿态附近的关键帧数量。

然后，获取最近关键帧的姿态和旋转，并计算当前姿态与最近关键帧之间的距离和旋转角度。如果旋转角度超过了设定的阈值，或者距离超过了设定的阈值且附近关键帧数量小于等于1，则认为需要更新关键帧。

最后，如果需要更新关键帧，则将当前姿态和点云、时间戳、法向量、变换矩阵等信息存储到关键帧向量中。

整个函数实现了关键帧的自适应更新，可以在SLAM系统中提高精度和效率。

void dlio::OdomNode::updateKeyframes() {

  // 计算轨迹中所有姿态和旋转的差异
  float closest_d = std::numeric_limits<float>::infinity();
  int closest_idx = 0;
  int keyframes_idx = 0;

  int num_nearby = 0;

  for (const auto &k : this->keyframes) {

    // 计算当前姿态与关键帧中的姿态之间的距离,这里和更新submap的操作一样
    float delta_d = sqrt(pow(this->state.p[0] - k.first.first[0], 2) +
                         pow(this->state.p[1] - k.first.first[1], 2) +
                         pow(this->state.p[2] - k.first.first[2], 2));

    //计算当前姿态附近的数量
    if (delta_d <= this->keyframe_thresh_dist_ * 1.5) {
      ++num_nearby;
    }

    // 将其存储到变量中
    if (delta_d < closest_d) {
      closest_d = delta_d;         //最近的距离
      closest_idx = keyframes_idx; //最近的关键帧的索引
    }

    keyframes_idx++;
  }

  // 获取最接近的姿势和相应的旋转
  Eigen::Vector3f closest_pose =
      this->keyframes[closest_idx].first.first; //最近的关键帧的位置
  Eigen::Quaternionf closest_pose_r =
      this->keyframes[closest_idx].first.second; //最近的关键帧的旋转

  // 计算当前姿势与最近的姿势之间的距离,和closest_d一致
  float dd = sqrt(pow(this->state.p[0] - closest_pose[0], 2) +
                  pow(this->state.p[1] - closest_pose[1], 2) +
                  pow(this->state.p[2] - closest_pose[2], 2));

  // 使用SLERP计算方向差异
  Eigen::Quaternionf dq;

  if (this->state.q.dot(closest_pose_r) <
      0.) { //如果两个四元数的点积小于0，说明两个四元数的方向相反
    Eigen::Quaternionf lq = closest_pose_r; //将最近的关键帧的旋转赋值给lq
    lq.w() *= -1.;
    lq.x() *= -1.;
    lq.y() *= -1.;
    lq.z() *= -1.;
    dq = this->state.q * lq.inverse(); //计算当前姿态与最近的姿态之间的旋转
  } else {
    dq = this->state.q *
         closest_pose_r.inverse(); //计算当前姿态与最近的姿态之间的旋转
  }

  double theta_rad =
      2. * atan2(sqrt(pow(dq.x(), 2) + pow(dq.y(), 2) + pow(dq.z(), 2)),
                 dq.w()); //计算当前姿态与最近的姿态之间的旋转角度
  double theta_deg = theta_rad * (180.0 / M_PI); //将弧度转换为角度

  // 更新关键帧
  bool newKeyframe = false;

  if (abs(dd) > this->keyframe_thresh_dist_ ||
      abs(theta_deg) >
          this->keyframe_thresh_rot_) { //如果距离或者旋转角度超过阈值
    newKeyframe = true;
  }

  if (abs(dd) <= this->keyframe_thresh_dist_ &&
      abs(theta_deg) > this->keyframe_thresh_rot_ &&
      num_nearby <=
          1) { //如果距离小于阈值，但是旋转角度超过阈值，且附近的关键帧数量小于等于1
    newKeyframe = true;
  }

  if (abs(dd) <= this->keyframe_thresh_dist_) { //如果距离小于阈值
    newKeyframe = false;
  } else if (abs(dd) <= 0.5) {
    newKeyframe = false;
  }

  if (newKeyframe) {

    // 更新关键帧向量
    std::unique_lock<decltype(this->keyframes_mutex)> lock(
        this->keyframes_mutex);
    this->keyframes.push_back(std::make_pair(
        std::make_pair(this->lidarPose.p, this->lidarPose.q),
        this->current_scan)); //将当前的姿态和点云压入到keyframes中
    this->keyframe_timestamps.push_back(
        this->scan_header_stamp); //将当前的时间戳压入到keyframe_timestamps中
    this->keyframe_normals.push_back(
        this->gicp
            .getSourceCovariances()); //将当前的法向量压入到keyframe_normals中
    this->keyframe_transformations.push_back(
        this->T_corr); //将当前的变换矩阵压入到keyframe_transformations中
    lock.unlock();
  }
}

4. propagateState–传播GEO状态

然后我们整个基本都过完了，只有一个IMU的feedback中漏掉的propagateState函数。通过IMU测量值来更新机器人的状态。首先通过一个mutex锁来确保线程安全。然后获取IMU测量的时间间隔，并将当前状态下的四元数和角速度赋值给变量qhat和omega。接下来将机体坐标系下的加速度转换到世界坐标系下，并使用加速度传播公式更新机器人的位置和速度。同时，将机器人的角速度更新为世界坐标系下的角速度。然后使用重力计传播公式更新四元数，确保其归一化。最后，将机器人的角速度和四元数更新到状态中。

…详情请参照古月居