高程圖 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 \tag{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\)不斷的變換，且在 \(0\leqslant 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_{\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;
}