Cartographer SLAM——submap的建立过程（一）

1. LocalTrajectoryBuilder3D::AddRangeData（）

接口具体为：

std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
LocalTrajectoryBuilder3D::AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& unsynchronized_data)
    {
    ...
    }

输入：激光雷达的传感器ID，未时间同步的激光点云原始数据；
输出：submap座标系下的匹配定位结果。
此处为local（局部座标系）下对激光原始数据进行处理和前端匹配定位的入口。主要分为以下6个部分内容：

1）激光点云原始数据的时间同步

how ？待完善：

  auto synchronized_data =
      range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
  if (synchronized_data.ranges.empty()) {
    LOG(INFO) << "Range data collator filling buffer.";
    return nullptr;
  }

2) 检查是否已经经过了基于IMU的姿态初始化，主要通过检测当前帧开始时间和上一帧pos的时间判断

  const common::Time& time = synchronized_data.time;
  if (extrapolator_ == nullptr) {
    // Until we've initialized the extrapolator with our first IMU message, we
    // cannot compute the orientation of the rangefinder.
    LOG(INFO) << "IMU not yet initialized.";
    return nullptr;
  }

  CHECK(!synchronized_data.ranges.empty());
  CHECK_LE(synchronized_data.ranges.back().point_time[3], 0.f);
  const common::Time time_first_point =
      time +
      common::FromSeconds(synchronized_data.ranges.front().point_time[3]);
  if (time_first_point < extrapolator_->GetLastPoseTime()) {
    LOG(INFO) << "Extrapolator is still initializing.";
    return nullptr;
  }

3) 对时间同步后的数据进行网格滤波，尺寸大小为0.15/2 m

  std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> hits =
      sensor::VoxelFilter(0.5f * options_.voxel_filter_size())
          .Filter(synchronized_data.ranges);

4） 根据当前激光laserscan的所有点云时间标签，查找对应的机器人pos

此处得到每个点云的pos是为了补偿在激光扫描360度（20Hz的话一圈就是50ms）的过程中机器人会产生运动，这会对激光点云产生影响，基于此pos可以消除扫描过程中运动对激光扫描的影响。

std::vector<transform::Rigid3f> hits_poses;
  hits_poses.reserve(hits.size());
  bool warned = false;
  for (const auto& hit : hits) {
    common::Time time_point = time + common::FromSeconds(hit.point_time[3]);
    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();
    }
    hits_poses.push_back(
        extrapolator_->ExtrapolatePose(time_point).cast<float>());
  }

5） 计算local系下的激光点云座标，并只保留（1，80）m间的点云数据，其他数据设为missing

点云距离的计算根据local系下的当前点云座标与origin点云座标求模得到。

if (num_accumulated_ == 0) {
    // 'accumulated_range_data_.origin' is not used.
    accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
  }

  for (size_t i = 0; i < hits.size(); ++i) {
    const Eigen::Vector3f hit_in_local =
        hits_poses[i] * hits[i].point_time.head<3>();
    const Eigen::Vector3f origin_in_local =
        hits_poses[i] * synchronized_data.origins.at(hits[i].origin_index);
    const Eigen::Vector3f delta = hit_in_local - 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 {
        // We insert a ray cropped to 'max_range' as a miss for hits beyond the
        // maximum range. This way the free space up to the maximum range will
        // be updated.
        accumulated_range_data_.misses.push_back(
            origin_in_local + options_.max_range() / range * delta);
      }
    }
  }
  ++num_accumulated_;

6） 使用当前pos的位移作为激光点云的origin点座标，并对当前的激光点云数据进行网格滤波

这里的当前pos指的是时间为time时的机器人位姿，也就是激光雷达帧初始和IMU数据对齐时刻的位姿，后面扫描的点云时间以此为基准。基于第4）步得到的位姿可以将扫描过程中的运动引起的激光点云座标变化补偿掉。将所有的激光点云转换为初始origin点下无运动影响的local系下的点云座标accumulated_range_data_（但是还没有转换到激光时间对齐时刻time下的local系）。

transform::Rigid3f current_pose =
        extrapolator_->ExtrapolatePose(time).cast<float>();
    const sensor::RangeData filtered_range_data = {
        current_pose.translation(),
        sensor::VoxelFilter(options_.voxel_filter_size())
            .Filter(accumulated_range_data_.returns),
        sensor::VoxelFilter(options_.voxel_filter_size())
            .Filter(accumulated_range_data_.misses)};

7） 将上一步整理后的数据传递给LocalTrajectoryBuilder3D::AddAccumulatedRangeData进行基于submap的位姿匹配和优化。

传入的时候同时基于同步时刻time的pos位姿矩阵将激光点云的数据变换到Time时刻local座标系下。

    return AddAccumulatedRangeData(
        time, sensor::TransformRangeData(filtered_range_data,
                                         current_pose.inverse()));