.hpp文件_代码解析之grid_map库grid_map_core工程（一）——GridMapMath文件

做占用栅格地图的话，相对于ROS中采用的costmap_2d，本文介绍的grid_map库的兼容性更强。本文主要记录我对grid_map_core/GridMapMath.cpp的理解。

grid_map库​github.com

GridMapMath.cpp比较底层的描述了在世界坐标系中建图和在内存（二维环形缓冲区）中对应存储的变换关系。该文件中使用了4个坐标系：

• 世界坐标系（world，唯一固定，X轴正方向向上，Y轴正方向向左）
• 地图坐标系（map frame，坐标系原点即map center，其在world中的位置就是mapPosition，X正向上，Y正向左)
• 中间过程坐标系（原点为地图左上角的cell，这个cell存储在内存中的坐标就是bufferStartIndex，X正向上，Y正向左。经过180度旋转变换后，就可以与内存对应上）
• 内存坐标系（原点为内存块的左上角，即第一个存储的cell，可以近似理解为图像坐标系，X正向下，Y正向右）。

GridMapMath.cpp中，position是在世界坐标系，单位米（m)，bufferIndex是内存块范围内的cell坐标，即满足bufferIndex<bufferSize，index一般是指在内存坐标系中没有wrap的坐标（cell），此时假设内存坐标系的XY轴是无限延伸的。但现实是内存块是有限大小的，故index经wrap后就得到了bufferIndex。indexVector变量都是用来描述变换关系的。

偷个懒，把GridMapMath.cpp原代码和我的注释贴上来。

#include 

bool getSubmapInformation(Index& submapTopLeftIndex,
                          Size& submapBufferSize,
                          Position& submapPosition,
                          Length& submapLength,
                          Index& requestedIndexInSubmap,
                          const Position& requestedSubmapPosition,
                          const Length& requestedSubmapLength,
                          const Length& mapLength,
                          const Position& mapPosition,
                          const double& resolution,
                          const Size& bufferSize,
                          const Index& bufferStartIndex)
{
    // (Top left / bottom right corresponds to the position in the matrix,
    // not the map frame)
    Eigen::Matrix2d transform = getMapFrameToBufferOrderTransformation().cast<double>();
 
    // Corners of submap.
    Position topLeftPosition = requestedSubmapPosition - transform *
                               0.5 * requestedSubmapLength.matrix();
    boundPositionToRange(topLeftPosition, mapLength, mapPosition);
    if(!getIndexFromPosition(submapTopLeftIndex, topLeftPosition, mapLength,
        mapPosition, resolution, bufferSize, bufferStartIndex)) return false;
    Index topLeftIndex;
    topLeftIndex = getIndexFromBufferIndex(
      submapTopLeftIndex, bufferSize, bufferStartIndex);
 
    Position bottomRightPosition = requestedSubmapPosition +
      transform * 0.5 * requestedSubmapLength.matrix();
    boundPositionToRange(bottomRightPosition, mapLength, mapPosition);
    Index bottomRightIndex;
    if(!getIndexFromPosition(bottomRightIndex, bottomRightPosition,
      mapLength, mapPosition, resolution, bufferSize, bufferStartIndex))
      return false;
    bottomRightIndex = getIndexFromBufferIndex(bottomRightIndex, bufferSize, bufferStartIndex);
 
    // Get the position of the top left corner of the generated submap.
    Position topLeftCorner;
    if(!getPositionFromIndex(topLeftCorner, submapTopLeftIndex, mapLength, mapPosition, 
                             resolution, bufferSize, bufferStartIndex)) return false;
    topLeftCorner -= transform * Position::Constant(0.5 * resolution);
 
    // Size of submap.
    submapBufferSize = bottomRightIndex - topLeftIndex + Index::Ones();
 
    // Length of the submap.
    submapLength = submapBufferSize.cast<double>() * resolution;
 
    // Position of submap.
    Vector vectorToSubmapOrigin;
    getVectorToOrigin(vectorToSubmapOrigin, submapLength);
    submapPosition = topLeftCorner - vectorToSubmapOrigin;
 
    // Get the index of the cell which corresponds the requested
    // position of the submap.
    if(!getIndexFromPosition(requestedIndexInSubmap, requestedSubmapPosition, submapLength,
                             submapPosition, resolution, submapBufferSize)) return false;
 
    return true;
}
 
Size getSubmapSizeFromCornerIndeces(const Index& topLeftIndex, const Index& bottomRightIndex,
                                    const Size& bufferSize, const Index& bufferStartIndex)
{
  const Index unwrappedTopLeftIndex = 
                getIndexFromBufferIndex(topLeftIndex, bufferSize, bufferStartIndex);
  const Index unwrappedBottomRightIndex = 
                getIndexFromBufferIndex(bottomRightIndex, bufferSize, bufferStartIndex);
  return Size(unwrappedBottomRightIndex - unwrappedTopLeftIndex + Size::Ones());
}
 
bool getBufferRegionsForSubmap(std::vector<BufferRegion>& submapBufferRegions,
                               const Index& submapIndex,
                               const Size& submapBufferSize,
                               const Size& bufferSize,
                               const Index& bufferStartIndex)
{
  if ((getIndexFromBufferIndex(submapIndex, bufferSize, bufferStartIndex) +
                               submapBufferSize > bufferSize).any()) return false;
 
  submapBufferRegions.clear();
 
  Index bottomRightIndex = submapIndex + submapBufferSize - Index::Ones();
  wrapIndexToRange(bottomRightIndex, bufferSize);
 
  BufferRegion::Quadrant quadrantOfTopLeft = getQuadrant(submapIndex, bufferStartIndex);
  BufferRegion::Quadrant quadrantOfBottomRight = getQuadrant(bottomRightIndex, bufferStartIndex);
 
  if (quadrantOfTopLeft == BufferRegion::Quadrant::TopLeft) {
 
    if (quadrantOfBottomRight == BufferRegion::Quadrant::TopLeft) {
      submapBufferRegions.push_back(
        BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::TopLeft));
      return true;
    }
 
    if (quadrantOfBottomRight == BufferRegion::Quadrant::TopRight) {
      Size topLeftSize(submapBufferSize(0), bufferSize(1) - submapIndex(1));
      submapBufferRegions.push_back(
        BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft));
 
      Index topRightIndex(submapIndex(0), 0);
      Size topRightSize(submapBufferSize(0), submapBufferSize(1) - topLeftSize(1));
      submapBufferRegions.push_back(
        BufferRegion(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight));
      return true;
    }
 
    if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomLeft) {
      Size topLeftSize(bufferSize(0) - submapIndex(0), submapBufferSize(1));
      submapBufferRegions.push_back(
        BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft));
 
      Index bottomLeftIndex(0, submapIndex(1));
      Size bottomLeftSize(submapBufferSize(0) - topLeftSize(0), submapBufferSize(1));
      submapBufferRegions.push_back(
        BufferRegion(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft));
      return true;
    }
 
    if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
      Size topLeftSize(bufferSize(0) - submapIndex(0), bufferSize(1) - submapIndex(1));
      submapBufferRegions.push_back(
        BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft));
 
      Index topRightIndex(submapIndex(0), 0);
      Size topRightSize(bufferSize(0) - submapIndex(0), submapBufferSize(1) - topLeftSize(1));
      submapBufferRegions.push_back(
        BufferRegion(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight));
 
      Index bottomLeftIndex(0, submapIndex(1));
      Size bottomLeftSize(submapBufferSize(0) - topLeftSize(0), bufferSize(1) - submapIndex(1));
      submapBufferRegions.push_back(
        BufferRegion(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft));
 
      Index bottomRightIndex = Index::Zero();
      Size bottomRightSize(bottomLeftSize(0), topRightSize(1));
      submapBufferRegions.push_back(
        BufferRegion(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight));
      return true;
    }
 
  } else if (quadrantOfTopLeft == BufferRegion::Quadrant::TopRight) {
 
    if (quadrantOfBottomRight == BufferRegion::Quadrant::TopRight) {
      submapBufferRegions.push_back(
        BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::TopRight));
      return true;
    }
 
    if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
 
      Size topRightSize(bufferSize(0) - submapIndex(0), submapBufferSize(1));
      submapBufferRegions.push_back(
        BufferRegion(submapIndex, topRightSize, BufferRegion::Quadrant::TopRight));
 
      Index bottomRightIndex(0, submapIndex(1));
      Size bottomRightSize(submapBufferSize(0) - topRightSize(0), submapBufferSize(1));
      submapBufferRegions.push_back(
        BufferRegion(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight));
      return true;
    }
 
  } else if (quadrantOfTopLeft == BufferRegion::Quadrant::BottomLeft) {
 
    if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomLeft) {
      submapBufferRegions.push_back(
        BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::BottomLeft));
      return true;
    }
 
    if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
      Size bottomLeftSize(submapBufferSize(0), bufferSize(1) - submapIndex(1));
      submapBufferRegions.push_back(
        BufferRegion(submapIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft));
 
      Index bottomRightIndex(submapIndex(0), 0);
      Size bottomRightSize(submapBufferSize(0), submapBufferSize(1) - bottomLeftSize(1));
      submapBufferRegions.push_back(
        BufferRegion(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight));
      return true;
    }
 
  } else if (quadrantOfTopLeft == BufferRegion::Quadrant::BottomRight) {
 
    if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
      submapBufferRegions.push_back(
        BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::BottomRight));
      return true;
    } 
  } 
  return false;
}
 
bool incrementIndex(Index& index, const Size& bufferSize, const Index& bufferStartIndex)
{
  Index unwrappedIndex = getIndexFromBufferIndex(index, bufferSize, bufferStartIndex); 
  // Increment index.
  if (unwrappedIndex(1) + 1 < bufferSize(1)) {
    // Same row.
    unwrappedIndex[1]++;
  } else {
    // Next row.
    unwrappedIndex[0]++;
    unwrappedIndex[1] = 0;
  } 
  // End of iterations reached.
  if (!checkIfIndexInRange(unwrappedIndex, bufferSize)) return false; 
  // Return true iterated index.
  index = getBufferIndexFromIndex(unwrappedIndex, bufferSize, bufferStartIndex);
  return true;
}
 
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;
 
  // Increment submap index.
  if (tempSubmapIndex[1] + 1 < submapBufferSize[1]) {
    // Same row.
    tempSubmapIndex[1]++;
  } else {
    // Next row.
    tempSubmapIndex[0]++;
    tempSubmapIndex[1] = 0;
  }
 
  // End of iterations reached.
  if (!checkIfIndexInRange(tempSubmapIndex, submapBufferSize)) return false;
 
  // Get corresponding index in map.
  Index unwrappedSubmapTopLeftIndex = getIndexFromBufferIndex(
    submapTopLeftIndex, bufferSize, bufferStartIndex);
  tempIndex = getBufferIndexFromIndex(unwrappedSubmapTopLeftIndex + tempSubmapIndex,
                                      bufferSize, bufferStartIndex);
 
  // Copy data back.
  index = tempIndex;
  submapIndex = tempSubmapIndex;
  return true;
}
 
//bufferIndex是<bufferSize的，
Index getIndexFromBufferIndex(const Index& bufferIndex, const Size& bufferSize,
                              const Index& bufferStartIndex)
{
  if (checkIfStartIndexAtDefaultPosition(bufferStartIndex)) return bufferIndex;
 
  Index index = bufferIndex - bufferStartIndex;
  //这个index表示wrapped后的cell与bufferStartIndex的偏移，<0说明发生了wrap，>=0说明没有wrap
  //因为内存块内坐标系是以左上角为原点，X正向下，Y正向右，
  //正常情况cell的坐标应该>bufferStartIndex，所以<0说明发生了wrap
  wrapIndexToRange(index, bufferSize);
  return index;
}
 
//index是在map frame原点（=地图左上角）存储在内存的左上角的前提下数的，然而map frame原点在内存中的实际存储位置是bufferStartIndex，
//所以bufferIndex是index对应点（position对应点）在unwrapped内存（此时假设内存无限大，左上角为（0,0））中的坐标。
//然而内存块是被bufferSize限定了大小的，只看该内存块，其左上角仍然记为（0,0），正因如此，内存块放不下bufferIndex，发生了wrap，
//对应的数据就放在了wrap后的bufferIndex中。底层怎么拷贝移动数据的现在还不懂？
Index getBufferIndexFromIndex(const Index& index, const Size& bufferSize,
                              const Index& bufferStartIndex)
{
  if (checkIfStartIndexAtDefaultPosition(bufferStartIndex)) return index;
 
  Index bufferIndex = index + bufferStartIndex;
  wrapIndexToRange(bufferIndex, bufferSize);
    //bufferIndex是<bufferSize的
  return bufferIndex;
}