cartographer_learn_12(点云匹配前的数据处理)

写在前面

作者之前急于快速的将代码阅读完，没有仔细的考虑其中的原理，导致出现了一些错误。为了避免误导读者，作者回去仔细学习其中的原理，并把之前写的第12篇删除。之前我们讨论到了位姿推断器及把多个LaserScan的数据打包一起分发出去。这次的文章会比较长。为了能更好的讨论，我们需要先去看看odometer数据和imu数据被传进前端前做了什么处理，再去讨论怎么把点云旋转到与重力垂直的方向，最后提一下体素滤波的事情。

odometer数据和imu数据处理

odometer数据

先来看odometer数据这里就不把代码一点点的贴上来带大家一起看了，因为它大致的过程和LaserScan数据一样。作者在此贴一张图表示处理这个数据过程中的函数调用关系，读者可自行查看

我们看看最后一个函数

std::unique_ptr<carto::sensor::OdometryData> SensorBridge::ToOdometryData(
    const nav_msgs::Odometry::ConstPtr& msg) {
  const carto::common::Time time = FromRos(msg->header.stamp);
  // tf找到odometer坐标系和tracking坐标系的关系
  const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
      time, CheckNoLeadingSlash(msg->child_frame_id));
  if (sensor_to_tracking == nullptr) {
    return nullptr;
  }
  //将odometer坐标系在local（？）下的位姿转换成tracking坐标系在local下的位姿
  return absl::make_unique<carto::sensor::OdometryData>(
      carto::sensor::OdometryData{
          time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()});
}

用一个公式表示上面函数就是，至于是不是Local坐标系下还需要后面看代码进一步确认。

imu数据

同样贴一张图表示处理这个数据过程中的函数调用关系

同样来看看最后一个函数

// 进行数据类型转换与坐标系的转换
std::unique_ptr<carto::sensor::ImuData> SensorBridge::ToImuData(
    const sensor_msgs::Imu::ConstPtr& msg) {
  ......// 做一些检查
  //tf获取imu坐标系和tracking坐标关系
  const carto::common::Time time = FromRos(msg->header.stamp);
  const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
      time, CheckNoLeadingSlash(msg->header.frame_id));
  if (sensor_to_tracking == nullptr) {
    return nullptr;
  }
  ......// 又做一些检查
  // 将加速度与角速度转换到tracking坐标系下
  return absl::make_unique<carto::sensor::ImuData>(carto::sensor::ImuData{
      time, sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration),
      sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)});
}

总结一下，结合之前看过的LaserScan数据的处理，可以说传入前端的imu数据是tracking坐标系的角速度及加速度，odometer数据是time这个时刻tracking坐标系的位姿，LaseScan是每次测量的点在tracking坐标系下的坐标。

点云的处理（匹配前含位姿推断器）

这个要从把多个LaserScan的数据按时间打包分发出去之后说起。即LocalTrajectoryBuilder2D::AddRangeData这个方法，为了更好的介绍，我们先整体上说说这个方法具体实现了什么。在说具体实现什么时，作者会提出自己在看源码时的一些疑问，并且后面会回答这些疑问（注意这里仅仅讨论匹配前的）
这个函数实现的功能：
1：依据分发出来的点云中每个点的时间（time），使用位姿推断器推断time时刻tracking坐标系的位姿。
2：依据1中推断出来的位姿将点云中的点转换到local坐标系下。此时此刻点云中的点到其对应的原点距离，距离超过最大值（传感器最大值），要做特别的处理（作者这里不是很理解）。问题（1）匹配时点云不应该在tracking坐标系下吗？在local坐标系下通过匹配得到tracking坐标系位姿？（2）可以看到点云中每个点对应的时间都不一样，那即使匹配得到了一个位姿，这个位姿代表哪个时刻的位姿呢？
3：将2得到的点云旋转（将详细的给出公式，以此回答问题（1）（2））并剔除，做体素滤波
4：将3得到的点云做自适应体素滤波，并做匹配。
5：根据匹配结果将点云写入submap
6：把匹配得到的位姿加入位姿推断器
为什么要先把它的功能都罗列出来呢？注意看6和1，1中使用位姿推断器推断出time时刻tracking坐标系的位姿推断的起点就是上一次匹配结果的tracking坐标系的pose，即6。这点对于源码阅读理解很重要，作者就是明白这一点后对源码的理解推进了一大段。好了接下来一点点的讨论。

使用位姿推断器推断位姿

我们现在源码找到这一部分：

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& unsynchronized_data) {
......//多个传感器数据打包
......//位姿推断器初始化
......//一些检查
//功能1的结果的存储
  std::vector<transform::Rigid3f> range_data_poses;
  range_data_poses.reserve(synchronized_data.ranges.size());
  bool warned = false;

  // 上诉功能1
  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();
    }
    range_data_poses.push_back(
        extrapolator_->ExtrapolatePose(time_point).cast<float>());
  }
  ......//一些其他的操作
}

代码先是为结果申请空间，然后遍历点云所有的点，计算它们对应的时间，并使用位姿推断器推断该使时间tracking坐标系的位姿。看到上面那段代码的操作没有什么特别的，除了最后一句的操作，调用了PoseExtrapolator::ExtrapolatePose。接下来我们来看看这个具体的方法。
看代码：

transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {
  const TimedPose& newest_timed_pose = timed_pose_queue_.back();
  CHECK_GE(time, newest_timed_pose.time);
  //检查记录的timepose是不是对应时间的，不是要重新计算，是直接返回
  if (cached_extrapolated_pose_.time != time) {
    // 预测的tracking坐标time时刻的位置
    const Eigen::Vector3d translation =
        ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
    // 预测的tracking坐标time时刻的姿态
    const Eigen::Quaterniond rotation =
        newest_timed_pose.pose.rotation() *
        ExtrapolateRotation(time, extrapolation_imu_tracker_.get());
    cached_extrapolated_pose_ =
        TimedPose{time, transform::Rigid3d{translation, rotation}};
  }
  return cached_extrapolated_pose_.pose;
}

可见位置和姿态是分开计算的，分别调用了PoseExtrapolator::ExtrapolateTranslation和PoseExtrapolator::ExtrapolateRotation（注意它使用了一个ImuTracker对象，不知道大家是否还记得第11篇说过imu_tracker_在每次加入新的pose（AddPose）后收会深度拷贝两份，它就是其中一份）。

位置推算

先来看ExtrapolateTranslation

Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time) {
  const TimedPose& newest_timed_pose = timed_pose_queue_.back();
  const double extrapolation_delta =
      common::ToSeconds(time - newest_timed_pose.time);
//具体计算位置的增量
  if (odometry_data_.size() < 2) {
    return extrapolation_delta * linear_velocity_from_poses_;
  }
  return extrapolation_delta * linear_velocity_from_odometry_;
}

可以想到odometry_data_.size() < 2是一开始的少数情况（可以查看PoseExtrapolator::TrimOdometryData），大多数时候都是odometry_data_.size() > 2。所以可以认为速度增量的计算是先用里程计算出线速度，再乘时间。那紧接着就像知道linear_velocity_from_odometry_怎么计算的？看下面这段代码：

oid PoseExtrapolator::AddOdometryData(
    const sensor::OdometryData& odometry_data) {
  CHECK(timed_pose_queue_.empty() ||
        odometry_data.time >= timed_pose_queue_.back().time);
  odometry_data_.push_back(odometry_data);
  TrimOdometryData();
  // 太少没法算
  if (odometry_data_.size() < 2) {
    return;
  }
  // 取最新与最老的两个里程计数据
  const sensor::OdometryData& odometry_data_oldest = odometry_data_.front();
  const sensor::OdometryData& odometry_data_newest = odometry_data_.back();
  // 数据间的时间差
  const double odometry_time_delta =
      common::ToSeconds(odometry_data_oldest.time - odometry_data_newest.time);
  // 见下面公式1
  const transform::Rigid3d odometry_pose_delta =
      odometry_data_newest.pose.inverse() * odometry_data_oldest.pose;

  // 角速度
  angular_velocity_from_odometry_ =
      transform::RotationQuaternionToAngleAxisVector(
          odometry_pose_delta.rotation()) /
      odometry_time_delta;
  if (timed_pose_queue_.empty()) {
    return;
  }
  // 线速度计算，此时的线速度再tracking坐标系下
  const Eigen::Vector3d
      linear_velocity_in_tracking_frame_at_newest_odometry_time =
          odometry_pose_delta.translation() / odometry_time_delta;
  //计算tracking坐标系此时的姿态
  const Eigen::Quaterniond orientation_at_newest_odometry_time =
      timed_pose_queue_.back().pose.rotation() *
      ExtrapolateRotation(odometry_data_newest.time,
                          odometry_imu_tracker_.get());
  //将线速度转换到local坐标系下
  linear_velocity_from_odometry_ =
      orientation_at_newest_odometry_time *
      linear_velocity_in_tracking_frame_at_newest_odometry_time;
}

公式1：

关键是计算tracking坐标系此时此刻位姿调用的那个函数PoseExtrapolator::ExtrapolateRotation（注意它和上面说的那个参数不一样，imu_tracker_拷贝了两份，一份上面说了，这里的第二个参数就是第二份）。所以我们再下一节推算位姿时再具体讨论这个函数。不过我们可以猜测一下，它就的只是位姿的增量，并不是位姿本身，因为该函数返回的结果要乘一个pose的rotation后才是tracking坐标系的姿态。

姿态增量的计算

PoseExtrapolator::ExtrapolateRotation，上代码：

Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
    const common::Time time, ImuTracker* const imu_tracker) const {
  CHECK_GE(time, imu_tracker->time());
  // 更新imu_tracker的状态到time时刻
  AdvanceImuTracker(time, imu_tracker);
  // 通过 imu_tracker_ 获取最初的状态，注意是imu_tracker_不是imu_tracker
  const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
  // 求姿态增量
  return last_orientation.inverse() * imu_tracker->orientation();
}

可以看到第二个参数传入一个ImuTracker的指针，将该指针指向的ImuTracker的状态更新到time这一时刻。至于第二个参数具体是谁上面有提过了。由于第二个指针指向的对象其实是imu_tracker_的深度拷贝，所以初始状态是imu_tracker_。

将点云转换到local坐标系下

由于上面已经计算出了每个点对应时间的tracking坐标系的位姿，所以这个理论就很简单了，至于作者目前不能理解的部分后面相信有机会解释。所以这里只是把代码贴出来看看，还请读者记住作者提出的两个问题

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& unsynchronized_data) {
......//多个传感器数据打包
......//位姿推断器初始化
......//一些检查
......//功能1
//注意功能2结果存储的地方
if (num_accumulated_ == 0) {
    // 'accumulated_range_data_.origin' is uninitialized until the last
    // accumulation.
    accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
  }
//遍历点云所有点
  for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
    const sensor::TimedRangefinderPoint& hit =
        synchronized_data.ranges[i].point_time;
    //该点的原点，即传感器位置
    const Eigen::Vector3f origin_in_local =
        range_data_poses[i] *
        synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
    //点的再local中的位置
    sensor::RangefinderPoint hit_in_local =
        range_data_poses[i] * sensor::ToRangefinderPoint(hit);
   //点到原点的距离
    const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;

    const float range = delta.norm();
    if (range >= options_.min_range()) {
      if (range <= options_.max_range()) {
        accumulated_range_data_.returns.push_back(hit_in_local);
      } else {
        hit_in_local.position =
            origin_in_local +
            options_.missing_data_ray_length() / range * delta;
        accumulated_range_data_.misses.push_back(hit_in_local);
      }
    }
  }
  ++num_accumulated_;
  ......//一些其他的操作
}

点云的旋转

在本节将顺带解决作者提出的两个问题，先上代码，看看顶层的调用：

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& unsynchronized_data) {
......//多个传感器数据打包
......//位姿推断器初始化
......//一些检查
......//功能1
......//功能2
//看到并不是每次分发LaserScan的数据后都做匹配，而是等待一定次数后才做匹配
if (num_accumulated_ >= options_.num_accumulated_range_data()) {
    const common::Time current_sensor_time = synchronized_data.time;
    absl::optional<common::Duration> sensor_duration;
    if (last_sensor_time_.has_value()) {
      sensor_duration = current_sensor_time - last_sensor_time_.value();
    }
    last_sensor_time_ = current_sensor_time;
    num_accumulated_ = 0;
    //预测最后一个点的时间对应tracking坐标系的姿态，把它转换成位姿的形式，公式2
    const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
        extrapolator_->EstimateGravityOrientation(time));
    // TODO(gaschler): This assumes that 'range_data_poses.back()' is at time
    // 'time'.
    //最后一个点的时间对应tracking坐标系的位置
    accumulated_range_data_.origin = range_data_poses.back().translation();
    //对点云做处理，匹配，将点云加入地图和功能6
    return AddAccumulatedRangeData(
        time,
        // 对点云做处理，体素滤波
        TransformToGravityAlignedFrameAndFilter(
            gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
            accumulated_range_data_),
        gravity_alignment, sensor_duration);
  }

  return nullptr;
}

本节重点有两部第一是：最后一个点的时间对应tracking坐标系的姿态的预测，第二是：对点云做处理，体素滤波。即PoseExtrapolator::EstimateGravityOrientation和LocalTrajectoryBuilder2DTransformToGravityAlignedFrameAndFilter这两个函数

tracking坐标系的姿态的预测

我们进去看看这个函数：

Eigen::Quaterniond PoseExtrapolator::EstimateGravityOrientation(
    const common::Time time) {
  ImuTracker imu_tracker = *imu_tracker_;
  // 将imu_tracker_更新到time时刻
  AdvanceImuTracker(time, &imu_tracker);
  return imu_tracker.orientation();
}

我们上一节讨论姿态增量的计算时，imu_tracker_时它只是作为初始状态提供者出现，这里终于要更新imu_tracker_自己了。我们来看看对于一个ImuTracker来说时怎么更新自己状态的，即PoseExtrapolator::AdvanceImuTracker，看代码：

void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
                                         ImuTracker* const imu_tracker) const {
  CHECK_GE(time, imu_tracker->time());
  //没有数据，或者要求更新的时间太早了
  if (imu_data_.empty() || time < imu_data_.front().time) {
    imu_tracker->Advance(time);
    // 没有数据就认为线加速度（即重力反方向）竖直向上
    imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
    //更新角速度
    imu_tracker->AddImuAngularVelocityObservation(
        odometry_data_.size() < 2 ? angular_velocity_from_poses_
                                  : angular_velocity_from_odometry_);
    return;
  }
  // imu_tracker的时间比imu数据队列中第一个数据的时间早, 就先预测到imu数据队列中第一个数据的时间
  if (imu_tracker->time() < imu_data_.front().time) {
    // Advance to the beginning of 'imu_data_'.
    imu_tracker->Advance(imu_data_.front().time);
  }
  auto it = std::lower_bound(
      imu_data_.begin(), imu_data_.end(), imu_tracker->time(),
      [](const sensor::ImuData& imu_data, const common::Time& time) {
        return imu_data.time < time;
      });

  //依据imu数据，按照预测，矫正，更新角速度一步步来
  while (it != imu_data_.end() && it->time < time) {
    imu_tracker->Advance(it->time);
    imu_tracker->AddImuLinearAccelerationObservation(it->linear_acceleration);
    imu_tracker->AddImuAngularVelocityObservation(it->angular_velocity);
    ++it;
  }
  // 最后预测一下time时刻的状态
  imu_tracker->Advance(time);
}

上面出现的几个函数，第11篇都有讨论过，这里就略过。
我们来看看这一步的到的时啥，公式2：

对点云做处理及体素滤波

在看代码前，我们先来看看这个函数的第一个参数，可以看到第一个参数可以理解（实际上时四元素加平移向量）一个旋转平移矩阵，我们用T来表示，即公式3

作者认为不要试图理解这两个矩阵计算的结果，因为它只是一个中间量，没有很明确的物理意义，要把它分成两个矩阵去理解。
好我们在进去看看LocalTrajectoryBuilder2DTransformToGravityAlignedFrameAndFilter这个函数：

sensor::RangeData
LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
    const transform::Rigid3f& transform_to_gravity_aligned_frame,
    const sensor::RangeData& range_data) const {
//先将点云中的每个点做转换，转换矩阵为公式3，再按照z轴坐标剔除一些点
  const sensor::RangeData cropped =
      sensor::CropRangeData(sensor::TransformRangeData(
                                range_data, transform_to_gravity_aligned_frame),
                            options_.min_z(), options_.max_z());
//体素滤波
  return sensor::RangeData{
      cropped.origin,
      sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()),
      sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())};
}

为了让读者更加清晰，我们看看sensor::TransformRangeData这个函数：

RangeData TransformRangeData(const RangeData& range_data,
                             const transform::Rigid3f& transform) {
 //TransformPointCloud这个函数就是做一个简单的矩阵乘点的乘法，把点做个转换，这里就不贴出来了
  return RangeData{
      transform * range_data.origin,
      TransformPointCloud(range_data.returns, transform),
      TransformPointCloud(range_data.misses, transform),
  };
}

回到作者之前提到的两个问题：问题（1）匹配时点云不应该在tracking坐标系下吗？在local坐标系下通过匹配得到tracking坐标系位姿？（2）可以看到点云中每个点对应的时间都不一样，那即使匹配得到了一个位姿，这个位姿代表哪个时刻的位姿呢？。知道这个函数之前，我们的点云中的点都是再Local坐标系中的点，可以表示为公式4：

我们先考虑点云最后一个点，因为公式3中的第二举证就是这个点的时间对应的tracking坐标系位姿的逆。如果用公式3乘以最后一个点对应的公式4，则得以下结果：

我们来理解一下这个结果这是将tracking坐标系下得点云做了个旋转，怎么样得旋转呢？旋转到使得坐标系与local坐标系方向一致，注意这里并不是把点转换到local坐标系中，还在tracking坐标系中。为什么要做这种旋转，作者得理解是，因为这是再做二维得slam，但是传感器难免会得到做slam这个平面以外得点，这个时候我们就要剔除这些点。要怎么剔除呢？要将点做旋转，然后再按z轴得坐标去做剔除。这就回答了问题1。
我们再来考虑最后一个点除外的其他点。要是用公式3乘以其他点对应的公式4会得到一下结果：

我们先假设这个其他点对应的时间为t1，最后一个点对应的时间为t2。这两个时间对应的tracking坐标系分别记为T1，T2。从后往前倒着看这个公式，第一步先将T1坐标系中的点转换到坐标系local中，第二部将local中的点转换到T2中，后面的就和上面讨论的最后一个点的情况一致了。这就解决了第二个问题。因为这里都是将点转换到最后一个点的时间对应的tracking坐标系T2中再做其他处理，想必第二个问题的答案是：匹配得到的位姿对应的时间是点云最后一个点的时间。ok，这个点云的转换就到这。
至于sensor::CropRangeData这个函数比较简单，就是去除掉z轴坐标不合格的。鉴于篇幅就不讨论了。
最后简单的说一下体素滤波，体素滤波有点像视觉slam中的图像金子塔一下，可以实现图像的降采样，它实现的是点云的降采样。这里推荐一个blog：https://blog.csdn.net/wolfcsharp/article/details/93198320，但是cartographer中的有点点不一样，它保留的是某个体素中的随机点。
好了可能要比较长的一段时间后再更新了，因为好多理论要学。。。。。。ceres，匹配。。。。。。