apollo7.0------浅谈激光雷达运动补偿(二)--计算解析

2023-05-16

背景介绍

运动补偿相关介绍参考第一篇博客：apollo7.0------浅谈激光雷达运动补偿_龙性的腾飞的博客-CSDN博客_lidar运动补偿

本篇博客主要解释一下上篇博客中运动补偿的计算部分，简单来说就是一个利用四元数球面线性插值（Slerp）计算每个点的位姿，然后根据坐标变换关系将该位姿下的坐标变换到需要补偿到的某个时间点下坐标．

在slam14讲中第3讲三维空间刚体运动的课后７题和是一个非常简单的示例如下，可以看一下，那个看明白这个就很简单了，答案可直接百度．

apollo 代码中是以每帧点云中最后激光点的时间作为该帧数据的时间戳，所以我们运动补偿的目标很清晰了，计算中间每个点在最大时间点位姿下的坐标值．

计算解析

根据里程计和激光雷达的关系，我们已知一帧点云中最大时间和最小时间激光雷达坐标系到世界坐标系下的变换关系 $T_{max}$ , $T_{min}$ 以及对应的时间点 $time_{max}$ , $time_{min}$ ,所以中间某点时间 $time_{t}$ 下的坐标 $p_{t}$ 到世界坐标的变换关系可以表示为的 $T_{t}$ , 假设该点在 $T_{max}$ 位姿下的坐标为 $p_{max}$ ,那么有：

$T_{max}*p_{max} = T_{t}*p_{t}$

$p_{max} = T_{max}^{-1}*T_{t}*p_{t}$

对于Transform矩阵

$T=\begin{bmatrix} R & t\\ 0^T & 1 \end{bmatrix}$

则：

$T^{-1}=\begin{bmatrix} R^{-1} & -R^{-1}t\\ 0^T & 1 \end{bmatrix}$

旋转矩阵为正交阵，它的逆即转置，描述了一个相反的旋转，对于四元数 $q$ 在, 用共轭 $q^*$ 表示一个相反的旋转.

$p_{max} = \begin{bmatrix} R_{max}^{-1} & -R_{max}^{-1}t_{max}\\ 0^T & 1 \end{bmatrix}*\begin{bmatrix} R_{t} & t_{t}\\ 0^T & 1 \end{bmatrix}*p_{t}$

即：

$p_{max} = \begin{bmatrix} R_{max}^{-1}R_{t} & R_{max}^{-1} (t_{t}-t_{max})\\ 0^T & 1 \end{bmatrix}*p_{t}$

变换矩阵分为旋转和平移部分：

旋转部分: 采用四元数表示即: $q_{max}^*q_{t}$ ,其中 $q_{t}$ 可以理解为 $time_{t}$ 时刻某点 $p_{t}$ 的姿态，根据球面线性插值（Spherical Linear Interpolation）公式：

$q_{t} = Slerp(q_{0},q_{1},t) = \frac{\sin ((1-t)\theta )}{\sin (\theta )}*q_{0}+\frac{\sin (t\theta )}{\sin (\theta )}*q_{1}$

我们定义的 $q_{max}$ 即相当于 $q_{0}$ ， $q_{min}$ 相当于 $q_{1}$ .

注意:插值公式里的的t为[0-1]的比例系数，不是上文中代表的时间t，此处t即：

$t =( time_{max}-time_{t})/( time_{max}-time_{min})$

详细推导可参考：https://krasjet.github.io/quaternion/quaternion.pdf 中5.3节,所以有

$q_{max}^*q_{t}=\frac{\sin ((1-t)\theta )}{\sin (\theta )}*q_{max}^**q_{max}+\frac{\sin (t\theta )}{\sin (\theta )}*q_{max}^**q_{min}$

其中： $q_{max}^**q_{max}=1$ ，此处即对应代码中

  Eigen::Quaterniond q1(q_max.conjugate() * q_min);
  Eigen::Quaterniond q0(Eigen::Quaterniond::Identity());

平移部分:即: $q_{max}^*(t_{t}-t_{max})$ , 其中

$t_{t}-t_{max} =(t_{min}-t_{max})*( time_{max}-time_{t})/( time_{max}-time_{min})$

根据Eigen中Eigen: Eigen::Transform< Scalar_, Dim_, Mode_, Options_ > Class Template Reference表述，一个Tramsform表示为如下

$\begin{pmatrix} linear & translation\\ 0...0& 1 \end{pmatrix}$ ，所以代码中平移和旋转代码如下：

  Eigen::Vector3d t1 = pose_min_time.translation();
  Eigen::Vector3d t2 = pose_max_time.translation();

  Eigen::Quaterniond q_max(pose_max_time.linear());
  Eigen::Quaterniond q_min(pose_min_time.linear());

下面再回顾下完整的代码吧！

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()));
  }
}

NOTE：

(1) 代码中四元数相乘后需要归一化，四元数归一化作用可参考博客理解：关于四元数归一化_可即的博客-CSDN博客_四元数归一化

归一化的意义：
四元数归一化：对四元数的单位化，单位化的四元数可以表示一个旋转.
规范化四元数作用：
1.表征旋转的四元数应该是规范化的四元数，但是由于计算误差等因素, 计算过程中四元数会逐渐失去规范化特性，因此必须对四元数做规范化处理
2.意义在于单位化四元数在空间旋转时是不会拉伸的,仅有旋转角度.这类似与线性代数里面的正交变换.
3.由于误差的引入，使得计算的变换四元数的模不再等于1，变换四元数失去规范性，因此需要再次更新四元数
————————————————
版权声明：本文为CSDN博主「可即」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/xiaojinger_123/article/details/126873205

(2) 代码中c1_sign的判断是为了判断两个四元数夹角，保证插值走的最短路径，理解可参考https://krasjet.github.io/quaternion/quaternion.pdf 中5.4 双倍覆盖带来的问题以及文章四元数的球面线性插值（slerp） - 知乎。

水平有限，错误难免，欢迎指正，留言交流。

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)