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