(02)Cartographer源码无死角解析-(30) LocalTrajectoryBuilder2D::AddRangeData()→激光雷达运动畸变较正

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$

 

一、前言

在上一篇博客中，对 LocalTrajectoryBuilder2D::AddRangeData() 函数中的如下部分进行了讲解：

  // Step: 1 进行多个雷达点云数据的时间同步, 点云的坐标是相对于tracking_frame的
  auto synchronized_data =
      range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
  if (synchronized_data.ranges.empty()) { //判断同步的点云数据是否为空，
  	//通常在多雷达传感器中，expected_sensor_ids_ 数据没有到齐。
    LOG(INFO) << "Range data collator filling buffer."; 
    return nullptr;//表示数据已经添加到数据整理器的缓存中。
  }

之前或许看过之后不是很明白，但是现在应该是比较清楚了，其是把多个雷达数据的点云，按时间戳排序整合到一起，且都是基于 tracking_frame 坐标系的。其返回的数据结构如下所示：

// 时间同步前的点云
struct TimedPointCloudData {
  common::Time time;        // 点云最后一个点的时间
  Eigen::Vector3f origin;   // 雷达传感器到 tracking_frame_ 的平移
  TimedPointCloud ranges;   // 数据点的集合, 每个数据点包含xyz与time, time是负的
  // 'intensities' has to be same size as 'ranges', or empty.
  std::vector<float> intensities; // 空的
};

// 时间同步后的点云
struct TimedPointCloudOriginData {
  struct RangeMeasurement {
    TimedRangefinderPoint point_time;   // 带时间戳的单个数据点的坐标 xyz
    float intensity;                    // 强度值
    size_t origin_index;                // // 该点用了“origins”中哪个原点，对单一、有序场景，既然origins.size()总是1，那它只能是0。
  };
  common::Time time;                    // 点云的时间
  // ranges中点云涉及哪些坐标系，origins存储着这些坐标系原点在tracking_frame_坐标系下的坐标。
  // 针对单一、有序场景，传感器只涉及一个坐标系，像“laser”，所以origins.size()总是1，值是TimedPointCloudData.origin。
  std::vector<Eigen::Vector3f> origins;
  std::vector<RangeMeasurement> ranges; // 雷达原点在机器人坐标系下的位置
};

为了方便讲解，我们就认为上面的包含的就是一帧数据，只是有包含多个雷达数据而已。后续称为多雷达时间同步数据、时间同步雷达数据、时间同步点云数据都差不多表达一个意思。注意时间同步之后的雷达数据，也就是上面结构体中 TimedPointCloudOriginData:: time 变量表示的是 current_start_，也就是时间同步点云最后一个数据的时间戳，TimedPointCloudOriginData::RangeMeasurement::point_time 表示相对于 TimedPointCloudOriginData:: time 的时间，一般来说该为0或者负数。

TimedPointCloudOriginData::origins 存储了所有雷达传感器原点相对于 tracking_frame_ 坐标系的平移，通过 TimedPointCloudOriginData ::ranges::origin_index 进行索引即可获取
 

二、AddRangeData() 续接

回到 LocalTrajectoryBuilder2D::AddRangeData 函数，继续往下分析。

  const common::Time& time = synchronized_data.time;
  // Initialize extrapolator now if we do not ever use an IMU.
  // 如果不用imu, 就在雷达这初始化位姿推测器
  if (!options_.use_imu_data()) {
    InitializeExtrapolator(time);
  }

因为我们使用了imu，所以这里不初始化位姿推断器。然后做个信息的打印，告诉用户目前还没有进行位姿初始化，本人打印类似如下：

[ INFO] [1669275089.560128501, 1606808654.733059071]: I1124 15:31:29.000000 173872 local_trajectory_builder_2d.cc:168] Extrapolator not yet initialized.

然后执行如下代码：

  CHECK(!synchronized_data.ranges.empty());
  // TODO(gaschler): Check if this can strictly be 0.
  CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);
  // 计算第一个点的时间
  const common::Time time_first_point =
      time +
      common::FromSeconds(synchronized_data.ranges.front().point_time.time);

保证点云数据不为空，且最后一个点云的数据，时间为0。且用 synchronized_data.time 时间戳，加上第一个点云相对于该时间戳的时间 synchronized_data.ranges.front().point_time.time 得到第一个点云的时间戳。

  // 只有在extrapolator_初始化时, GetLastPoseTime()是common::Time::min()
  if (time_first_point < extrapolator_->GetLastPoseTime()) {
    LOG(INFO) << "Extrapolator is still initializing.";
    return nullptr;
  }

如果 第一个点云数据的时间，低于位置估计器最新的位姿时间，说明器还没有完成初始化，但是已经再初始化之中了，因为 extrapolator_->GetLastPoseTime() 没有初始化，则其返回是一个很小的时间戳，判断条件不成立，当然也不会打印。进一步会执行如下代码：

  // 预测得到每一个时间点的位姿
  for (const auto& range : synchronized_data.ranges) {
    common::Time time_point = time + common::FromSeconds(range.point_time.time);
    // 如果该时间比上次预测位姿的时间还要早,说明这个点的时间戳往回走了, 就报错
    if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
      // 一个循环只报一次错
      if (!warned) {
        LOG(ERROR)
            << "Timestamp of individual range data point jumps backwards from "
            << extrapolator_->GetLastExtrapolatedTime() << " to " << time_point;
        warned = true;
      }
      time_point = extrapolator_->GetLastExtrapolatedTime();
    }
    
    // Step: 2 预测出 每个点的时间戳时刻, tracking frame 在 local slam 坐标系下的位姿
    range_data_poses.push_back(
        extrapolator_->ExtrapolatePose(time_point).cast<float>());
  }

该部分的代码暂且简单理解一下，就是 extrapolator_ 会预测出每个点云的时间戳时刻，其 tracking frame 在 local slam(后面称为local) 坐标系下的位姿。这些都后续再进行详细讲解。继续往下看代码：

  if (num_accumulated_ == 0) {//该变量构造时，声明为0
    // 'accumulated_range_data_.origin' is uninitialized until the last
    // accumulation.
    //对accumulated_range_data_进行一个空数据的赋值
    accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
  }

sensor::RangeData 的定义如下：

/**
 * @brief local_slam_data中存储所有雷达点云的数据结构
 * 
 * @param origin  点云的原点在local坐标系下的坐标
 * @param returns 所有雷达数据点在local坐标系下的坐标, 记为returns, 也就是hit
 * @param misses  是在光线方向上未检测到返回的点(nan, inf等等)或超过最大配置距离的点
 */
struct RangeData {
  Eigen::Vector3f origin;
  PointCloud returns;
  PointCloud misses; // local坐标系下的坐标
};

 

三、激光雷达数据运动畸变矫正原理讲解

接着下面时比较总要的一块代码。那就是对雷达数据因为运动产生的畸变进行矫正，其畸变产生原因以及常用矫正手段可以参考如下博客：
史上最简SLAM零基础解读(11) - 雷达数据运动畸变与矫正

1、形成原因

这里简单的提及一下激光雷达数据运动畸变的原：其主要因为一帧雷达数据的所有点云并非同一时刻产生的，如下图三图 a、b、c。(备注:这里不讨论由雷达自身旋转造成的畸变，该种畸变结合激光雷达的角度辨率即可解决)

图1

$(1)$ 先来看 a图，假设在某个时刻，激光雷达测得三个点云数据分别为 $P_1$、$P_2$、$P_3$。可以从图像看出，$P_2$ 应该是距离机器人最近的点云。

$(2)$ 但是实际情况是很难获得这样的点云，除非激光雷达传感器即不旋转也不移动。通常情况下如 b图 所示，机器人的运动是无规则的，同一帧数据时间内的的3个时刻，$T_1^1$ 表示 $T^1$ 时刻测得点云 $P_1$， $T_2^2$ 表示 $T^2$ 时刻测得点云 $P_2$，$\cdots \cdots$。

$(3)$ 从图 b 可以很明显得看到，由于机器人的运动是无规则的，姿态与位置都在发生改变，其测的的点云都是畸变的。如果把其当作正常的点云的使用则会类似 c图 的效果。

$(4)$ 图c 可以看出，其把所有的点云，都认为是在 $T^1$ 时刻产生：
①首先 $P_1$ 是没有畸变，是正确的；
②对于$P_2$，来说，测得得距离变短了，然后方向也错了
②对于$P_3$，与$P_2$类似，只是距离更短，方向更偏

2、解决方式

解决距离上的畸变方式有很多种，这里说一下源码是如何实现的。从图二可以看出，只要计算出 $P_2^{\prime }$、$P_3^{\prime }$ 与 $P_2^{\prime }$、$P_3^{\prime }$ 距离差，即可对他们进行矫正。那么如何才能知道这个距离呢?

如果我们知道机器人分别在 $T^1$ 、$T^2$、$T^3$ 时刻下的位姿， 那么可以计算出 $T^2$ 与 $T^3$ 相对于 $T^1$ 的位移，那么就是对 $P_2^{\prime }$、$P_3^{\prime }$ 与 $P_2^{\prime }$ 进行修正呢。不过要注意，不能简单的加法，因为移动过程中，并非所有位移量都造成了距离上的畸变，如下图所示

根据几何关系是很容易求解的。但是通常会通过矩阵的方式进行求解，源码中也是使用这种方式。因为后续追踪都是基于局部地图 local 坐标系，所以其直接把 $T^2$ 时刻测得得点云数据 $P_2^{\prime }$ 或者 $T^3$ 时刻测得得点云数据 $P_3^{\prime }$) 结合机器人在 $T^x$ 时刻的位姿( local 坐标系)，把点云数据变换到 local 坐标系下。原理还是一样的，就是通过 $T^x$ 时刻的位姿对该时刻测得得点云 $P_x^{\prime }$ 进行矫正，矫正之后点云是相对于 local 坐标系。
 

四、激光雷达数据运动畸变矫正源码分析

下面来看看 Cartographer 中是如何处理的，接着前面的分析，可以首先可以看到如下代码，其进入到一个循环，对所有点云数据进行遍历：

  // 对每个数据点进行处理
  for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {

1、点云数据在机器人坐标系的坐标

    // 获取在点云数据基于 tracking frame 坐标系的坐标
    const sensor::TimedRangefinderPoint& hit =
        synchronized_data.ranges[i].point_time;

获取在点云数据基于机器人(tracking frame) 坐标系的坐标，记为 $Poin{t}^{tracking}$
 
2、计算雷达原点在local坐标系下的位置

    //把雷达在机器人坐标系的位置,变换成雷达在local slam坐标系的位置
    const Eigen::Vector3f origin_in_local =
        range_data_poses[i] *
        synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);

这里的 synchronized_data.origins 存储的是雷达原点在机器人坐标系下的位置，从 SensorBridge::HandleRangefinder 函数中的的如下代码可以看出：

sensor_to_tracking->translation().cast<float>(),

雷达原点在机器人坐标系下的位置记为 $LaserRada{r}_{origin}^{tracking}$，range_data_poses 与前面一样，表示机器人在 local 坐标系下的位姿，记为 ${\mathbf{R}\mathbf{o}\mathbf{b}\mathbf{o}\mathbf{t}}_{tracking}^{local}$，最终获得的结果 origin_in_local 表示激光雷达原点在local坐标系下的位置，记为 $LaserRada{r}_{origin}^{local}$，则计算公式如下

$$LaserRada{r}_{origin}^{local}={\mathbf{R}\mathbf{o}\mathbf{b}\mathbf{o}\mathbf{t}}_{tracking}^{local}\ast LaserRada{r}_{origin}^{tracking}\text{\hspace{1em}(01)}$$

3、激光雷达运动畸变校正

    // Step: 3 运动畸变的去除, 将相对于tracking_frame的hit坐标 转成 local坐标系下的坐标
    // 把点云数据变换到 local 坐标系下
    sensor::RangefinderPoint hit_in_local =
        range_data_poses[i] * sensor::ToRangefinderPoint(hit);

其目的是获得点云数在 local 坐标系下的坐标，记 hit_in_local 为 $Poin{t}^{local}$。range_data_poses 表示机器人在 local 坐标系下的位姿，记为 ${\mathbf{R}\mathbf{o}\mathbf{b}\mathbf{o}\mathbf{t}}_{tracking}^{local}$，sensor::ToRangefinderPoint(hit) 表示点云在机器人坐标系下的位置，记为$Poin{t}^{tracking}$，那么计算公式如下：
$$Poin{t}^{local}={\mathbf{R}\mathbf{o}\mathbf{b}\mathbf{o}\mathbf{t}}_{tracking}^{local}\ast Poin{t}^{tracking}\text{\hspace{1em}(02)}$$
4、计算去畸变点云与激光雷达原点距离

    // 计算这个点的距离, 这里用的是去畸变之后的点的距离
    const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
    const float range = delta.norm();

hit_in_local.position 表示点云在local坐标系下的坐标系，与前面一样，记为 $Poin{t}^{local}$，origin_in_local 表示激光雷达原点相对于 local 坐标系的位置，记为 $LaserRada{r}_{origin}^{local}$，可知他们都位于 local 坐标系下，即可做差运算得到 delta，也就是点云先对于雷达原点的坐标，记为 $Poin{t}^{origin}$,那么计算公式如下：
$$Poin{t}^{origin}=Poin{t}^{local}-LaserRada{r}_{origin}^{local}\text{\hspace{1em}(03)}$$这里的$Poin{t}^{origin}$也可以看作是从激光雷达原点指向点云数据的向量，对其求范数 .norm() 也就是求该向量的长度等价于激光雷达原点到点云的直线距离。
 
4、根据参数对雷达数据进行过滤
在 src/cartographer/configuration_files/trajectory_builder_2d.lua 文件中可以看到如下参数：

  min_range = 0.,                 -- 雷达数据的最近滤波, 保存中间值
  max_range = 30.,                -- 雷达数据的最远滤波, 保存中间值

对于于源码中的 options_.min_range() 与 options_.max_range()，代码如下：

    // param: min_range max_range
    if (range >= options_.min_range()) {//如果该点云大于等于最近测量距离
      if (range <= options_.max_range()) {//如果该点小于等于最远测量距离
        // 在这里可以看到, returns里保存的是local slam下的去畸变之后的点的坐标
        accumulated_range_data_.returns.push_back(hit_in_local);
      } else {
        // Step: 4 超过max_range时的处理: 用一个距离进行替代, 并放入misses里
        hit_in_local.position =
            origin_in_local +
            // param: missing_data_ray_length, 是个比例, 不是距离
            options_.missing_data_ray_length() / range * delta;
        accumulated_range_data_.misses.push_back(hit_in_local);
      }
    }

如果点云大于等于最近测量距离、且小于等于最远测量距离，那么则把该点云添加到 accumulated_range_data_.returns 变量中，如果该点云大于最远测量距离，则用如下代码代替：

        hit_in_local.position =
            origin_in_local +
            // param: missing_data_ray_length, 是个比例, 不是距离
            options_.missing_data_ray_length() / range * delta;

origin_in_local 表示激光雷达原点在 local 坐标系的位置，然后加上一个缩小后的delta(激光雷达原点到点云的直线距离)。缩小比例由 src/cartographer/configuration_files/trajectory_builder_2d.lua 的 missing_data_ray_length 参数控制，该值约小，则缩 hit_in_local.position 的值越小，但是始终大于 origin_in_local，因为雷达数据不应该为负数。然后存储在 accumulated_range_data_.misses 变量之中。
 

五、结语

  // 对每个数据点进行处理
  for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {

通过上面的循环之后，就对所有的点云数据都进行了校正，值得注意的是，校正之后的点云数据都存储于 accumulated_range_data_ 变量之中，且所有的点云都是相对于 local 坐标系的。这样，该篇博客对于激光雷达运动畸变校正就讲解完成了。其中跳过了如下代码：

    // Step: 2 预测出 每个点的时间戳时刻, tracking frame 在 local slam 坐标系下的位姿
    range_data_poses.push_back(
        extrapolator_->ExtrapolatePose(time_point).cast<float>());

如何获取 range_data_poses，即机器人在local 坐标系下的位姿，还没有进行详细的讲解，暂且放一放。