cartographer代码阅读(4)——submap

前言

几个关键数据结构

• submap：父类，由submap2d继承后，还会有栅格类型等的属性

  //子图的局部位姿
  const transform::Rigid3d local_pose_;
  //子图包含的激光帧数
  int num_range_data_ = 0;
  //子图插入是否结束
  bool insertion_finished_ = false;

• 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;

    // The node pose in the local SLAM frame.
    transform::Rigid3d local_pose;
  };

  common::Time time() const { return constant_data->time; }

  // 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;

• TrajectoryNodePose:实际上是去掉了点云信息的TrajectoryNode，只包含位姿信息

  struct ConstantPoseData {
    common::Time time;
    transform::Rigid3d local_pose;
  };
  // The node pose in the global SLAM frame.
  transform::Rigid3d global_pose;

  absl::optional<ConstantPoseData> constant_pose_data;

• InsertionResult：用于回传到LocalSlamResultCallback函数中的数据类型，回传了节点id，这帧激光的数据，以及插入的子图的信息

    NodeId node_id;  
    std::shared_ptr<const TrajectoryNode::Data> constant_data;
    std::vector<std::shared_ptr<const Submap>> insertion_submaps;

如下：

 using LocalSlamResultCallback =
      std::function<void(int /* trajectory ID */, common::Time,
                         transform::Rigid3d /* local pose estimate */,
                         sensor::RangeData /* in local frame */,
                         std::unique_ptr<const InsertionResult>)>;

• TrajectoryBuilderInterface:轨迹构建器的接口,可以看到出了把传感器数据加入到轨迹构建器中，还把局部位姿估计的结果加入到轨迹构建器中

struct InsertionResult ...
using LocalSlamResultCallback ...
struct SensorId ...
AddSensorData ...
virtual void AddLocalSlamResultData(
      std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data) = 0

• LocalTrajectoryBuilder2D:接口类TrajectoryBuilderInterface的模板实现，不是继承关系

  struct InsertionResult ...
  struct MatchingResult {
    common::Time time;
    transform::Rigid3d local_pose;
    sensor::RangeData range_data_in_local;
    // 'nullptr' if dropped by the motion filter.
    std::unique_ptr<const InsertionResult> insertion_result;
  };
  std::unique_ptr<MatchingResult> AddRangeData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& range_data);
  void AddImuData(const sensor::ImuData& imu_data);
  void AddOdometryData(const sensor::OdometryData& odometry_data);

private:
  AddAccumulatedRangeData ...
  TransformToGravityAlignedFrameAndFilter ...
  InsertIntoSubmap ...
  ScanMatch ...
  InitializeExtrapolator ...
  ActiveSubmaps2D active_submaps_;
  MotionFilter motion_filter_;
  scan_matching::RealTimeCorrelativeScanMatcher2D
      real_time_correlative_scan_matcher_;
  scan_matching::CeresScanMatcher2D ceres_scan_matcher_;
  std::unique_ptr<PoseExtrapolator> extrapolator_;
  int num_accumulated_ = 0;
  sensor::RangeData accumulated_range_data_;
  RangeDataCollator range_data_collator_;

这里没看出来有AddLocalSlamResultData函数，那么局部位姿估计的结果是怎么回传的。暂时不看，先把数据类型理清楚

• ActiveSubmaps2D：新旧子图，新子图插入数据，旧子图匹配。这里生成了栅格地图，子图，还有值转换表

  std::vector<std::shared_ptr<const Submap2D>> InsertRangeData(
      const sensor::RangeData& range_data);
private:
  std::unique_ptr<RangeDataInserterInterface> CreateRangeDataInserter();
  std::unique_ptr<GridInterface> CreateGrid(const Eigen::Vector2f& origin);
  void FinishSubmap();
  void AddSubmap(const Eigen::Vector2f& origin);

  std::vector<std::shared_ptr<Submap2D>> submaps_;
  std::unique_ptr<RangeDataInserterInterface> range_data_inserter_;
  ValueConversionTables conversion_tables_;

• ValueConversionTables：值转换表。将16位数转成浮点数。

 public:
  const std::vector<float>* GetConversionTable(float unknown_result,
                                               float lower_bound,
                                               float upper_bound);

 private:
  std::map<const std::tuple<float /* unknown_result */, float /* lower_bound */,
                            float /* upper_bound */>,
           std::unique_ptr<const std::vector<float>>>
      bounds_to_lookup_table_;

• Submap2D：submap的继承类，包含了grid栅格数据和值转换表

...
void InsertRangeData(const sensor::RangeData& range_data,
                       const RangeDataInserterInterface* range_data_inserter);
 void Finish();
private:
  std::unique_ptr<Grid2D> grid_;
  ValueConversionTables* conversion_tables_;

• Grid2D：栅格地图，这里函数不做解释，细节后面再看。

...
 private:
  MapLimits limits_;
  std::vector<uint16> correspondence_cost_cells_;
  float min_correspondence_cost_;
  float max_correspondence_cost_;
  std::vector<int> update_indices_;

  // Bounding box of known cells to efficiently compute cropping limits.
  Eigen::AlignedBox2i known_cells_box_;
  const std::vector<float>* value_to_correspondence_cost_table_;