cartographer代码阅读(4)——世界坐标系点和像素坐标系点的转换

构建栅格地图，要弄清楚坐标之间的关系。本篇根据代码，画出了坐标转换的关系。

如下图：

由于像素坐标系通常都是从左上角开始，所以把上面的图旋转一下：

 

这样就清晰了。可以看到，cartographer地图坐标系的原点在地图的右下角，只不过x轴朝上，y轴朝右。在实际转换的时候，直接在标准世界坐标系下画好图就好了，并不需要去死记坐标系原点的方位在哪。

cartographer中的坐标转换的代码如下：

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

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

 

 

 

——————————————————————————————————————————————————————————————————————————————————————

再深入看构建栅格地图的过程，就可以发现，这个世界坐标系的原点，其实是插入新子图时，对应关键激光帧的pose为地图原点，这和许多建图算法是一致的。且地图原点位于像素坐标系下的中心。如下图，对上面的图做了一些调整，看的更清晰。

cartographer中对应的具体的代码如下：

case proto::GridOptions2D::PROBABILITY_GRID:
      return absl::make_unique<ProbabilityGrid>(
          MapLimits(resolution,
                    origin.cast<double>() + 0.5 * kInitialSubmapSize *
                                                resolution *
                                                Eigen::Vector2d::Ones(),
                    CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
          &conversion_tables_);