LiDAR의 움직임을 PointCloud에 보상(Motion compensate)하는 방법

회전식 LiDAR를 통해 얻어지는 PointCloud는 모터가 회전하며 주변 물체를 scan 하기 때문에 필연적으로  1 Frame 안에 있는 point 간 취득된 시점이 달라지게 됩니다. 따라서 자율주행차량과 같이 LiDAR가 움직인다면, 이를 고려하여 Data Point 각각의 위치를 보상해줘야 합니다.  

 

Frame과 timestamp

LiDAR가 한 번 회전하면서 scan한 PointCloud는 3개의 Data Block씩 timestamp를 공유하며, 여러 timestamp 값을 가집니다. 이에 대한 자세한 설명은 아래 포스팅을 참고하면 됩니다.

2022.07.30 - [Perception Engineering (LiDAR)/LiDAR] - Robosense LiDAR의 Data Packet과 PointCloud Frame의 관계

Robosense LiDAR의 Data Packet과 PointCloud Frame의 관계

모션 보상을 하기 위해서는, 먼저 timestamp 중 가장 큰 시점과, 가장 작은 시점에서의 LiDAR Position (translation + rotation)을 구해야 합니다. Position을 구하기 위해서는 Localization과 Transform 기능의 정상 동작을 전제로 합니다. 

 

Quaternion 연산 정리

먼저 다음 블로그의 글을 참고해 주시길 바랍니다. 

http://chanhaeng.blogspot.com/2018/07/blog-post.html

쿼터니언 이해하기

1. Quaternion 표현

1) 쿼터니언의 일반적인 표현 형태

2) 쿼터니언의 순서쌍 표현

2. Quaternion Conjugate(켤레)

한 쿼터니언의 Conjugate(켤레)는 다음과 같이 구할 수 있습니다.

이때, 다음 관계식이 만족합니다.

3. Quaternion Normalization

쿼터니언을 표준화하는 방법은 쿼터니언의 크기(norm)으로 나누는 것입니다.

4. Quaternion Dot Product

쿼터니언 내적은 벡터 내적과 유사하게 대응되는 부분끼리 곱해주고, 전체 합해서 구합니다.

쿼터니언 사이의 각도를 쿼터니언 내적을 사용해 구할 수 있습니다.

5. (참고) Quaternion Interpolation : SLERP, SQUAD

Motion Compensate Code와 물리적 해석

해당 코드는 가장 큰 시점과, 가장 작은 시점에서의 LiDAR Position을 구했다 가정하고 이를 입력으로 모션 보상을 하는 함수입니다.

void Compensator::MotionCompensation(
    const std::shared_ptr<const PointCloud>& msg,
    std::shared_ptr<PointCloud> 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<double>(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<double>(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<float>(p.x()));
      point_new->set_y(static_cast<float>(p.y()));
      point_new->set_z(static_cast<float>(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<double>(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<float>(p.x()));
    point_new->set_y(static_cast<float>(p.y()));
    point_new->set_z(static_cast<float>(p.z()));
  }
}

먼저, abs_d 변수를 계산하는데, 이는 방향 전환의 크기를 계산하는 것입니다.. 1e-8은 LiDAR에 기본적으로 존재하는 Rotation 오차 값이고, 이보다 방향 전환이 작을 때와 작지 않을 때를 나눠서 모션 보상을 하게 됩니다.

---

https://github.com/ApolloAuto/apollo/blob/master/modules/drivers/lidar/velodyne/compensator/compensator.cc

GitHub - ApolloAuto/apollo: An open autonomous driving platform