讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解:https://blog.csdn.net/weixin_43013761/article/details/127350885
文末正下方中心提供了本人 联系方式, 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式,\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式,点击本人照片即可显示WX→官方认证
一、前言
在上一篇博客中,对 src/cartographer/cartographer/mapping/2d/submap_2d.cc 文件中的类 ActiveSubmaps2D 各个成员函数都进行了介绍,其主要功能如下图所示:
通过调用 ActiveSubmaps2D::InsertRangeData() 函数向子图 submaps_ 中插入数据,其会使得两个连续的子图之间的数据存在交集。如上图的子图1与子图2存在交集,同时子图2与子图3也存在交集。
从类名可以轻易的分辨出,ActiveSubmaps2D 表示激活的子图,其包含的成员变量 std::vector<std::shared_ptr<Submap2D>> submaps_ 表示的就是目前处于激活状态的子图(通常情况下是两个子图),如果 submaps_ 中的第一个子图插入数据足够了,则会被标记为完成,然后从 submaps_ 中擦除。
Submap2D 与 ActiveSubmaps2D 的成员函数都是在 src/cartographer/cartographer/mapping/2d/submap_2d.cc 文件中实现,既然分析完了 ActiveSubmaps2D,那么就来看看 Submap2D。不过在分析 Submap2D 之前,先要看看其父类 Submap。
在前面的博客中已经提及过,Submap2D 继承于 Submap,Submap 在 src/cartographer/cartographer/mapping/submaps.h 文件中被声明,主要定义了一些纯虚函数,以及一些成员变量。该些成员变量如下:
private:
const transform::Rigid3d local_pose_; // 子图原点在local坐标系下的坐标
int num_range_data_ = 0; //子图中数据的数目,初始为0
bool insertion_finished_ = false; //是否为插入完成状态,初始为否。
由于这些属性是私有的,所以无法被其派生类 Submap2D 继承,不过没有关系,因为提供了对该些属性访问或者操作的 public 接口,如下:
// 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;
}
另外,需要注意到的是,Submap 的构造函数需要传入 local_submap_pose 变量,完成对成员变量 local_pose_ 的初始化,其表示子图在 local 坐标系下的位姿。也就是说,每创建一个子图,都需要指定好该子图在 local 坐标系下的位姿。
二、Submap2D
1、Submap2D::Submap2D()
Submap2D 继承于 Submap,其存在两个私有属性:
private:
std::unique_ptr<Grid2D> grid_; // 地图栅格数据
// 转换表, 第[0-32767]位置, 存的是[0.9, 0.1~0.9]的数据
ValueConversionTables* conversion_tables_;
后续对于这两个属性会进行详细的分析,关于 Submap2D 的两个重载构造函数都会对这两个属性进行初始化。其第一个构造函数,直接接收 grid 与 conversion_tables 参数,然后利用初始化列表直接赋值给 grid_ 与 conversion_tables_,代码如下所示:
/**
* @brief 构造函数
*
* @param[in] origin Submap2D的原点,保存在Submap类里
* @param[in] grid 地图数据的指针
* @param[in] conversion_tables 地图数据的转换表
*/
Submap2D::Submap2D(const Eigen::Vector2f& origin, std::unique_ptr<Grid2D> grid,
ValueConversionTables* conversion_tables)
: Submap(transform::Rigid3d::Translation(
Eigen::Vector3d(origin.x(), origin.y(), 0.))),
conversion_tables_(conversion_tables) {
grid_ = std::move(grid);
}
还需要传递一个参数 origin,其表示子图的原点,也是就子图在 local 坐标系下的位姿。除上述构造函数外,还有另外一个构造函数,通过 proto 格式的数据构建 ProbabilityGrid 或者 TSDF2D 对象指针赋值给 grid_。代码就不再这里复制展示了。
2、Submap2D::InsertRangeData()
在 Submap2D 中,还有几个成员函数:Submap2D::ToProto(), Submap2D::UpdateFromProto(),Submap2D::ToResponseProto() 都与 proto 相关,暂时不讲解。先来看看看其中另外一个比较重要的函数Submap2D::InsertRangeData():
I n s e r t R a n g e D a t a ( ) = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = : {\color{Purple} InsertRangeData()======================================================================================================================================================}: InsertRangeData()======================================================================================================================================================:
功能 : {\color{Purple} 功能}: 功能: 把点云数据插入到子图之中
输入 : {\color{Purple} 输入}: 输入: 【参数①range_data】→需要被插入的点云数据。【参数②range_data_inserter】→负责数据插入的实例对象,为 RangeDataInserterInterface 的派生类。
返回 : {\color{Purple} 返回}: 返回: 无
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = : {\color{Purple} ================================================================================================================================================================}: ================================================================================================================================================================:
该函数实际上就是调用了 range_data_inserter->Insert(range_data, grid_.get()) 函数,将数据写入到栅格地图 grid_ 之中。该函数注释如下:
// 将雷达数据写到栅格地图中
void Submap2D::InsertRangeData(
const sensor::RangeData& range_data,
const RangeDataInserterInterface* range_data_inserter) {
CHECK(grid_);
CHECK(!insertion_finished());
// 将雷达数据写到栅格地图中
range_data_inserter->Insert(range_data, grid_.get());
// 插入到地图中的雷达数据的个数加1
set_num_range_data(num_range_data() + 1);
}
从这里还可以看出每插入一帧数据,num_range_data 才会 +1,因为 range_data 中存储的并不是一个点云数据,而是一帧。
3、Submap2D::Finish()
该函数比较简单,其调用了 grid_->ComputeCroppedGrid() 函数,该函数后续再进行分析,然后设置 insertion_finished_ 变量,标记当前子图为完成状态。
三、MapLimits
结合前面的分析,可以知道 Submap2D 中的 Grid2D 实例对象 grid_ 是十分重要的组成部分,回到之前 ActiveSubmaps2D::AddSubmap() 函数,存在如下代码:
std::unique_ptr<Grid2D>(static_cast<Grid2D*>(CreateGrid(origin).release()))
由此可知,Grid2D 的构建来自于 ActiveSubmaps2D::CreateGrid() 函数,该函数会构建 Grid2D 派生类对象 ProbabilityGrid 或者 TSDF2D 的独占指针。需要注意,在函数中可以看到如下代码:
MapLimits(resolution,
// 左上角坐标为坐标系的最大值, origin位于地图的中间
origin.cast<double>() + 0.5 * kInitialSubmapSize *
resolution *
Eigen::Vector2d::Ones(),
CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
也就是是说,无论构建 ProbabilityGrid 还是 TSDF2D 实例对象指针,都需要传入MapLimits 对象作为实参。那么就来看看 MapLimits 代码中是如何实现的,位于 src/cartographer/cartographer/mapping/2d/map_limits.h 文件中。从命名来看,地图限制,其是限制了那些东西呢?
首先每个子图 Submap2D 或者说都对应的一个栅格(Grid),后续每个栅格都会再进一步划分,划分之后以以 cell 为单位,如下图所示,每个小方格都表示一个一个 call:
既然要把子图 Submap2D 或者 Grid2D 划分成 call 形式,那么肯定需要指定每个 Grid2D 应该被划分成多少个 cell。先来看看 MapLimits 的构造函数。
1、MapLimits::MapLimits
/**
* @brief 构造函数
*
* @param[in] resolution 地图分辨率
* @param[in] max 左上角的坐标为地图坐标的最大值
* @param[in] cell_limits 地图x方向与y方向的格子数
*/
MapLimits(const double resolution, const Eigen::Vector2d& max,
const CellLimits& cell_limits)
: resolution_(resolution), max_(max), cell_limits_(cell_limits) {
CHECK_GT(resolution_, 0.);
CHECK_GT(cell_limits.num_x_cells, 0.);
CHECK_GT(cell_limits.num_y_cells, 0.);z
}
该构造函数首先指定了地图的分辨率,该分辨率表示由 options_.grid_options_2d().resolution() 确定。这里我们约定两个坐标系,如下:
①地图坐标系→该坐标系以物理单位作为衡量。
②像素坐标系→该坐标系以像素为单位
约定了上述两个坐标系之后,那么所谓的分辨率就表示地图坐标系与是像素坐标系的比值,简单的说就是栅格地图中一个像素代表地图坐标系多个个物理单位(米)。
第二个参数 max,表示的地图坐标的最大值,第三个参数 cell_limits 表示每个子图,或者说每个栅格x,y方向上包含了多少个 cell。
2、MapLimits::GetCellIndex()
该函数从命名可以看出来,其是获得 cell 在 gred 中的索引。代码如下所示:
// Returns the index of the cell containing the 'point' which may be outside
// the map, i.e., negative or too large indices that will return false for
// Contains().
// 计算物理坐标点的像素索引
Eigen::Array2i GetCellIndex(const Eigen::Vector2f& point) const {
// Index values are row major and the top left has Eigen::Array2i::Zero()
// and contains (centered_max_x, centered_max_y). We need to flip and
// rotate.
return Eigen::Array2i(
common::RoundToInt((max_.y() - point.y()) / resolution_ - 0.5),
common::RoundToInt((max_.x() - point.x()) / resolution_ - 0.5));
}
传入的 point 是地图坐标系的物理单位,计算方式也比较简单,物理坐标除以分辨率即可,等价于把 地图坐标 变换成 像素坐标。那么这里为什还要用 max_ 减去 point 呢? 如下所示:
/**
* note: 地图坐标系可视化展示
* ros的地图坐标系 cartographer的地图坐标系 cartographer地图的像素坐标系
*
* ^ y ^ x 0------> x
* | | |
* | | |
* 0 ------> x y <------0 y
*
* ros的地图坐标系: 左下角为原点, 向右为x正方向, 向上为y正方向, 角度以x轴正向为0度, 逆时针为正
* cartographer的地图坐标系: 坐标系右下角为原点, 向上为x正方向, 向左为y正方向
* 角度正方向以x轴正向为0度, 逆时针为正
* cartographer地图的像素坐标系: 左上角为原点, 向右为x正方向, 向下为y正方向
*/
其主要原因是因为 cartographer的地图坐标系 与 cartographer地图的像素坐标系 是不一样的,像素坐标的原点是在左上角。根据对源码的分析,像素坐标系中的一个像素代表地图坐标的一个cell。
3、MapLimits::GetCellCenter()
该函数的作用可以与 MapLimits::GetCellIndex() 是相反的,其输入一个像素索引,然后返回该像素对应在地图坐标系下的物理坐标:
// Returns the center of the cell at 'cell_index'.
// 根据像素索引算物理坐标
Eigen::Vector2f GetCellCenter(const Eigen::Array2i cell_index) const {
return {
max_.x() - resolution() * (cell_index[1] + 0.5),
max_.y() - resolution() * (cell_index[0] + 0.5)};
}
这里返回的是地图 cell 中心坐标。源码计算过程还是比较简单的,就是 MapLimits::GetCellIndex() 的逆操作。
4、MapLimits::Contains()
该函数输入一个像素坐标索引,其会判断该像素是否存于栅格地图内部,代码注释如下:
// Returns true if the ProbabilityGrid contains 'cell_index'.
// 判断给定像素索引是否在栅格地图内部
bool Contains(const Eigen::Array2i& cell_index) const {
return (Eigen::Array2i(0, 0) <= cell_index).all() &&
(cell_index <
Eigen::Array2i(cell_limits_.num_x_cells, cell_limits_.num_y_cells))
.all();
}
四、结语
关于 MapLimits 还有一些成员函数没有讲解,如 MapLimits::ToProto() 不过这已经不影响后续的分析了。再了解了 ActiveSubmaps2D、Submap、Submap2D、MapLimits 之后,接下来就要看一个大头部分:Grid2D 与 ProbabilityGrid