cartographer代码阅读笔记

map_builder.h

是算法的入口，封装了local submap和 global pose-graph的优化

int MapBuilder:: AddTrajectoryBuiler() {
      std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_2d_options()) {
      local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>(
          trajectory_options.trajectory_builder_2d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    }

    DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
// 创建global trajectory的builder
    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,
        CreateGlobalTrajectoryBuilder2D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast<PoseGraph2D*>(pose_graph_.get()),
            local_slam_result_callback)));
}

// 通过这个去处理range data， imu的数据
  mapping:: TrajectoryBuilderInterface *GetTrajectoryBuilder(int trajectory_id) const override {
    return trajectory_builders_.at(trajectory_id).get();
  }

collated_trajectory_builder.h

// Collates sensor data using a sensor::CollatorInterface, then passes it on to
// a mapping::TrajectoryBuilderInterface
// 收集传感器数据，然后传给mapping::TracjectoryBuilderInterface

  CollatedTrajectoryBuilder(
      const proto::TrajectoryBuilderOptions& trajectory_options,
      sensor::CollatorInterface* sensor_collator, int trajectory_id,
      const std::set<SensorId>& expected_sensor_ids,
      std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder);

// 不同传感器的数据
  void AddSensorData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data) override {
    AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
  }
  void AddSensorData(const std::string& sensor_id,
                     const sensor::ImuData& imu_data) override {
    AddData(sensor::MakeDispatchable(sensor_id, imu_data));
  }
  void AddSensorData(const std::string& sensor_id,
                     const sensor::OdometryData& odometry_data) override {
    AddData(sensor::MakeDispatchable(sensor_id, odometry_data));
  }

// 最后都传到trajectory builder
    void CollatedTrajectoryBuilder::HandleCollatedSensorData(
        const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
      data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
    }

template <typename DataType>
class Dispatchable : public Data {
 public:
  Dispatchable(const std::string &sensor_id, const DataType &data)
      : Data(sensor_id), data_(data) {}

  common:: Time GetTime() const override { return data_.time; }
  // sensor:: Data 最后还是调用trajectory_builder:: AddSensorData
  void AddToTrajectoryBuilder(

      mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
    trajectory_builder->AddSensorData(sensor_id_, data_);
  }
  const DataType &data() const { return data_; }

 private:
  const DataType data_; 
}; 

global_trajectory_builder.cc

template <typename LocalTrajectoryBuilder, typename PoseGraph>
class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {

// 构造函数是一个local trajectory加上一个pose graph， 一个local slam result的callback函数
  GlobalTrajectoryBuilder(
      std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder,
      const int trajectory_id, PoseGraph* const pose_graph,
      const LocalSlamResultCallback& local_slam_result_callback)；

// 点云数据
    void AddSensorData(const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) override {
        std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
            matching_result = local_trajectory_builder_->AddRangeData(
                sensor_id, timed_point_cloud_data);
        if (matching_result->insertion_result != nullptr) {
            kLocalSlamInsertionResults->Increment();
    // pose graph add node
            auto node_id = pose_graph_->AddNode(
                matching_result->insertion_result->constant_data, trajectory_id_,
                matching_result->insertion_result->insertion_submaps);
            CHECK_EQ(node_id.trajectory_id, trajectory_id_);
            insertion_result = absl::make_unique<InsertionResult>(InsertionResult{
                node_id, matching_result->insertion_result->constant_data,
                std::vector<std::shared_ptr<const Submap>>(
                    matching_result->insertion_result->insertion_submaps.begin(),
                    matching_result->insertion_result->insertion_submaps.end())});
        }
        if (local_slam_result_callback_) {
            local_slam_result_callback_(
                trajectory_id_, matching_result->time, matching_result->local_pose,
                std::move(matching_result->range_data_in_local),
                std::move(insertion_result));
        }
    }
    

    void AddSensorData(const std::string& sensor_id,
                     const sensor::ImuData& imu_data) override {
        if (local_trajectory_builder_) {
        local_trajectory_builder_->AddImuData(imu_data);
        }
        pose_graph_->AddImuData(trajectory_id_, imu_data);
    }

    void AddSensorData(const std::string& sensor_id,
                     const sensor::OdometryData& odometry_data) override {
        CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
        if (local_trajectory_builder_) {
        local_trajectory_builder_->AddOdometryData(odometry_data);
        }
        pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
    }
}

local_trajectory_builder_2d.h

std::unique_ptr<LocalTrajectoryBuilder2D:: MatchingResult>
LocalTrajectoryBuilder2D:: AddRangeData(const std::string& sensor_id, const sensor::TimedPointCloudData& unsynchronized_data) {
    auto synchronized_data =
      range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);

    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;
        const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
            extrapolator_->EstimateGravityOrientation(time));
        // TODO(gaschler): This assumes that 'range_data_poses.back()' is at time
        // 'time'.
        accumulated_range_data_.origin = range_data_poses.back().translation();
        return AddAccumulatedRangeData(time,
            TransformToGravityAlignedFrameAndFilter(
                gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
            accumulated_range_data_),
            gravity_alignment, sensor_duration);
    }

}

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D:: AddAccumulatedRangeData(const common::Time time,
    const sensor::RangeData& gravity_aligned_range_data,
    const transform::Rigid3d& gravity_alignment,
    const absl::optional<common::Duration>& sensor_duration) {          std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
      ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);

    if (pose_estimate_2d == nullptr) {
        LOG(WARNING) << "Scan matching failed.";
        return nullptr;
    }

    std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
      time, range_data_in_local, filtered_gravity_aligned_point_cloud,
      pose_estimate, gravity_alignment.rotation());

    return absl::make_unique<MatchingResult>(
      MatchingResult{time, pose_estimate, std::move(range_data_in_local),
                     std::move(insertion_result)});
  }