cartographer给ros开放的接口

Posted 2021-08-27 Hill_LAI

通过查询Interface， cartograher给ros端留了3个接口：
1-cartographer::mapping::MapBuilderInterface
2-cartographer::mapping::PoseGraphInterface
3-cartographer::mapping::TrajectoryBuilderInterface
主要是2个，MapBuilderInterface，TrajectoryBuilderInterface，

我们看看MapBuilderInterface和TrajectoryBuilderInterface分别在ros端使用了哪些接口函数

MapBuilderInterface

1-加载地图

     map_builder_->LoadState(&stream, 
      ::cartographer::mapping::PoseGraphInterface::TrajectoryState::FROZEN);

2-构建一条slam地图路线

  const int trajectory_id = map_builder_->AddTrajectoryBuilder(
      expected_sensor_ids, trajectory_options.trajectory_builder_options,
      [this](const int trajectory_id, const ::cartographer::common::Time time,
             const Rigid3d local_pose,
             ::cartographer::sensor::RangeData range_data_in_local,
             const std::unique_ptr<
                 const ::cartographer::mapping::TrajectoryBuilderInterface::
                     InsertionResult>) {
        OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
      });

3-根据地图的id获取该slam路线类，说明TrajectoryBuilderInterface类也属于MapBuilderInterface

      map_builder_->GetTrajectoryBuilder(trajectory_id));

4-根据id结束构建该条slam路线

map_builder_->FinishTrajectory(trajectory_id);

5-通过pose_graph()来实现后端最终的位姿图解算（又叫做后端优化）

map_builder_->pose_graph()->RunFinalOptimization();

6-保存地图，英文翻译：根据id序列化该slam数据到文件中

  return map_builder_->SerializeStateToFileWithId(
      trajectory_id, include_unfinished_submaps, filename);

7-子图数据序列化，目的是快速传给前端进行显示

  const std::string error =
      map_builder_->SubmapToProto(submap_id, &response_proto);

8-获得所有slam路径的状态，状态有4种（活跃，完成，删除，冻结）

  auto trajectory_states = map_builder_->pose_graph()->GetTrajectoryStates();

9-获得子图的所有位姿

map_builder_->pose_graph()->GetAllSubmapPoses()

10-检测路径是否被冻结，冻结的意思是后端不再对该条路径进行优化（就是这条路径被固定住了）

map_builder_->pose_graph()->IsTrajectoryFrozen

11-获得最后一个子图（submao）局部位姿到全局（map）的转换矩阵（就是为了获得当前的全局位姿）

map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id)

12-获得路径上所有点（代码叫node）的位姿，所谓的位姿图优化（就是node和submap之间的优化）

map_builder_->pose_graph()->GetTrajectoryNodePoses();

13-获取所有位姿约束，就是node和submap之间的约束（就是相对位置关系（数学上叫转换矩阵（就是平移+旋转）））

map_builder_->pose_graph()->constraints();

14-获取路标位姿，路标是个好东西，就是用来定位的，比如你迷路时看到一个熟悉的建筑（路标），就能马上定位自己所在的方位和距离，根据路标来提高定位精度的前提是你识别路标和你之间的相对位置的精度。

map_builder_->pose_graph()->GetLandmarkPoses();

15-根据id删除路径

map_builder_->DeleteTrajectory(trajectory_id);
map_builder_->pose_graph()->DeleteTrajectory(trajectory_id);

再来看TrajectoryBuilderInterface

TrajectoryBuilderInterface通过重载实现了传输各种数据
1-传输odom

    trajectory_builder_->AddSensorData(
        sensor_id,
        carto::sensor::OdometryData{odometry_data->time, odometry_data->pose});

2-传输GPS数据

    trajectory_builder_->AddSensorData(
        sensor_id,
        carto::sensor::FixedFramePoseData{time, absl::optional<Rigid3d>()});

3-传输landmark数据

  trajectory_builder_->AddSensorData(sensor_id, landmark_data);

4-传输IMU数据

    trajectory_builder_->AddSensorData(
        sensor_id,
        carto::sensor::ImuData{imu_data->time, imu_data->linear_acceleration,
                               imu_data->angular_velocity});

5-传输TimedPointCloudData（带时间戳的点云数据）

    trajectory_builder_->AddSensorData(
        sensor_id, carto::sensor::TimedPointCloudData{
                       time, sensor_to_tracking->translation().cast<float>(),
                       carto::sensor::TransformTimedPointCloud(
                           ranges, sensor_to_tracking->cast<float>())});