高程图 GridMap

Grid_map总结

  Gridmap是由苏黎世理工自动驾驶实验室ASL发布的一款地图，Grid_map也是一种高程图，和Octomap等空间占有地图有着明显的区别。其底层的存储和运算使用Eigen。

1 地图的构造、存储和索引

  Grid_map是由多层layer组成的一种复合地图，每一层地图可以表达不同的信息，不同的layer由各自的属性的cell组成，但是所有的cell对应的空间(2D)坐标是统一的。例如A层layer表达地图每个cell在空间中的高度，B层layer表达地图每个cell的颜色，C层layer表达每个cell的可遍历性等等。其多层特性，可以用图1来描述。

图1 Grid Map模型空间结构图 对于多层地图可以简单理解为，cell的位置是固定不变的，只是每一层地图描述了cell的不同特性。   单层地图的构造。单层地图由许多个cell组成，一般可以认为单层地图有两个坐标系：位置坐标系(以m为单位)和索引坐标系(以cell为单位)。在地图构造完成后，会有索引坐标系到位置坐标系的转换关系，所以在定位某个cell的时候可以使用仁义坐标系，不存在本质性的区别。单张地图的构造，如图2所示。  图2 单层地图 图2中包含了单层地图的构造形式，以及一些API接口函数。在图1的左上角和右下角也可以清楚地看到地图的连个坐标系。左上角的是索引坐标系，以左上角的cell为原点，一个cell为单位长度，描述了地图中所有cell的二维vector的索引。右下角的地图是位置坐标系，以地图的距离中心为原点，以单个cell的长度 $l$ 为单位长度，每个cell的位置坐标是其中心的位置。   具体的代码实现。因为Grid_map的最底层使用的是Eigen，所以在数据存储和基本的运算方面都可以使用库函数。 - 地图存储 ```c++ std::unordered_map data_; ``` 地图在存储过程中，按照``形式存储，每一层layer对应一个矩阵，然后将其以c++ unordered_map容器存储，在具体的存储过程中使用到了hash结构。 - 地图初始化   地图初始化的过程中，主要要初始化四个量：layer名称、地图的长宽、分辨率(每个cell的大小)以及位置坐标系的原点。 - cell单元的索引   地图中的cell单元是地图最基本的元素，对地图的操作就是对cell单元的操作。而对cell单元操作的关键在于对cell单元的索引。Grid_map索引的方式有两种，分别是采用位置(Position)和索引ID(Index)。两种方式的效果是一样的，使用的时候主要看知道什么或者想要得到什么。所以在Index和Position之间就有相应的对应关系。 - Index—Position $$ P = P_{map}+P_{off}-(I_{index}+0.5)* r ag{1} $$   其中 $P_{map}$ 为位置坐标系原点的偏移，$P_{off}=1/2 L_{map}$ ，其中$L_{map}$ 为地图的长宽大小。$I_{index}$ 位cell单元的索引，$r$ 为地图的分辨率。 __Position—Index的转换反之即可。__

  Grid_map不是Global map，其属于一种局部的可以移动的地图，在机器人运动的过程中，地图会随着机器人而发生整体移动，如地图是以机器人为中心，长宽各为 (L)的局部地图。如图3所示。

图3 地图移动示意图

2 Gridmap中的迭代器

  为了方便地图中cell单元的遍历，作者在Grid_map中加入了7个迭代器，涉及到的内容主要是计算机图形学方面的内容。

2.1 Grid map迭代器

  Grid map迭代器是地图中最常见的一个迭代器，其迭代原理和大多数二维地图的迭代原理类似，比较简单。其原理可以描述为：
  设地图大小(Cell个数)为 (m*n)，其乘积也为所有的cell数目。设定一个起始的一维索引值 (l)，在迭代的过程中 (l)不断的变换，且在 (0leqslant l leqslant m*n) 范围内有且对应一个cell单元。那么该cell单元在在索引坐标系下对应的二维索引坐标为 ((l/n,l \% n )) Grid map迭代器的迭代效果，如图4所示。

图4 Gridmap迭代器示意图 #####2.2 Submap迭代器   子地图迭代器的构造。子地图迭代器在构造的过程中只需要给定子地图的 __起始索引__ 以及 __子地图的大小__(行列个数)即可。   Submap迭代器的迭代过程。Submap迭代器是Gridmap地图中的一个局部地图中cell单元的迭代形式。Submap在叠加的时候也是一个简单的累加的过程，原理比较简单，需要分清楚子地图的索引中心 $o$和父地图的索引中心$O$。最终的索引就是 $o$到$O$的cell个数，加上子地图 $Index$的索引叠加值(初值为0)。具体的程序如下： ```c++ /** * @function [incrementIndexForSubmap] * @description [在子地图迭代的时候，递增索引值] * @param submapIndex [子地图索引值，初始值为0] * @param index [父地图下，子地图的索引中心] * @param submapTopLeftIndex [子地图起始索引在父地图下的索引] * @param submapBufferSize [子地图的大小，一般指cell的个数] * @param bufferSize [实际地图大小] * @param bufferStartIndex [实际地图的起始索引值] * @return */ bool incrementIndexForSubmap(Index& submapIndex, Index& index, const Index& submapTopLeftIndex, const Size& submapBufferSize, const Size& bufferSize, const Index& bufferStartIndex) { // Copy the data first, only copy it back if everything is within range. Index tempIndex = index; Index tempSubmapIndex = submapIndex; // 1. 增加索引值 // 索引的时候是按照y轴方向增加的 // Increment submap index. if (tempSubmapIndex[1] + 1 < submapBufferSize[1]) { // Same row. tempSubmapIndex[1]++; } else { // Next row. tempSubmapIndex[0]++; tempSubmapIndex[1] = 0; } // 2. 判断增加后的索引值是否还满足要求（是否超过子地图范围） // End of iterations reached. if (!checkIfIndexWithinRange(tempSubmapIndex, submapBufferSize)) return false; // Get corresponding index in map. // 3. 计算子地图索引中心距离父地图索引中心的cell个数 Index unwrappedSubmapTopLeftIndex = getIndexFromBufferIndex(submapTopLeftIndex, bufferSize, bufferStartIndex); // 4. 由3中计算出来的cell个数 + 1中计算出来的cell个数 = 当前迭代后位置距父地图索引中心的cell个数，然后在计算索引值即可 tempIndex = getBufferIndexFromIndex(unwrappedSubmapTopLeftIndex + tempSubmapIndex, bufferSize, bufferStartIndex); // Copy data back. index = tempIndex; submapIndex = tempSubmapIndex; return true; } ``` #####2.3 Circle 迭代器   Circle迭代器的构造。圆形迭代器构造会指定 __圆心__、__半径__。接着构建圆形子地图的外界矩形，再将其左上角cell和右下角cell限制在父地图范围之内后，将左上角的cell作为索引起始点。   Circle迭代器的迭代过程。再将圆形转换为其外接矩形之后，Circle map的叠加过程和Submap的叠加过程相同，不过在叠加完成之后要判断叠加后的cell是否还处在圆形地图之内。 #####2.4 Line 迭代器   线性迭代器是指子地图区域近似于一条直线。为了加快迭代速度，迭代器是沿着直线方向进行更新的或者递增的。   线性迭代器的构造，线性迭代器的构造只需要给性父地图名称以及起始和终止cell的索引即可。在构造过程中，特别的要判断直线迭代器的起始点是否在地图之内，如果不在，则将起始点沿着直线的方向平移一个cell，直至其返回地图当中。不用判断其终止点，因为终止点Gridmap父地图会做限制。 ```c++ initialize(gridMap, start, end); ```   线性迭代器的迭代的过程中使用了Bresenham画线算法进行索引值的叠加，具体的算法思想可以参考[wiki](https://zh.wikipedia.org/wiki/%E5%B8%83%E9%9B%B7%E6%A3%AE%E6%BC%A2%E5%A7%86%E7%9B%B4%E7%B7%9A%E6%BC%94%E7%AE%97%E6%B3%95)或者[文章](https://www.cs.helsinki.fi/group/goa/mallinnus/lines/bresenh.html)，其中代码和wiki中的最佳化方法比较一致。下面对该算法做简单总结。   __Bresenham画线算法.__ 如图5所示，假设图中的单元格是屏幕的像素点或者地图中的cell单元，那么我们在取点的时候只能去整数部分，即图中的蓝色点，而红色的线是标准的线段。我们要做的就是根据红线的起始点和终止点将这些蓝色的点求取，以此来描绘红色的线。要描绘蓝色的点也很容易，让 $x+1$，然后决定取上一个点($d_{1}$)的右上角点($d_{u}$)还是右侧点($d_{2}$)。判断条件很简单，红色线的实际值更靠近哪个点就选哪个点。  图5 Bresenham画线算法示意图

  为了使算法适应不同直线，减少浮点运算，将算法的核心思想做进一步的改进之后，伪代码如下。如需进一步了解该算法可以参考上面的wiki。

function line(x0, x1, y0, y1)
    boolean steep := abs(y1 - y0) > abs(x1 - x0)
    if steep then
        swap(x0, y0)
        swap(x1, y1)
    if x0 > x1 then
        swap(x0, x1)
        swap(y0, y1)
    int deltax := x1 - x0
    int deltay := abs(y1 - y0)
    int error := deltax / 2
    int ystep
    int y := y0
    if y0 < y1 then ystep := 1 else ystep := -1
    for x from x0 to x1
        if steep then plot(y,x) else plot(x,y)
        error := error - deltay
        if error < 0 then
            y := y + ystep
            error := error + deltax

2.5 Polygon(多边形) 迭代器

  多边形迭代器是Gridmap迭代器中最复杂的一个迭代器。多边形迭代器在构造的过程中需要提前设定多边形的各个顶点，其中顶点的坐标以Position坐标系表示。
  下面主要对多边形迭代器的迭代过程做总结。
2.5.1 获取起始索引和索引区
  对起始索引点和索引区的获取还是老思路，即求取多边形 (n) 个顶点中，(x) 方向和 (y) 方向的最大值和最小值，然后将两个轴的最大值作为左上角点，即起始点，两个轴向的最小值作为右下角点，即终止点(在Position坐标系下)，相当于做了一个外接矩形。
2.5.2 判断点是否在多边形内
  点与多边形的关系有内部、外部和多边形上，判断方法也有很多种。
   引射线法，即由点向多边形的左侧引射线，如果射线与多边形的交点个数为奇数，则点在多边形内或多边形上，反之点在多边形外。或者将其理解为光源照射后与梯形区域的关系，都是一样的原理。如图6所示。

图6 点与多边形的关系

   面积法。若点在多边形上或者点在多边形内部，那么点与多边形各个边构成的三角形的面积之和是等于多边形面积，反之在外部则不相等。此种方法会因为计算精度会带来一定的误差。关于多边形面积求取，多边形的面积等于由组成多边形三角形的面积之和。可用式(2)取

[S_{Omega }=sum_{k=1}^{infty }S_{ riangle op_{k}p_{k+1}}=frac{1}{2}sum_{k=1}^{infty }(x_{k}y_{k+1}-x_{k+1}y_{k}) ag{2} ]

  关于点与多边形关系判断的其他方法可以参考博客。
2.5.3 求取多边形的质心
  多边形质心在求取，可以参考wiki中多边形部分。利用式(3)求取。

[egin{align} onumber C_{x}&=frac{1}{6A}sum_{n-1}^{i=0}(x_{i}+x_{i+1})(x_{i}y_{i+1}-x_{i+1}y_{i}) \ onumber C_{y}&=frac{1}{6A}sum_{n-1}^{i=0}(y_{i}+y_{i+1})(x_{i}y_{i+1}-x_{i+1}y_{i}) \ onumber A&=frac{1}{2}sum_{n-1}^{i=0}(x_{i}y_{i+1}-x_{i+1}y_{i}) ag{3} end{align} ]

2.5.3 多边形的缩放
  如图7所示，多边形的缩放可以看做是将多边形的顶点向内做一个偏移 (s) 而多边形的形状不发生变化，所以最直接的方法就是做多边形任意两条边的平行线，平行线之之间的距离是 (s)，两条平行线的交点就是新的顶点。新的顶点求取方法也比较简单，如图7所示。

[egin{align} onumber Q_{i}&=P_{i}+(V_{1}+V_{2}) \ onumber Q_{i}&=P_{i}+norm(V_{2})(V_{1}.norm+V_{2},norm)\ onumber Q_{i}&=P_{i}+ frac{s}{sin( heta )}(V_{1}.norm+V_{2},norm) end{align} ]

其中， $ heta $为两条边的夹角。

2.6 Ellipse(椭圆) 迭代器

  椭圆迭代器也比较简单，其构造需要提供椭圆的圆心，长短轴和椭圆的旋转角度等变量。

EllipseIterator::EllipseIterator(const GridMap& gridMap, const Position& center, const Length& length, const double rotation)

  椭圆迭代器的迭代过程。椭圆迭代器是Submap迭代器的继承，所以仅需要找到起始和终止迭代点，然后按照子地图的迭代方式迭代即可，只不过在迭代过程中需要判断index对应的cell是否在地图之内。具体的过程可以描述为：

1).求取旋转之后的长短轴。
2).以圆形为中心，长短轴平方和的根作为长，求取椭圆的外接矩形，主要是求取外接矩形的左上角点和右下角点。
3).将矩形的左上角点作为起始点，右下角点作为终止点。
4).按子地图迭代方式，迭代地图，并判断cell是否在椭圆内。判断的方法也比较简单，即 (frac{x^{2}}{a_{2}}+frac{y^{2}}{b_{2}}leq 1)其中 ((x,y)) 是cell单元相对于圆心的坐标。

2.7 Spiral(螺旋) 迭代器

&esmp; 由螺旋迭代器可以看出，螺旋迭代器是圆形子地图除了线性迭代的另一种迭代方式。在迭代器初始化的时候需要迭代器所属的父地图、子地图的圆形和半径等变量。

SpiralIterator::SpiralIterator(const grid_map::GridMap& gridMap, const Eigen::Vector2d& center, const double radius)

  螺旋迭代器的迭代过程。螺旋迭代器的迭代原理比较简单，应该可以算是参加工作笔试编程题的难度吧！！！！具体过程可以描述为

1. 以子地图圆心为中心，(r) 为半径，安顺时针方向取cell单元存放到 vector 中。
2. 每迭代器一次，就从vector中弹一次，若vector为空了，执行1)即可。

所以整个螺旋迭代器的原理还是比较简单的，迭代的代码如下：

/**
 * @function        [generateRing]
 * @description     [获取距圆心距离为distance的环上的点
 *                  按照顺时针的顺序，绕圆心，以distance_为半径，求取圆上的点存放到pointsRing_中]
 *
 *
 */
void SpiralIterator::generateRing()
{
    distance_++;
    Index point(distance_, 0);
    Index pointInMap;
    Index normal;
    do
    {
        // 1.增加mappoint点
        pointInMap.x() = point.x() + indexCenter_.x();
        pointInMap.y() = point.y() + indexCenter_.y();
        // 判断增加了坐标值的点是否还在map内
        if (checkIfIndexWithinRange(pointInMap, bufferSize_))
        {
            // 当距离值等于或者接近半径时，要判断该点是否还在圆内
            if (distance_ == nRings_ || distance_ == nRings_ - 1)
            {
                if (isInside(pointInMap))
                    pointsRing_.push_back(pointInMap);
            }
            else
            {
                pointsRing_.push_back(pointInMap);
            }
        }
        // 2. 螺旋式递增算法
        // 2.1 获取下一个迭代点的方向：0，不动  1，向右或者向下  -1，向左或者向下
        normal.x() = -signum(point.y());
        normal.y() = signum(point.x());

        // 2.1 在x轴上判断，移动后的点是否满足要求
        if (normal.x() != 0
            && (int) Vector(point.x() + normal.x(), point.y()).norm() == distance_)
            point.x() += normal.x();
        // 2.2 在y轴上判断，移动后的点是否满足要求
        else if (normal.y() != 0
            && (int) Vector(point.x(), point.y() + normal.y()).norm() == distance_)
            point.y() += normal.y();
        // 2.3 如果上面两个条件都不满足，则在两个轴上同时移动
        else
        {
            point.x() += normal.x();
            point.y() += normal.y();
        }
    // 当且仅当point.x()等于当前距离且y的值等于0的时候跳出
    } while (point.x() != distance_ || point.y() != 0);
}

const Eigen::Array2i& SpiralIterator::operator *() const
{
  return pointsRing_.back();
}

/**
 * @function        [++]
 * @description     [迭代重载运算符]
 * @return
 ****/
SpiralIterator& SpiralIterator::operator ++()
{
    // 先把最后一个index给删了，相当于return一次弹一次
    pointsRing_.pop_back();
    // 当这个环空的时候，再增加环
    if (pointsRing_.empty() && !isPastEnd())
        generateRing();
    return * this;
}