apollo7.0------浅谈激光雷达运动补偿(一)

0. 简介：激光雷达帧率一般为10hz, 也就是100ms一帧，在车辆高速行驶或者转弯时，一帧点云中的点并不是同一个坐标系下获得的测量结果，对于同一个目标在三维点云中就会出现畸变，这时要想获得精确的测量，就需要对点云做运动补偿，或者说即便校正，将同一帧点云中所有点统一到某一个时间点下的坐标系中。

1. 举例：如果做低速移动平台，可能对即便矫正的需求没有那么高，毕竟效果不明显，还额外增加了耗时，但是对于高速运动的车辆，激光雷达的运动畸变对于感知和定位都有着非常大的影响，是非常基础的一个环节。简单举几个例子：

（1）在起伏路面，激光雷达点云在一帧点云的起始位置会产生多条线的交错，在点云中形成高度差，仿佛检测到了目标，极易造成地面物检。

待补图

（2）如果一个目标正处于一帧点云的0-360度交叉处，试想一下车辆是72km/h, 就是20m/s，100ms时间就差了2m, 感知目标检测极有可能将目标检测为两个，造成误检，随着目标的移动两个目标可能合二为一，对于追踪预测，速度估计的精准度都会产生一定的影响。

 如上图左侧展示的为旋转过程中点云的畸变，可以看到红圈中车辆出现重影，右侧为运动补偿后车辆点云的轮廓，清晰完整。

（3）对slam中建图定位更是需要运动补偿，没有运动补偿的点云在大规模建图极易跑偏，导致最后地图不在一个水平面上，点云的运动畸变相当于直接降低了激光测距的精度，从2-3cm精度下降到几十厘米甚至几米的误差，将直接导致定位精度无法提升，定位飘移问题。

 如上图所示，白色为车辆在旋转过程中的原始点云，彩色为运动补偿后的点云，可以看到墙面发生了很严重的畸变，远处甚至到达了几米的差距，这种情况下没有运动补偿的情况下激光雷达定位效果肯定会变差。

2. apollo 程序

（1）程序在/apollo/modules/drivers/lidar/velodyne/compensator，apollo将运动补偿程序放到了lidar驱动的velodyne驱动下面，其他robosense和hesai的驱动也是用的这个模块，比apollo 3.0的代码结构更简洁了，3.0里面每个雷达驱动下面都有一个运动补偿程序，代码重复性很高。

（2）modules/drivers/lidar/velodyne/compensator/compensator_component.cc中Proc()函数就是主要函数，没什么东西，主要实现都在compensator.cc的

bool Compensator::MotionCompensation(
    const std::shared_ptr& msg,
    std::shared_ptr msg_compensated) {
// 1. 获取原始点云的最大最小时间，前提是你的点云是带时间戳的
//     注意这里是用的uint64_t，计数是纳秒1s = 1e9ns
//     GetTimestampInterval 这里直接暴力循环获得最大最小时间
// 2. 根据一帧点云的起始时间查询坐标变换tf激光雷达在世界坐标系下的两个位姿
//     QueryPoseAffineFromTF2 就是ros里面tf查询该的
// 3. 根据时间戳对位姿进行插值，以最大的时间戳为基准，计算每个点和最后一个点时间差，计算坐标变换关系，        将点转换到最大时间点的坐标系下
//     MotionCompensation
}

（3）核心就是这个位姿插值坐标变换，这里做了一个判断，旋转误差是否显著--"significant"，激光雷达在70m测据精度2cm, 一个显著的旋转应该是

0.02 / 70 = 0.0003 rad.

不显著的仅作坐标平移即可。

（计算部分解释参考第二篇博客apollo7.0------浅谈激光雷达运动补偿(二)--计算解析_龙性的腾飞的博客-CSDN博客）

void Compensator::MotionCompensation(
    const std::shared_ptr& msg,
    std::shared_ptr msg_compensated, const uint64_t timestamp_min,
    const uint64_t timestamp_max, const Eigen::Affine3d& pose_min_time,
    const Eigen::Affine3d& pose_max_time) {
  using std::abs;
  using std::acos;
  using std::sin;

  Eigen::Vector3d translation =
      pose_min_time.translation() - pose_max_time.translation();
  Eigen::Quaterniond q_max(pose_max_time.linear());
  Eigen::Quaterniond q_min(pose_min_time.linear());
  Eigen::Quaterniond q1(q_max.conjugate() * q_min);
  Eigen::Quaterniond q0(Eigen::Quaterniond::Identity());
  q1.normalize();
  translation = q_max.conjugate() * translation;

  // int total = msg->width * msg->height;

  double d = q0.dot(q1);
  double abs_d = abs(d);
  double f = 1.0 / static_cast(timestamp_max - timestamp_min);

  // Threshold for a "significant" rotation from min_time to max_time:
  // The LiDAR range accuracy is ~2 cm. Over 70 meters range, it means an angle
  // of 0.02 / 70 =
  // 0.0003 rad. So, we consider a rotation "significant" only if the scalar
  // part of quaternion is
  // less than cos(0.0003 / 2) = 1 - 1e-8.
  if (abs_d < 1.0 - 1.0e-8) {
    double theta = acos(abs_d);
    double sin_theta = sin(theta);
    double c1_sign = (d > 0) ? 1 : -1;
    for (const auto& point : msg->point()) {
      float x_scalar = point.x();
      if (std::isnan(x_scalar)) {
        // if (config_.organized()) {
        auto* point_new = msg_compensated->add_point();
        point_new->CopyFrom(point);
        // } else {
        //   AERROR << "nan point do not need motion compensation";
        // }
        continue;
      }
      float y_scalar = point.y();
      float z_scalar = point.z();
      Eigen::Vector3d p(x_scalar, y_scalar, z_scalar);

      uint64_t tp = point.timestamp();
      double t = static_cast(timestamp_max - tp) * f;

      Eigen::Translation3d ti(t * translation);

      double c0 = sin((1 - t) * theta) / sin_theta;
      double c1 = sin(t * theta) / sin_theta * c1_sign;
      Eigen::Quaterniond qi(c0 * q0.coeffs() + c1 * q1.coeffs());

      Eigen::Affine3d trans = ti * qi;
      p = trans * p;

      auto* point_new = msg_compensated->add_point();
      point_new->set_intensity(point.intensity());
      point_new->set_timestamp(point.timestamp());
      point_new->set_x(static_cast(p.x()));
      point_new->set_y(static_cast(p.y()));
      point_new->set_z(static_cast(p.z()));
    }
    return;
  }
  // Not a "significant" rotation. Do translation only.
  for (auto& point : msg->point()) {
    float x_scalar = point.x();
    if (std::isnan(x_scalar)) {
      AERROR << "nan point do not need motion compensation";
      continue;
    }
    float y_scalar = point.y();
    float z_scalar = point.z();
    Eigen::Vector3d p(x_scalar, y_scalar, z_scalar);

    uint64_t tp = point.timestamp();
    double t = static_cast(timestamp_max - tp) * f;
    Eigen::Translation3d ti(t * translation);

    p = ti * p;

    auto* point_new = msg_compensated->add_point();
    point_new->set_intensity(point.intensity());
    point_new->set_timestamp(point.timestamp());
    point_new->set_x(static_cast(p.x()));
    point_new->set_y(static_cast(p.y()));
    point_new->set_z(static_cast(p.z()));
  }
}

在直行路上运动补偿效果如下：白色为原始点云，车辆向前行驶，激光雷达顺时针旋转