地图坐标系-cartographer
Kong Liangqian Lv6
 2021-11-18 18:38:29  

ActiveSubmaps2D::AddSubmap 添加子图的过程中，需要新建一个子图（类Submap2D）然后进行添加。创建Submap2D则需要一个创建Grid2D

在ActiveSubmaps2D::CreateGrid函数中（cartographer/mapping/2d/submap2d.h），可以创建不同类型的Grid2D：PROBABILITY_GRID与TSDF

类ProbabilityGrid 和 TSDF2D 均继承于 Grid2D，Grid2D 继承于 GridInterface

Grid2D

该类表示一个2D的网格地图

保存的值包括 所有可能的栅格值以及其对应的free 概率的值，即转换表

成员变量

1
2
3
4
5
6
7
8
9
10
11
12
MapLimits limits_;  // 地图大小边界, 包括x和y最大值, 分辨率, x和y方向栅格数

// 地图栅格值, 存储的是free的概率转成uint16后的[0, 32767]范围内的值, 0代表未知
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_;           // 栅格的bounding box, 存的是像素坐标
// 将[0, 1~32767] 映射到 [0.9, 0.1~0.9] 的转换表
const std::vector<float>* value_to_correspondence_cost_table_;

说明！

  • probability: 栅格被占据的概率

    kMinProbability = 0.1, kMaxProbability = 0.9, kUnknownProbabilityValue = 0

  • Odds: probability / (1.0f - probability)

  • CorrespondenceCost: 栅格是free的概率; CorrespondenceCost + probability = 1

    kMinCorrespondenceCost = 0.1, kMaxCorrespondenceCost = 0.9

    kUnknownCorrespondenceValue = 0, kUpdateMarker = 32768

  • Value: 代码里存储的栅格值, 是[0, 32767]范围内的 uint16 整数

  • valueto_correspondence_cost_table: 将[0, 1~32767] 映射到 [0, 0.1~0.9] 的转换表

  • hit_table 计算[0, 1~32767] 按照占用概率0.55更新之后的值

  • misstable 计算[0, 1~32767] 按照空闲概率0.49更新之后的值

构造函数

基本上就是简单粗暴的赋值操作

correspondencecost_cells ： 初始化为一个长度为 地图长 地图宽的，默认值为0的vector。*保存地图栅格值！

valueto_correspondence_cost_table ：一个转换表，存储着栅格值[0, 1~32767] 映射到 [0, 0.1~0.9] 的表

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
/**
 * @brief 构造函数
 * 
 * @param[in] limits 地图坐标信息
 * @param[in] min_correspondence_cost 最小correspondence_cost 0.1
 * @param[in] max_correspondence_cost 最大correspondence_cost 0.9
 * @param[in] conversion_tables 传入的转换表指针
 */
Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost,
               float max_correspondence_cost,
               ValueConversionTables* conversion_tables)
    : limits_(limits),
      correspondence_cost_cells_(
          limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells,
          kUnknownCorrespondenceValue),  // 0
      min_correspondence_cost_(min_correspondence_cost),  // 0.1
      max_correspondence_cost_(max_correspondence_cost),  // 0.9
      // 新建转换表
      value_to_correspondence_cost_table_(conversion_tables->GetConversionTable(
          max_correspondence_cost, min_correspondence_cost,
          max_correspondence_cost)) {
  CHECK_LT(min_correspondence_cost_, max_correspondence_cost_);
}

GetConversionTable 返回的内容是一个vector，长度为65536 + 1，内容为如下的第二行

1
2
3
[0,   1,   ..., 32767, 0,   1,  ..., 32767]
通过映射转换如下，除了0直接变成0.9， 其他的数都缓缓往上均匀增加
[0.9, 0.1, ..., 0.9,   0.9, 0.1,..., 0.9  ]

结果为存储的值相对应的free的概率。

注意，这里conversion_tables 经过调用，成员变量已经被赋值了。

因此，调用的Grid2D的ProbabilityGrid的构造函数中， conversiontables 也就有值了给conversion_tables 了

1
2
3
4
5
ProbabilityGrid::ProbabilityGrid(const MapLimits& limits,
                                 ValueConversionTables* conversion_tables)
    : Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost,
             conversion_tables),
      conversion_tables_(conversion_tables) {}

成员函数

FinishUpdate

雷达的点是散射的，因此有的栅格会被多次打到，为了防止多次更新，可以设置结束更新。

在更新的一次，都会加一次kUpdateMarker，32768。 和转换表内的两次0-32767对应

1
2
3
4
5
6
7
8
9
void Grid2D::FinishUpdate() {
  while (!update_indices_.empty()) {
    DCHECK_GE(correspondence_cost_cells_[update_indices_.back()],
              kUpdateMarker);
    // 更新的时候加上了kUpdateMarker, 在这里减去
    correspondence_cost_cells_[update_indices_.back()] -= kUpdateMarker;
    update_indices_.pop_back();
  }
}

GrowLimits

地图每一次扩大都是两倍的进行扩大。

bounding box平移，从左上角移动到以old为左上角的一个box

image-20211119224129580

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
// 根据坐标决定是否对地图进行扩大
void Grid2D::GrowLimits(const Eigen::Vector2f& point,
                        const std::vector<std::vector<uint16>*>& grids,
                        const std::vector<uint16>& grids_unknown_cell_values) {
  CHECK(update_indices_.empty());
  // 判断该点是否在地图坐标系内
  while (!limits_.Contains(limits_.GetCellIndex(point))) {
    const int x_offset = limits_.cell_limits().num_x_cells / 2;
    const int y_offset = limits_.cell_limits().num_y_cells / 2;
    // 将xy扩大至2倍, 中心点不变, 向四周扩大
    const MapLimits new_limits(
        limits_.resolution(),
        limits_.max() +
            limits_.resolution() * Eigen::Vector2d(y_offset, x_offset),
        CellLimits(2 * limits_.cell_limits().num_x_cells,
                   2 * limits_.cell_limits().num_y_cells));
    const int stride = new_limits.cell_limits().num_x_cells;
    // 老坐标系的原点在新坐标系下的一维像素坐标
    const int offset = x_offset + stride * y_offset;
    const int new_size = new_limits.cell_limits().num_x_cells *
                         new_limits.cell_limits().num_y_cells;

    // grids.size()为1
    for (size_t grid_index = 0; grid_index < grids.size(); ++grid_index) {
      std::vector<uint16> new_cells(new_size,
                                    grids_unknown_cell_values[grid_index]);
      // 将老地图的栅格值复制到新地图上
      for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) {
        for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) {
          new_cells[offset + j + i * stride] =
              (*grids[grid_index])[j + i * limits_.cell_limits().num_x_cells];
        }
      }
      // 将新地图替换老地图, 拷贝
      *grids[grid_index] = new_cells;
    } // end for
    // 更新地图尺寸
    limits_ = new_limits;
    if (!known_cells_box_.isEmpty()) {
      // 将known_cells_box_的x与y进行平移到老地图的范围上
      known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset));
    }
  }
}

MapLimits

此类就是计算物理坐标和像素坐标之间的一个转换，对地图的大小保存一些元数据

几个地图坐标系

成员变量

  • resolution：分辨率。默认0.05，室外可以是0.1，室内精度要高的话可以0.02，需要注意内存
  • Eigen::Vector2d max: 左上角的坐标为地图坐标的最大值
  • CellLimits celllimits： 一个struct保存着，地图x方向与y方向的格子数

构造函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
/**
  * @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.);
 }

调用处

于cartographer/mapping/2d/submap_2d.cc文件中，函数ActiveSubmaps2D::CreateGrid 里，创建了MapLimits

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
// 以当前雷达原点为地图原件创建地图
std::unique_ptr<GridInterface> ActiveSubmaps2D::CreateGrid(
    const Eigen::Vector2f& origin) {
  // 地图初始大小,100个栅格  
  constexpr int kInitialSubmapSize = 100;
  float resolution = options_.grid_options_2d().resolution(); // param: grid_options_2d.resolution
  switch (options_.grid_options_2d().grid_type()) {
    // 概率栅格地图
    case proto::GridOptions2D::PROBABILITY_GRID:
      return absl::make_unique<ProbabilityGrid>(
          MapLimits(resolution,
                    // 左上角坐标为坐标系的最大值, origin位于地图的中间
                    origin.cast<double>() + 0.5 * kInitialSubmapSize *
                                                resolution *
                                                Eigen::Vector2d::Ones(),
                    // x方向和y方向都给100个栅格
                    CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
          &conversion_tables_);

左上角的最大值 = origin坐标点 + 栅格数*分辨率。

如果分辨率越小，地图左上角能覆盖到的距离越短，但是格子多，因此分辨率会很高，很清晰

成员函数

  • contain，判断给出的点的下标是否在坐标系内
  • GetCellCenter：从下标点转为坐标点
  • GetCellIndex：从坐标点转换为下标点
 Comments