Cartographer之前后端入口GlobalTrajectoryBuilder

这部分的功能是将前端的子图、位姿信息给后端进行闭环检测和全局优化。

下面解析代码,只分析2D情况下的LocalTrajectory2D和PoseGraph2D类。

1.构造函数 GlobalTrajectoryBuilder

/** 
 * @brief GlobalTrajectoryBuilder的构造
 * @param local_trajectory_builder 记录了轨迹跟踪器，是前端的核心类型
 * @param trajectory_id 轨迹索引
 * @param pose_graph 稀疏位姿图
 * @param local_slam_result_callback 记录了一个std::function类型的回调函数
 */
  GlobalTrajectoryBuilder(
      std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder,
      const int trajectory_id, PoseGraph* const pose_graph,
      const LocalSlamResultCallback& local_slam_result_callback)
      : trajectory_id_(trajectory_id),
        pose_graph_(pose_graph),
        local_trajectory_builder_(std::move(local_trajectory_builder)),
        local_slam_result_callback_(local_slam_result_callback) {}
  ~GlobalTrajectoryBuilder() override {}

2.添加IMU数据接口  AddSensorData(const std::string& sensor_id, const sensor::ImuData& imu_data)

新添加的IMU数据会同时进local和global进行位姿推测

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

3.添加ODOM数据接口 AddSensorData(const std::string& sensor_id, const sensor::OdometryData& odometry_data)

新添加的IMU数据会同时进local和global进行位姿推测

 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);
    }
    // TODO(MichaelGrupp): Instead of having an optional filter on this level,
    // odometry could be marginalized between nodes in the pose graph.
// Related issue: cartographer-project/cartographer/#1768
//这里通过motion filter去降采样
    if (pose_graph_odometry_motion_filter_.has_value() &&
        pose_graph_odometry_motion_filter_.value().IsSimilar(
          odometry_data.time, odometry_data.pose))
    {
      return;
    }
    pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
  }

4.添加绝对位置数据接口 AddSensorData(const std::string& sensor_id, const sensor::FixedFramePoseData& fixed_frame_pose)

这里添加的数据应该是GPS之类转换出来的位置，这里只用于后端的优化

void AddSensorData(
    const std::string& sensor_id, const sensor::FixedFramePoseData& fixed_frame_pose) override
  {
    if (fixed_frame_pose.pose.has_value())
    {
      CHECK(fixed_frame_pose.pose.value().IsValid())
        << fixed_frame_pose.pose.value();
    }
    pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
  }

5.添加landmark数据接口 AddSensorData(const std::string& sensor_id, const sensor::LandmarkData& landmark_data)

一开始的时候我以为landmark是当绝对位置来做的，后来发现landmark其实是当特征点做的，在建图的时候就输入landmark的数据，将其做为特征点来构建约束，这也就意味着，图建完后，如果新加landmark，需要在建图的二进制流中也添加该landmark，否则会出现定位被landmark带飞的情况。以及在使用landmark时，对landmark的置信度，需要根据使用场景来进行配置。同绝对位置输入接口，landmark也是在后端中使用的。

  void AddSensorData(const std::string& sensor_id, const sensor::LandmarkData& landmark_data) override
  {
    pose_graph_->AddLandmarkData(trajectory_id_, landmark_data);
  }

6.将前端数据传给数据接口 AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data)

AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData>
                              local_slam_result_data) override
  {
    CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with "
                                         "local_trajectory_builder_ present.";
    local_slam_result_data->AddToPoseGraph(trajectory_id_, pose_graph_);
  }

7.核心部分添加点云数据接口AddSensorData(

    const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data)

这里涉及到前后端点云数据的输入，先是如果有前端的话就把点云数据给前端，后端先添加node，接着在回调函数中去做优化。

 void AddSensorData(
    const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) override
  {
    CHECK(local_trajectory_builder_)
      << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";
    std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
    matching_result = local_trajectory_builder_->AddRangeData(
      sensor_id, timed_point_cloud_data);   // 这里是前端
    if (matching_result == nullptr)
    {
      // The range data has not been fully accumulated yet.
      return;
    }
    kLocalSlamMatchingResults->Increment();
    std::unique_ptr<InsertionResult> insertion_result;
    if (matching_result->insertion_result != nullptr)
    {
      kLocalSlamInsertionResults->Increment();
// 这里后端
      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));
    }
  }