高程图 GridMap

/**
 * @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;
}

initialize(gridMap, start, end);

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

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

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

/**
 * @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;
}