cartographer之Node和submap

Posted 2021-08-30 COCO_PEAK_NOODLE

Node比较简单，先来看，在代码里这是TrajectoryNode

namespace cartographer{
namespace mapping{

struct TrajectoryNodePose{
   struct ConstantPoseDate {
     common::time time;
     transform::Rigid local_pose;
   }
   transform::Rigid3d global_pose;
   absl::optional<ConstantPoseData> constant_pose_data;
}

strcut TrajectoryNode{
   struct Data{
      common::Time time;
      Eigen::Quaterniond gravity_alignment;
 
      sensor::PointCloud filtered_gravity_aligned_point_cloud;
      //3D
      sensor::PointCloud high_resolution_point_cloud;
      sensor::PointCloud low_resolution_point_cloud;
      transform::Rigid3d local_pose; 
   }
   common::Time time() const {return constant_data->time;}

   std::shared_ptr<const Data> constant_data;
   transform::Rigid3d global_pose;
}

proto::TrajectoryNodeData ToProto(const TrajectoryNode::Data& constant_data);
TrajectoryNode::Data FromProto(const proto::TrajectoryNodeData& proto);

} //mapping
} //cartographer

这里面使用了transform::Rigid3d、common::Time等数据，因为作者要使用protobuf来保存所以自定义了这些变量。

我们再来看看submap类，这个类是个虚基类，后面分别实现了submap2D和submap3D等

class Submap{
  public:
    Submap(const transform::Rigid3d& local_submap_pose)
        : local_pose_(local_submap_pose){}
    
    virtual ~Submap(){}
    virtual proto::Submap ToProto(bool include_grid_data) const = 0;
    virtual void UpdateFromProto(const proto::Submap& proto) = 0;
    
    virtual void ToResponseProto(
          const transform::Rigid3d& global_submap_pose,
          proto::SubmapQuery::Response* response
       )  const = 0;

     transform::Rigid3d local_pose() const {return local_pose;}
 
     int num_range_data() const {return num_range_data;}
     void set_num_range_data(cosnt int num_range_data){
          num_range_data_ = num_range_date;
    }
     
     bool insertion_finished() const { return insertion_finished_;}
     void set_insertion_finished(bool insertion_finished){
        insertion_finished_ = insertion_finished;
     }
  private:
    const tranform::Rigid3d local_pose_;
    int num_range_data_ = 0;
    bool insertion_finished_ = false;
}

submap2D

namespace cartographer{
namespace mapping{

class Submap2D ： public Submap {

public:
   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 inclue_grid_data) const override;
    void UpdateFromProto(const proto::Submap& proto) override;

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

     const Grid2D* grid() const { return grid_.get();};
 
     void InsertRangeData(const sensor::RangeData& range_data,
                const RangeDataInserterInterface* range_data_inserter);

     void Finish();

private:
  std::unique_ptr<Grid2D> grid_;
  ValurConversionTables* conversion_tables_;
}

}//mapping
}//cartographer

submap2D的创建，是在ActiveSubmap2D中创建的，当旧的submap插入的node满足要求，则创建新的子图

std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(
    const sensor::RangeData& range_data) {
  if (submaps_.empty() ||
      submaps_.back()->num_range_data() == options_.num_range_data()) {
    //这里创建
    AddSubmap(range_data.origin.head<2>());
  }
  for (auto& submap : submaps_) {
    submap->InsertRangeData(range_data, range_data_inserter_.get());
  }
  if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) {
    submaps_.front()->Finish();
  }
  return submaps();
}

  submaps_.push_back(absl::make_unique<Submap2D>(
      origin,
      std::unique_ptr<Grid2D>(
          static_cast<Grid2D*>(CreateGrid(origin).release())),
      &conversion_tables_));

知识点1-：Eigen::Vector3f如何转Eigen::Vector2f，应该是重写了模板函数

   AddSubmap(range_data.origin.head<2>())；

知识点2-std::unique_ptr release的使用
https://www.cnblogs.com/zengtx/p/11911853.html