Cartographer基础之Scan、Node、Submap、Trajectory

一、Cartographer 中 scan、node 、submap、trajectory之间的关系

每条trajectory由多个submap组成，每个submap由多个node组成，每个node由一帧或多帧scan得到。

二、SCAN

Scan 通过

LocalTrajectoryBuilder2D::AddRangeData(
  const std::string& sensor_id, const sensor::TimedPointCloudData& unsynchronized_data)

传给node和submap,这里的基本就是由订阅的scan话题转换为点云获得。

三、Node

先看类

TrajectoryNode::Data
struct TrajectoryNode {
  struct Data {
    common::Time time;

    // Transform to approximately gravity align the tracking frame as
    // determined by local SLAM.
    Eigen::Quaterniond gravity_alignment;

    // Used for loop closure in 2D: voxel filtered returns in the
    // 'gravity_alignment' frame.
    sensor::PointCloud filtered_gravity_aligned_point_cloud;   // 经过水平投射后的点云数据，可用于2D情况下做Loop Closure.

    // Used for loop closure in 3D.
    sensor::PointCloud high_resolution_point_cloud; //高分辨率点云
    sensor::PointCloud low_resolution_point_cloud;  //低分辨率点云
    Eigen::VectorXf rotational_scan_matcher_histogram;  //旋转匹配直方图；VectorXf是一个长度可变的向量。

    // The node pose in the local SLAM frame.
    transform::Rigid3d local_pose;              //节点在Local SLAM中的Pose
  };

  common::Time time() const { return constant_data->time; }     // 返回成员变量constant_data的时间

  // This must be a shared_ptr. If the data is used for visualization while the
  // node is being trimmed, it must survive until all use finishes.
  std::shared_ptr<const Data> constant_data;

  // The node pose in the global SLAM frame.
  transform::Rigid3d global_pose;              //节点在世界坐标系下的位姿
};

这里主要有两个重要的成员 constant_data 和 global_pose，其中 constant_data主要用了重力方向投影后的点云以及local_pose

主要用到的地方为

NodeId PoseGraph2D::AddNode(
  std::shared_ptr<const TrajectoryNode::Data> constant_data, const int trajectory_id, const std::vector<std::shared_ptr<const Submap2D> >& insertion_submaps)

其中 这里会根据 预估位姿、constant_data和trajectory_id 添加数据到trajectory。

constant_data 来自 InsertIntoSubmap内的

 return absl::make_unique<InsertionResult>(InsertionResult {
        std::make_shared<const TrajectoryNode::Data>(TrajectoryNode::Data {
          time,
          gravity_alignment,
          filtered_gravity_aligned_point_cloud,
          {},  // 'high_resolution_point_cloud' is only used in 3D.
          {},  // 'low_resolution_point_cloud' is only used in 3D.
          {},  // 'rotational_scan_matcher_histogram' is only used in 3D.
          pose_estimate
        }),
        std::move(insertion_submaps)
      });

红色的部分就是constant_data ，也就是node的数据，也就是说2d的node是由点云、重力的四元数、local slam下的位姿组成的。

四、Submap

再看Submap这里2D、3D都看一下

先看2D的

class Submap2D : public Submap {
 public:
  //构造函数；第一个参数是原点坐标，第二个参数是一个Grid2D变量，第三个参数是存储栅格化坐标和坐标上的概率值
  //Grid2D定义在/mapping/2d/grid_2d.h中，继承了GridInterface(/mapping/grid_interface.h)，
  //Grid2D又被ProbabilityGrid继承，定义在/mapping/2d/probability_grid.h中
  //基本数据都存储在Grid2D的成员变量grid_中。
  Submap2D(const Eigen::Vector2f& origin, std::unique_ptr<Grid2D> grid,
           ValueConversionTables* conversion_tables);
  explicit Submap2D(const proto::Submap2D& proto,
                    ValueConversionTables* conversion_tables);

  proto::Submap ToProto(bool include_grid_data) const override;
  void UpdateFromProto(const proto::Submap& proto) override;

  void ToResponseProto(const transform::Rigid3d& global_submap_pose,
                       proto::SubmapQuery::Response* response) const override;

  const Grid2D* grid() const { return grid_.get(); }

  // Insert 'range_data' into this submap using 'range_data_inserter'. The
  // submap must not be finished yet.
  void InsertRangeData(const sensor::RangeData& range_data,
                       const RangeDataInserterInterface* range_data_inserter);// 利用RangeDataInserterInterface来插入并更新概率图。该接口在/mapping/range_data_inserter_interface.h中定义
  // ProbabilityGridRangeDataInserter2D继承了该接口，定义在/mapping/2d/probability_grid_range_data_inserter_2d.h中
  void Finish();

 private:
  std::unique_ptr<Grid2D> grid_;
  ValueConversionTables* conversion_tables_; //概率图数据存储在这里。
};

这里submap就由初始点、概率地图和参数表组成。其中激光数据用来更新概率地图

五、Trajectory

Trajectory从

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

可以看到由time 、local_pose 、 range_data_in_local 、InsertionResult

组成，其中range_data_in_local 与scan密切联系，InsertionResult 与node和submap密切联系。