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来描述。
对于多层地图可以简单理解为，cell的位置是固定不变的，只是每一层地图描述了cell的不同特性。
单层地图的构造。单层地图由许多个cell组成，一般可以认为单层地图有两个坐标系：位置坐标系(以m为单位)和索引坐标系(以cell为单位)。在地图构造完成后，会有索引坐标系到位置坐标系的转换关系，所以在定位某个cell的时候可以使用仁义坐标系，不存在本质性的区别。单张地图的构造，如图2所示。
图2中包含了单层地图的构造形式，以及一些API接口函数。在图1的左上角和右下角也可以清楚地看到地图的连个坐标系。左上角的是索引坐标系，以左上角的cell为原点，一个cell为单位长度，描述了地图中所有cell的二维vector的索引。右下角的地图是位置坐标系，以地图的距离中心为原点，以单个cell的长度 \(l\) 为单位长度，每个cell的位置坐标是其中心的位置。
具体的代码实现。因为Grid_map的最底层使用的是Eigen，所以在数据存储和基本的运算方面都可以使用库函数。
- 地图存储
std::unordered_map data_; 地图在存储过程中，按照
- 地图初始化
地图初始化的过程中，主要要初始化四个量：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 \tag{1} \]
其中 \(P_{map}\) 为位置坐标系原点的偏移，\(P_{off}=1/2 L_{map}\) ，其中\(L_{map}\) 为地图的长宽大小。\(I_{index}\) 位cell单元的索引，\(r\) 为地图的分辨率。 Position—Index的转换反之即可。
- Index—Position
Grid_map不是Global map，其属于一种局部的可以移动的地图，在机器人运动的过程中，地图会随着机器人而发生整体移动，如地图是以机器人为中心，长宽各为 \(L\)的局部地图。如图3所示。
2 Gridmap中的迭代器
为了方便地图中cell单元的遍历，作者在Grid_map中加入了7个迭代器，涉及到的内容主要是计算机图形学方面的内容。
2.1 Grid map迭代器
Grid map迭代器是地图中最常见的一个迭代器，其迭代原理和大多数二维地图的迭代原理类似，比较简单。其原理可以描述为：
设地图大小(Cell个数)为 \(m*n\)，其乘积也为所有的cell数目。设定一个起始的一维索引值 \(l\)，在迭代的过程中 \(l\)不断的变换，且在 \(0\leqslant l \leqslant m*n\) 范围内有且对应一个cell单元。那么该cell单元在在索引坐标系下对应的二维索引坐标为 \((l/n,l \% n )\) Grid map迭代器的迭代效果，如图4所示。
2.2 Submap迭代器
子地图迭代器的构造。子地图迭代器在构造的过程中只需要给定子地图的 起始索引 以及 子地图的大小(行列个数)即可。
Submap迭代器的迭代过程。Submap迭代器是Gridmap地图中的一个局部地图中cell单元的迭代形式。Submap在叠加的时候也是一个简单的累加的过程，原理比较简单，需要分清楚子地图的索引中心 \(o\)和父地图的索引中心\(O\)。最终的索引就是 \(o\)到\(O\)的cell个数，加上子地图 \(Index\)的索引叠加值(初值为0)。具体的程序如下：
/**
* @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父地图会做限制。
initialize(gridMap, start, end); 线性迭代器的迭代的过程中使用了Bresenham画线算法进行索引值的叠加，具体的算法思想可以参考wiki或者文章，其中代码和wiki中的最佳化方法比较一致。下面对该算法做简单总结。
Bresenham画线算法. 如图5所示，假设图中的单元格是屏幕的像素点或者地图中的cell单元，那么我们在取点的时候只能去整数部分，即图中的蓝色点，而红色的线是标准的线段。我们要做的就是根据红线的起始点和终止点将这些蓝色的点求取，以此来描绘红色的线。要描绘蓝色的点也很容易，让 \(x+1\)，然后决定取上一个点(\(d_{1}\))的右上角点(\(d_{u}\))还是右侧点(\(d_{2}\))。判断条件很简单，红色线的实际值更靠近哪个点就选哪个点。
为了使算法适应不同直线，减少浮点运算，将算法的核心思想做进一步的改进之后，伪代码如下。如需进一步了解该算法可以参考上面的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 + deltax2.5 Polygon(多边形) 迭代器
多边形迭代器是Gridmap迭代器中最复杂的一个迭代器。多边形迭代器在构造的过程中需要提前设定多边形的各个顶点，其中顶点的坐标以Position坐标系表示。
下面主要对多边形迭代器的迭代过程做总结。
2.5.1 获取起始索引和索引区
对起始索引点和索引区的获取还是老思路，即求取多边形 \(n\) 个顶点中，\(x\) 方向和 \(y\) 方向的最大值和最小值，然后将两个轴的最大值作为左上角点，即起始点，两个轴向的最小值作为右下角点，即终止点(在Position坐标系下)，相当于做了一个外接矩形。
2.5.2 判断点是否在多边形内
点与多边形的关系有内部、外部和多边形上，判断方法也有很多种。
引射线法，即由点向多边形的左侧引射线，如果射线与多边形的交点个数为奇数，则点在多边形内或多边形上，反之点在多边形外。或者将其理解为光源照射后与梯形区域的关系，都是一样的原理。如图6所示。
面积法。若点在多边形上或者点在多边形内部，那么点与多边形各个边构成的三角形的面积之和是等于多边形面积，反之在外部则不相等。此种方法会因为计算精度会带来一定的误差。关于多边形面积求取，多边形的面积等于由组成多边形三角形的面积之和。可用式(2)取
\[ S_{\Omega }=\sum_{k=1}^{\infty }S_{\triangle op_{k}p_{k+1}}=\frac{1}{2}\sum_{k=1}^{\infty }(x_{k}y_{k+1}-x_{k+1}y_{k}) \tag{2} \]
关于点与多边形关系判断的其他方法可以参考博客。
2.5.3 求取多边形的质心
多边形质心在求取，可以参考wiki中多边形部分。利用式(3)求取。
\[ \begin{align}\nonumber 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}) \\\nonumber 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}) \\\nonumber A&=\frac{1}{2}\sum_{n-1}^{i=0}(x_{i}y_{i+1}-x_{i+1}y_{i}) \tag{3} \end{align} \]
2.5.3 多边形的缩放
如图7所示，多边形的缩放可以看做是将多边形的顶点向内做一个偏移 \(s\) 而多边形的形状不发生变化，所以最直接的方法就是做多边形任意两条边的平行线，平行线之之间的距离是 \(s\)，两条平行线的交点就是新的顶点。新的顶点求取方法也比较简单，如图7所示。
\[ \begin{align}\nonumber Q_{i}&=P_{i}+(V_{1}+V_{2}) \\\nonumber Q_{i}&=P_{i}+norm(V_{2})(V_{1}.norm+V_{2},norm)\\\nonumber Q_{i}&=P_{i}+ \frac{s}{sin(\theta )}(V_{1}.norm+V_{2},norm) \end{align} \]
其中， $\theta $为两条边的夹角。
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;
}