专栏名称: 计算机视觉工坊
专注于计算机视觉、VSLAM、目标检测、语义分割、自动驾驶、深度学习、AI芯片、产品落地等技术干货及前沿paper分享。这是一个由多个大厂算法研究人员和知名高校博士创立的平台，我们坚持工坊精神，做最有价值的事~
目录
相关文章推荐
EETOP  ·  之前400人被传原地失业！国产GPU独角兽— ... ·  14 小时前  
EETOP  ·  台积电2nm、Intel 18A 首次工艺对决 ·  17 小时前  
哎咆科技  ·  iPhone 壁纸 | 美拉德色系高清壁纸 ·  昨天  
EETOP  ·  数字IC面试题汇总 ·  2 天前  
EETOP  ·  宁波电子产业与前沿技术研讨会：通信|物联网| ... ·  2 天前  
51好读  ›  专栏  ›  计算机视觉工坊

无惧任何复杂地形！开启机器人导航新纪元！全新通用网格地图gridmap论文+源码解析

计算机视觉工坊  · 公众号  ·  · 2024-10-23 07:00

正文

点击下方 卡片 ，关注 「3D视觉工坊」 公众号
选择 星标 ，干货第一时间送达

来源：计算机视觉工坊，作者： 进阶的花生

添加小助理：cv3d008，备注：方向+学校/公司+昵称，拉你入群。文末附3D视觉行业细分群。

扫描下方二维码，加入「 3D视觉从入门到精通 」知识星球 ，星球内凝聚了众多3D视觉实战问题，以及各个模块的学习资料： 近20门秘制视频课程 、 最新顶会论文 、计算机视觉书籍 、 优质3D视觉算法源码 等。想要入门3D视觉、做项目、搞科研，欢迎扫码加入！

github地址： https://github.com/ANYbotics/grid_map

应用示例视频： https://www.youtube.com/watch?v=puyRbPtPyXw https://www.youtube.com/watch?v=phaBKFwfcJ4

一. 基本介绍

1.1 概述

gridmap属于一种 复合局部移动地图。底层基于Eigen实现，使用时可以高效、便捷的操作地图数据。

gridmap通常由多层layer组成，每一层用于表达相互关联的不同信息。并且可以无拷贝高效地实现地图中心跟随robot移动。

下图是单层gridmap栅格地图示意图。首先要明确栅格地图的尺寸、分辨率以及中心位置，通常会将grid map center设置为机器人当前位置（全局坐标系下），并根据实际需求（观测精度及范围要求）合理设置地图尺寸以及分辨率。

本文主要对gridmap的 基础数据结构、存储模式、坐标转换、迭代器展开介绍。

1.2 特性

1.3 应用场景

（1）占据栅格地图，障碍物位置根据概率更新；https://zhuanlan.zhihu.com/p/21738718

（2）高度栅格地图，移动机器人崎岖路形导航；https://www.cnblogs.com/buxiaoyi/p/7451497.html

（3）高效存取数据；

二. 源码梳理

2.1 文件结构

本文重点介绍grid_map_core部分源码实现。

详细的代码解释及拓扑结构：https://docs.ros.org/en/kinetic/api/grid_map_core/html/files.html

2.2 数据结构定义

gridmap底层数据存储采用Eigen数据格式。直接调用Egien方法处理地图数据，使得gridmap处理数据高效、便捷。 

using




    
 Matrix = Eigen::MatrixXf;
using DataType = Matrix::Scalar;
using Position = Eigen::Vector2d; // gridmap用于表示位置的结构
using Vector = Eigen::Vector2d;
using Position3 = Eigen::Vector3d; // 可以用于将Position与Position在地图中的值合并组成Position3
using Vector3 = Eigen::Vector3d;
using Index = Eigen::Array2i; // gridmap中使用索引获取数据，为包含两个元素的整形数组
using Size = Eigen::Array2i;  // 栅格两个维度的栅格数，单位:个
using Length = Eigen::Array2d; // 地图实际尺寸，单位:m
using Time = uint64_t;

2.3 存储模式

2.3.1 多层数据存储

gridmap的多层数据存储是通过哈希表unordered_map形式实现，其中每一层存储数据的底层结构为Eigen::MatrixXf。

建图时通常会使用额外的信息去解释数据，gridmap 使用多层网格地图存储不同类型的信息。

如占据栅格地图，若需要同时考虑某一位置障碍物的存在概率、类型、距离。可以将概率、类型、距离抽象为三个独立层，三个层同一索引对应的 占据概率值、障碍物类型、障碍物距离 三个信息相互匹配。 

/*!
 * Grid map managing multiple overlaying maps holding float values.
 * Data structure implemented as two-dimensional circular buffer so map
 * can be moved efficiently.
 */
//! Grid map data stored as layers of matrices.
std::unordered_map<std::string, Matrix> data_;

2.3.2 二维循环矩阵

在机器人运动的过程中，地图会随着机器人发生整体移动。移动时，地图方向不变，地图中心点位置跟随机器人移动。

gridmap 提供了一种高效计算且无损的方法来更改栅格地图的位置。使用二维循环缓冲区存储数据，可以在不分配新内存的情况下移动地图。

  • 位置更改前后重叠区域的数据仍然存储；
  • 在地图新位置之外的数据被丢弃；
  • 以前未知区域的网格被清空（设置为nan）；

这意味着地图重叠区域中的数据不会在内存中移动。最大限度地减少了移动地图时更改的数据量。

下图展示了机器人移动过程中（移动过程和上图一致），二维循环缓冲区中数据的变化情况，其中bufferStartIndex为移动后地图的起始索引位置。

2.3.3 move

gridmap中通过GridMap::move()接口实现地图的移动。

（1）获取移动前后位置偏移positionShift与索引偏移indexShift；

（2）清除移出地图的数据；

  • 若某一方向偏移大于地图size，清除整个栅格地图；
  • 若某一方向偏移小于地图size
    • 根据移动方向确定清除区域的起始索引 startIndex和终止索引 endIndex；
    • 若起始索引和终止索引在一个连续区域，按行或列方式删除连续区域；（One region to drop）
    • 若起始索引和终止索引栅格跨越地图边界时，分两个区域删除；（Two regions to drop）

（3）更新地图起始位置内存索引startIndex_和地图中心位置position_；

setPosition&move

There are two different methods to change the position of the map:

  • setPosition(...): Changes the position of the map without changing data stored in the map. This changes the corresponce between the data and the map frame.
  • move(...): Relocates the region captured by grid map w.r.t. to the static grid map frame. Use this to move the grid map boundaries without relocating the grid map data. Takes care of all the data handling, such that the grid map data is stationary in the grid map frame.
    • Data in the overlapping region before and after the position change remains stored.
    • Data that falls outside the map at its new position is discarded.
    • Cells that cover previously unknown regions are emptied (set to nan). The data storage is implemented as two-dimensional circular buffer to minimize computational effort.

2.4 坐标系转换

2.4.1 坐标系介绍

1. 全局坐标系

全局坐标系是描述机器人实时位置的全局基准。

全局坐标系在整个运动过程中固定不变，所有坐标系都相对于全局坐标系做转换。

2. 局部坐标系

局部坐标系是指机器人自身的坐标系，会随机器人移动、旋转发生变化的坐标系。

3. gridmap地图坐标系

gridmap地图坐标系，原点位于整个栅格地图的中心点，方向通常与全局坐标系一致。gridmap地图坐标系原点随机器人运动而实时移动，方向始终保持不变。

机器人在每次移动时，需要维护gridmap坐标系原点在全局坐标系下的位置。因此gridmap坐标系下任意位置都可以通过转换关系获取其全局坐标系下的坐标。

4. 内存坐标系

内存坐标系的方向主要由存储数据的顺序决定。为方便进行position和Index之间转换，内存坐标系通常与gridmap坐标系保持相反。

如下图所示，内存坐标系原点为gridmap坐标系第一象限的角点，并按gridmap坐标系相反的方向存储数据。

5. gridmap中间坐标系

gridmap中间坐标系，如下图所示，即将gridmap坐标系原点向两个方向各平移地图尺寸的一半，移动后与内存坐标系原点位于同一个cell中。

引入gridmap中间坐标系，主要为方便说明Index和Position的转换关系。gridmap中间坐标系与内存坐标系原点一致，方向相反。

这样在gridmap中间坐标系下计算的结果，直接取相反数就可以转换到内存坐标系下。

6. 内存中间坐标系

由于gridmap使用二维循环数组存储地图数据，所以内存坐标系下索引(0,0)的位置并非gridmap实际的起始位置（除非没有move过）。

每次执行move后，内存起始位置（bufferStartIndex）均会随地图移动。所以内存坐标系也会随之移动，如下图所示内存坐标系原点会平移到bufferStartIndex的位置，方向与原内存坐标系一致。

2.4.2 Index&Position转换

gridmap设计的巧妙之处。并没有将位置Position和值做一一映射。而是通过将地图栅格化，建立Position和Index的映射关系，再通过Index索引值。

Position -> Index -> value

机器人每次移动时会更新gridmap地图原点的位置。由于地图尺寸与分辨率固定，这样确定了gridmap地图中心位置，任意cell的位置都可以确定。

getIndexFromPosition（Position->Index）

通过全局坐标position获取内存索引

全局坐标系->gridmap坐标系：position-mapPosition

gridmap坐标系->gridmap中间坐标系：position-mapPosition-offset

gridmap中间坐标系->内存坐标系：-1*(position-mapPosition-offset)/resolusion

内存坐标系->内存中间坐标系：-1*(position-mapPosition-offset)/resolusion+bufferStartIndex

规范二维循环buffer内存索引：warpIndexToRange(-1*(position-mapPosition-offset)/resolusion+bufferStartIndex)

getPositionFromIndex（Index->Position）

通过内存索引获取全局坐标系位置

内存中间坐标系->内存坐标系：bufferIndex-bufferStartIndex

规范二维循环buffer内存索引：warpIndexToRange(bufferIndex-bufferStartIndex)

内存坐标系->gridmap中间坐标系：-1*warpIndexToRange(bufferIndex-bufferStartIndex)

gridmap中间坐标系->gridmap坐标系：-1*warpIndexToRange(bufferIndex-bufferStartIndex)+offset

gridmap坐标系->全局坐标系：-1*warpIndexToRange(bufferIndex-bufferStartIndex)+offset+mapPosition

2.5 迭代器

gridmap提供了一些列迭代器用来高效的迭代 特定范围 栅格数据。迭代器实现需要重载"*"，"++"，"!="运算符，并需要明确迭代终止条件。

"*"：获取当前cell的索引Index；

"++"：定位下一个cell的索引Index；

"!="：判断两个迭代器的索引是否一致；

"isPastEnd"：判断迭代是否结束标志位；

2.5.1 GridMapIterator 

/*!
 * @param gridMap 栅格地图
 */
GridMapIterator ( const grid_map::GridMap& gridMap) ;

迭代范围：整个栅格地图。将二维栅格地图使用一维索引linearIndex_形式表示。

"++"：递增一维索引linearIndex_，当linearIndex_超过cell数量，isPastEnd=true；

"*"：获取linearIndex_，并将一维索引转换为二维索引Index；

2.5.2 SubmapIterator 

/*!
 * @param gridMap 栅格地图
 * @param submapStartIndex 子图起始索引
 * @param submapSize 子图尺寸
 */
SubmapIterator(const grid_map::GridMap& gridMap, const Index& submapStartIndex, const Size& submapSize);

迭代范围：子图范围。索引开始cell为子图左上角cell索引。

"++"：先迭代子图索引，再根据子图索引获取目标cell全局内存索引。

"*"：获取当前index_；

关键逻辑 

// 迭代子图索引
if (submapIndex[1] + 1 1]) {
    // Same row.
    submapIndex[1]++;
} else {
    // Next row.
    submapIndex[0]++;
    submapIndex[1] = 0;
}
// End of iterations reached.（isPastEnd）
if (!checkIfIndexInRange(tempSubmapIndex, submapBufferSize)) {
    return false;
}
// 获取bufferIndex
Index unwrappedSubmapStartIndex = getIndexFromBufferIndex(SubmapStartIndex, bufferSize, bufferStartIndex);
Index index_ = getBufferIndexFromIndex(unwrappedSubmapStartIndex + submapIndex, bufferSize, bufferStartIndex);

2.5.3 CircleIterator 

/*!
 * @param gridMap 栅格地图
 * @param center 圆心
 * @param radius 半径
 */
CircleIterator(const GridMap& gridMap, const Position& center, const double radius);

获取圆的外接矩形。外接矩形可以视作子图，迭代方式基本和子图一致。

boundPositionToRange：确保圆的topLeft和bottomRight在栅格地图内

不同的是迭代时需要判断cell是否在圆内，如果当前cell不在圆内，按照子图方式继续向下迭代。

判断cell是否在圆内的方式：cell对应的点到圆心距离的欧式距离平方 < 半径的平方。

关键逻辑 

bool CircleIterator::isInside() const {
  Position position;
  getPositionFromIndex(position, *(*internalIterator_), mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
  double squareNorm = (position - center_).array().square().sum();
  return (squareNorm <= radiusSquare_);
}

2.5.4 EllipseIterator 

/*!
 * @param gridMap 栅格地图
 * @param center 圆心
 * @param length 长短半轴
 * @param length 旋转角度
 */
EllipseIterator(const GridMap& gridMap, const Position&              
    center, const Length& length, const double rotation);

与圆形迭代器类似。取椭圆的外接矩形（轴对齐边界框）视为子图，迭代方式与子图一致。

迭代时需要判断cell是否在椭圆内，如果当前cell不在椭圆内，按照子图方式继续向下迭代。

判断cell是否在圆内的方式：

关键逻辑 

// 获取椭圆轴对齐边界框半长度
const Eigen::Rotation2Dd rotationMatrix(rotation);
Eigen::Vector2d u = rotationMatrix * Eigen::Vector2d(length(0), 0.0);
Eigen::Vector2d v = rotationMatrix * Eigen::Vector2d(0.0, length(1));
const Length boundingBoxHalfLength = (u.cwiseAbs2() + v.cwiseAbs2()).array().sqrt();
// 判断是否在椭圆内
bool CircleIterator::isInside() const {
  Position position;
  getPositionFromIndex(position, *(*internalIterator_), mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
  double squareNorm = (position - center_).array().square().sum();
  return (squareNorm <= radiusSquare_);
}

2.5.5 PolygonIterator 

/*!
 * @param gridMap 栅格地图
 * @param polygon 多边形角点队列
 */
PolygonIterator(const grid_map::GridMap& gridMap, const grid_map::Polygon& polygon);

迭代方式与圆形、椭圆迭代器类似，取多边形外接矩形视为子图，迭代方式与子图一致。

迭代时需要判断cell是否在多边形内，这里使用的 射线交叉法。

判断点是否在多边形内：射线交叉法
从该点出发向任意一个方向引一条射线，计算与多边形边线交点个数。交点个数为奇数则在多边形内，偶数则在多边形外。

关键逻辑 

// 射线交叉法
bool Polygon::isInside(const Position& point) const {
  int cross = 0;
  for (size_t i = 0, j = vertices_.size() - 1; i     // 相当于是沿该点引一条向上的射线，计算与多边形边线交点（不包含多边形上的点）
    if ( ((vertices_[i].y() > point.y()) != (vertices_[j].y() > point.y()))
           && (point.x()             (vertices_[j].y() - vertices_[i].y()) + vertices_[i].x()) ) {
      cross++;
    }
  }
  return bool(cross % 2);
}

2.5.6 LineIterator 

/*!
 * @param gridMap 栅格地图
 * @param start 线段起点
 * @param start 线段终点
 */
LineIterator(const grid_map::GridMap& gridMap, const Position& 
    start, const Position& end);

图像线段绘制方法：Bresenham画线算法

（1）确定线段起始点和终止点所在cell的索引；（确保index在地图内）

（2）初始化迭代参数。Bresenham迭代参数，确定迭代主方向，线段占用栅格数目等；

（3）迭代时主方向索引递增，次方向索引利用Bresenham确定是否需要递增；

（4）迭代终止条件，迭代次数>线段占用栅格数目；

关键逻辑 

// Bresenham画线，伪代码(dy>dx,y为主方向)
// 初始化参数
increment1_[-1, 0] // 这个是x方向递增值，不需要考虑y
increment2_[0, 1] // 这个是y方向迭代，不需要递增x
dy = delta.y();
err = dy / 2;
dx = delta.x();
nCells_ = delta.y() + 1; // 统计直线占用格子数目
// Bresenham画线
err += dx;  // Increase the numerator by the top of the fraction.
// ++后x是否需要递增，需要通过Bresenham判断
if (err >= dy) {
    err -= dy;
    const Index unwrappedIndex = getIndexFromBufferIndex(index_, bufferSize_, bufferStartIndex_) + increment1_;
    index_ = getBufferIndexFromIndex(unwrappedIndex, bufferSize_, bufferStartIndex_);
}
// y为主方向，每次++需要递增y
const Index unwrappedIndex = getIndexFromBufferIndex(index_, bufferSize_, bufferStartIndex_) + increment2_;
index_ = getBufferIndexFromIndex(unwrappedIndex, bufferSize_, bufferStartIndex_);

2.5.7 SpiralIterator 

/*!
 * @param gridMap 栅格地图
 * @param center 圆心
 * @param radius 半径
 */
SpiralIterator(const grid_map::GridMap& gridMap, 
    Eigen::Vector2d center, const double radius);

选取指定圆内所有的cell栅格，与CircleIterator不同的是。迭代方式不是使用线型迭代，而是螺旋式迭代的方式。

（1）圆心位置坐标转换为gridmap中的Index，并将Index存入当前 环队列pointsRing_；

（2）每次迭代，从环队列pointsRing_中弹出最后一个数据；（相当于顺时针取元素） 若环队列pointsRing_非空，直接取最后一个元素；若环队列pointsRing_为空且不是最后一环，产生新的环；

（3）产生新的环，环半径distance_递增1。逆时针方向将距离圆心等于当前环半径distance_（去尾法截断转int类型）的cell索引存入环队列pointsRing_中。位于最后两层的环，对于每一个索引都要判断是否在圆内；

（4）当环队列元素pointsRing_为空且位于最后一层环时，迭代终止；

关键逻辑 

/*!
 * Uses the current distance to get the points of a ring
 * around the center.
 */
void SpiralIterator::generateRing() {
  distance_++;
  Index point(distance_, 0); // 从圆顶边点为初始点
  Index pointInMap;
  Index normal;
  do {
    pointInMap.x() = point.x() + indexCenter_.x();
    pointInMap.y() = point.y() + indexCenter_.y();
    if (checkIfIndexInRange(pointInMap, bufferSize_)) {
      if (distance_ == nRings_ || distance_ == nRings_ - 1) {
        if (isInside(pointInMap)) {
          pointsRing_.push_back(pointInMap);
        }
      } else {
        pointsRing_.push_back(pointInMap);
      }
    }
    // 确定旋转方向（逆时针）
    normal.x() = -signum(point.y());
    normal.y() = signum(point.x());
    // 先迭代x方向，若不再环内，再迭代y方向，若也不再环上，同时迭代x和y方向
    if (normal.x() != 0 && static_cast<unsigned int>(Vector(point.x() + normal.x(), point.y()).norm()) == distance_) {
      point.x() += normal.x();
    } else if (normal.y() != 0 && static_cast<unsigned int>(Vector(point.x(), point.y() + normal.y()).norm()) == distance_) {
      point.y() += normal.y();
    } else {
      point.x() += normal.x();
      point.y() += normal.y();
    }
  } while (static_cast<unsigned int>(point.x()) != distance_ || point.y() != 0);
}

2.5.8 SlidingWindowIterator 

/*!
 * @param gridMap[in] 栅格地图
 * @param layer[in] layer name
 * @param edgeHandling[in] edge handle mode
 * @param windowSize[in] window size
 */
SlidingWindowIterator(const GridMap& gridMap, const std::string& layer,
    const EdgeHandling& edgeHandling, const size_t windowSize);

滑窗迭代器SlidingWindowIterator，用于处理窗口数据。SlidingWindowIterator 通过在 GridMapIterator 的基础上添加窗口逻辑，实现了对窗口的迭代。这种设计模式允许 SlidingWindowIterator 复用 GridMapIterator 的基本迭代逻辑，同时加入额外的功能来处理窗口相关的逻辑。

处理边界滑窗四种模式

INSIDE：只在窗口完全位于地图内部时处理窗口

CROP：允许窗口在边缘处被裁剪，以适应地图边界

EMPTY：在窗口超出地图边界时用默认值（如NAN）填充

MEAN：用窗口内有限值的均值填充超出范围的部分

关键逻辑 

// 迭代递增，与GridMapIterator一致。INSIDE需要保证窗口完全在地图内
SlidingWindowIterator& SlidingWindowIterator::operator ++() {
  if (edgeHandling_ == EdgeHandling::INSIDE) {
    while (!isPastEnd()) {
      GridMapIterator::operator++();
      // 确保窗口完全在地图内
      if (dataInsideMap()) {
        break;
      }
    }
  } else {
    GridMapIterator::operator++();
  }
  return *this;
}
// 获取窗口矩阵
Matrix SlidingWindowIterator::getData() const {
  const Index centerIndex(*(*this));
  const Index windowMargin(Index::Constant(static_cast<int>(windowMargin_)));
  const Index originalTopLeftIndex(centerIndex - windowMargin);
  Index topLeftIndex(originalTopLeftIndex);
  boundIndexToRange(topLeftIndex, size_);
  Index bottomRightIndex(centerIndex + windowMargin);
  boundIndexToRange(bottomRightIndex, size_);
  const Size adjustedWindowSize(bottomRightIndex - topLeftIndex + Size::Ones());
  switch (edgeHandling_) {
    // INSIDE模式在"++"时，已经确保窗口完全在地图内
    case EdgeHandling::INSIDE:
    case EdgeHandling::CROP:
      return data_.block(topLeftIndex(0), topLeftIndex(1), adjustedWindowSize(0), adjustedWindowSize(1));
    case EdgeHandling::EMPTY:
    case EdgeHandling::MEAN:
      const Matrix data = data_.block(topLeftIndex(0), topLeftIndex(1), adjustedWindowSize(0), adjustedWindowSize(1));
      Matrix returnData(windowSize_, windowSize_);
      if (edgeHandling_ == EdgeHandling::EMPTY) {
        returnData.setConstant(NAN); // EMPTY模式设置为NAN
      } else if (edgeHandling_ == EdgeHandling::MEAN) {
        returnData.setConstant(data.meanOfFinites()); // MEAN模式设置为平均值
      }
      // 地图内部区域保留原始值
      const Index topLeftIndexShift(topLeftIndex - originalTopLeftIndex)






请到「今天看啥」查看全文


推荐文章
EETOP  ·  之前400人被传原地失业！国产GPU独角兽——象帝先彻底复活！
14 小时前
EETOP  ·  台积电2nm、Intel 18A 首次工艺对决
17 小时前
哎咆科技  ·  iPhone 壁纸 | 美拉德色系高清壁纸
昨天
EETOP  ·  数字IC面试题汇总
2 天前
EETOP  ·  宁波电子产业与前沿技术研讨会：通信|物联网|汽车电子|自动驾驶|5G/6G|产业发展【2月27日|宁波】
2 天前
达达兔  ·  你男朋友不把你公布在朋友圈的原因。。。
8 年前
孤读先生  ·  一位淘宝购物达人的匿名爆料！
7 年前
透明家  ·  5月二手房Top10揭示刚需新流向的四大要素
7 年前
河南新闻广播  ·  情系父亲节！河南新闻广播义诊送您健康！
7 年前
TMD大厂研究  ·  做生意，比“倒闭潮”更可怕的事 是“低价竞争”！
7 年前
Sov5搜索   ·   小百科   ·   今天看啥   ·   移动版
51好读 - 好文章就要读起来!