Cartographer中Submap(子图)没有被直接的调用进行维护, 而是针对2D和3D场景分别派生出子类Submap2D和Submap3D, 进行调用. 以2D为例, 为了方便维护, 又把Submap2D封装成了ActiveSubmaps2D进行维护, 其维护方式类似与滑窗, 也是只维护最近的一些数据.
1. Submap类
/**
* @brief 独立的子地图, 3个功能
*
* 保存在local坐标系下的子图的坐标
* 记录插入到子图中雷达数据的个数
* 标记这个子图是否是完成状态
*/
class Submap {
public:
// 构造函数, 将传入的local_submap_pose作为子图的坐标原点
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;
// Fills data into the 'response'.
virtual void ToResponseProto(
const transform::Rigid3d& global_submap_pose,
proto::SubmapQuery::Response* response) const = 0;
// Pose of this submap in the local map frame.
// 在local坐标系的子图的坐标
transform::Rigid3d local_pose() const { return local_pose_; }
// Number of RangeData inserted.
// 插入到子图中雷达数据的个数
int num_range_data() const { return num_range_data_; }
void set_num_range_data(const int num_range_data) {
num_range_data_ = num_range_data;
}
bool insertion_finished() const { return insertion_finished_; }
// 将子图标记为完成状态
void set_insertion_finished(bool insertion_finished) {
insertion_finished_ = insertion_finished;
}
private:
const transform::Rigid3d local_pose_; // 子图原点在local坐标系下的坐标
int num_range_data_ = 0;
bool insertion_finished_ = false;
};
从私有变量可以看到, Submap维护了三个东西
- 自己的坐标, 这个坐标是在local坐标系下的坐标
- 本子图中节点的个数, 也就是有几帧雷达数据
- 是不是个结束插入的子图, 节点数大于一定值后就不能在插入雷达数据了
Submap的坐标系在构造的时候就得到了, 是子图第一帧激光雷达的坐标系, 分析如下
Submap的第一次构造在
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
active_submaps_.InsertRangeData(range_data_in_local);
中, 调用的是active_submap的InsertRangeData这个函数.
在submap_2d.cc中看 ActiveSubmaps2D::InsertRangeData这个函数,
// 将点云数据写入到submap中
std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(
const sensor::RangeData& range_data) {
// 如果第二个子图插入节点的数据等于num_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()); // 是submap类的, 不是ActiveSubmaps2D的InsertRangeData
}
// 第一个子图的节点数量等于2倍的num_range_data时,第二个子图节点数量应该等于num_range_data
if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) { //一个子图有180个扫描
submaps_.front()->Finish();
}
return submaps();
}
其中新插入子图是调用了AddSubmap(range_data.origin.head<2>()),
// 新增一个子图,根据子图个数判断是否删掉第一个子图
void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) {
// 调用AddSubmap时第一个子图一定是完成状态,所以子图数为2时就可以删掉第一个子图了
if (submaps_.size() >= 2) {
// This will crop the finished Submap before inserting a new Submap to
// reduce peak memory usage a bit.
CHECK(submaps_.front()->insertion_finished());
// 删掉第一个子图的指针
submaps_.erase(submaps_.begin());
}
// 新建一个子图, 并保存指向新子图的智能指针
submaps_.push_back(absl::make_unique<Submap2D>(
origin,
std::unique_ptr<Grid2D>(
static_cast<Grid2D*>(CreateGrid(origin).release())),
&conversion_tables_));
}
也就是把range_data.origin.head<2>()放进来作为submap的原点, 再看看range_data
/**
* @brief local_slam_data中存储所有雷达点云的数据结构
*
* @param origin 点云的原点在local坐标系下的坐标
* @param returns 所有雷达数据点在local坐标系下的坐标, 记为returns, 也就是hit
* @param misses 是在光线方向上未检测到返回的点(nan, inf等等)或超过最大配置距离的点
*/
struct RangeData {
Eigen::Vector3f origin;
PointCloud returns;
PointCloud misses; // local坐标系下的坐标
};
origin就是这个点云在local坐标系下的坐标.
所以说submap的坐标就是submap第一帧点云点在local坐标系下的坐标.
2. submap2D类
…
3. ActiveSubmap2D类
ActiveSubmaps2D类中的submaps_列表实际最多只两个submap，一个认为是old_map，另一个认为是new_map，类似于滑窗操作。当new_map插入激光scan的个数达到阈值时，则会将old_map进行结束，并且不再增加新的scan。同时将old_map进行删除，将new_map作为oldmap，然后重新初始化一个新的submap作为newmap。代码很简单
// 将点云数据写入到submap中
std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(
const sensor::RangeData& range_data) {
// 如果第二个子图插入节点的数据等于num_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()); // 是submap类的, 不是ActiveSubmaps2D的InsertRangeData
}
// 第一个子图的节点数量等于2倍的num_range_data时,第二个子图节点数量应该等于num_range_data
if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) { //一个子图有180个扫描
submaps_.front()->Finish();
}
return submaps();
}
......
// 新增一个子图,根据子图个数判断是否删掉第一个子图
void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) {
// 调用AddSubmap时第一个子图一定是完成状态,所以子图数为2时就可以删掉第一个子图了
if (submaps_.size() >= 2) {
// This will crop the finished Submap before inserting a new Submap to
// reduce peak memory usage a bit.
CHECK(submaps_.front()->insertion_finished());
// 删掉第一个子图的指针
submaps_.erase(submaps_.begin());
}
// 新建一个子图, 并保存指向新子图的智能指针
submaps_.push_back(absl::make_unique<Submap2D>(
origin,
std::unique_ptr<Grid2D>(
static_cast<Grid2D*>(CreateGrid(origin).release())),
&conversion_tables_));
}
…
