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

        大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。

---

做占用栅格地图的话，相对于ROS中采用的costmap_2d，本文介绍的grid_map的兼容性更强。（https://github.com/ethz-asl/grid_map）库的基本特性请自行查阅。本文重点记录我在读该库grid_map_core工程中GridMapMath文件时的理解。

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

 

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

      GridMapMath文件中，position是在世界坐标系，单位米（m)，bufferIndex是内存块范围内的cell坐标，即满足bufferIndexbufferIndex。indexVector变量都是用来描述变换关系的。

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

/*
 * GridMapMath.cpp
 *
 *  Created on: Dec 2, 2013
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, Autonomous Systems Lab
 */

#include "GridMap/GridMapMath.hpp"

// fabs

// Limits

using namespace std;

namespace positec {

namespace internal {

/*!
 * Gets the vector from the center of the map to the origin
 * of the map data structure.
 * @param[out] vectorToOrigin the vector from the center of the map the origin of the map data structure.
 * @param[in] mapLength the lengths in x and y direction.
 * @return true if successful.
 */
inline bool getVectorToOrigin(Vector& vectorToOrigin, const Length& mapLength)
{
  vectorToOrigin = (0.5 * mapLength).matrix();
    //vectorToOrigin是gridmap的中心点(m)
  return true;
}

/*!
 * Gets the vector from the center of the map to the center
 * of the first cell of the map data.
 * @param[out] vectorToFirstCell the vector from the center of the cell to the center of the map.
 * @param[in] mapLength the lengths in x and y direction.
 * @param[in] resolution the resolution of the map.
 * @return true if successful.
 */
inline bool getVectorToFirstCell(Vector& vectorToFirstCell,
                                 const Length& mapLength, const double& resolution)
{
  Vector vectorToOrigin;
  getVectorToOrigin(vectorToOrigin, mapLength);

  // Vector to center of cell.
  //vectorToFirstCell是从map center到左上角点cell中心的偏移(m)
    //该cell也是在内存中顺序存储的第一个cell，同样在内存块的左上角点
  vectorToFirstCell = (vectorToOrigin.array() - 0.5 * resolution).matrix();
  return true;
}

//图像坐标系到地图坐标系的转换，二者相差180度的旋转
inline Eigen::Matrix2i getBufferOrderToMapFrameTransformation()
{
  return -Eigen::Matrix2i::Identity();
}

inline Eigen::Matrix2i getMapFrameToBufferOrderTransformation()
{
    //单位矩阵转置后还是单位矩阵
  return getBufferOrderToMapFrameTransformation().transpose();
}

inline bool checkIfStartIndexAtDefaultPosition(const Index& bufferStartIndex)
{
    //没有移动
  return ((bufferStartIndex == 0).all());
}

inline Vector getIndexVectorFromIndex(
    const Index& index,
    const Size& bufferSize,
    const Index& bufferStartIndex)
{
  Index unwrappedIndex;
    //输入的index表示内存块中的坐标cell，index();
    //返回值是内存中index到以内存左上角为原点，X正向上，Y正向左的坐标系的转换关系
}

inline Index getIndexFromIndexVector(
    const Vector& indexVector,
    const Size& bufferSize,
    const Index& bufferStartIndex)
{
    //将内存块（以左上角为原点，X正向上，Y正向左，坐标轴方向与world系相同）内坐标系(cell)做180度旋转变换，
    // 变换后，内存块可以按图像坐标系理解，即X正向下，Y正向右。
    //此时，index表示了从world系（position，m）到内存（bufferIndex，cell）的转换
  Index index = (getMapFrameToBufferOrderTransformation() * indexVector.cast()).array();
  return getBufferIndexFromIndex(index, bufferSize, bufferStartIndex);
}

inline BufferRegion::Quadrant getQuadrant(const Index& index, const Index& bufferStartIndex)
{
  if (index[0] >= bufferStartIndex[0] && index[1] >= bufferStartIndex[1]) return BufferRegion::Quadrant::TopLeft;
  if (index[0] >= bufferStartIndex[0] && index[1] <  bufferStartIndex[1]) return BufferRegion::Quadrant::TopRight;
  if (index[0] <  bufferStartIndex[0] && index[1] >= bufferStartIndex[1]) return BufferRegion::Quadrant::BottomLeft;
  if (index[0] <  bufferStartIndex[0] && index[1] <  bufferStartIndex[1]) return BufferRegion::Quadrant::BottomRight;
  return BufferRegion::Quadrant::Undefined;
}

} // namespace

using namespace internal;


bool getPositionFromIndex(Position& position,
                          const Index& index,
                          const Length& mapLength,
                          const Position& mapPosition,
                          const double& resolution,
                          const Size& bufferSize,
                          const Index& bufferStartIndex)
{
    //既然index是和bufferSize作比较的，说明正常应该 index < bufferSize，
    //即index是以当前内存块的左上角（起始位置）开始计数的，是wrap后的
  if (!checkIfIndexInRange(index, bufferSize)) return false;
  Vector offset;
  getVectorToFirstCell(offset, mapLength, resolution);
  //offset是从map center到左上角的偏移(m)
    //getIndexVectorFromIndex()的返回值是index转换到以内存块左上角为原点，X正向上，Y正向左的坐标系中的坐标（cell）
    //上述坐标*分辨率+offset(m)，转换到map frame坐标系(m)，再+mapPosition，转换到world系(m)
  position = mapPosition + offset + resolution * getIndexVectorFromIndex(index, bufferSize, bufferStartIndex);
  return true;
}

bool getIndexFromPosition(Index& index,
                          const Position& position,
                          const Length& mapLength,
                          const Position& mapPosition,
                          const double& resolution,
                          const Size& bufferSize,
                          const Index& bufferStartIndex)
{
  Vector offset;
  getVectorToOrigin(offset, mapLength);
    //position是点在全局世界坐标系下的坐标（m），world系与map frame可以重合也可以不重合，但方向是一样的。
    //map frame的坐标原点就是map center，就是mapPosition（map center在world系下的坐标）
    //offset是从map frame到world系的偏移，即position=点在map frame下的坐标+offset
    //offset=mapPosition
    //position-mapPosition是把world系下的点转换到map frame下，保持坐标轴方向不变
    //在上一步基础上，（position-mapPosition）-offset是进一步转换到以map frame左上角的点为原点的坐标系，保持坐标轴方向不变
    //故，indexVector描述了从world系下的点坐标(m)转换到内存块（以左上角为原点，X正向上，Y正向左，坐标轴方向与world系相同）内坐标(cell)的转换
    //在getIndexFromIndexVector()中有对上述内存块坐标系的180度旋转变换，变换后，内存块可以按图像坐标系理解，即X正向下，Y正向右。
  Vector indexVector = ((position - offset - mapPosition).array() / resolution).matrix();
  index = getIndexFromIndexVector(indexVector, bufferSize, bufferStartIndex);
    //index是在内存块中wrap后的坐标(cell)，即index() * (position - mapPosition - offset);
    //positionTransformed是把world系下的点（m）转换到map frame左上角为原点，X正向下，Y正向右的坐标系中

  if (positionTransformed.x() >= 0.0 && positionTransformed.y() >= 0.0
      && positionTransformed.x() < mapLength(0) && positionTransformed.y() < mapLength(1)) {
    return true;
  }
  return false;
}

//@param[in] position: the position of the map.
void getPositionOfDataStructureOrigin(const Position& position,
                                      const Length& mapLength,
                                      Position& positionOfOrigin)
{
    //DataStructureOrigin是内存中存储的第一个cell，对应的是map frame中左上角的position点
    //输入的position是map center，即mapPosition(m)
  Vector vectorToOrigin;
  getVectorToOrigin(vectorToOrigin, mapLength);
    //vectorToOrigin是从map center到map中左上角点的偏移(m)
  positionOfOrigin = position + vectorToOrigin;
    //positionOfOrigin是在world系下(m)
}

bool getIndexShiftFromPositionShift(Index& indexShift,
                                    const Vector& positionShift,
                                    const double& resolution)
{
  Vector indexShiftVectorTemp = (positionShift.array() / resolution).matrix();
  Eigen::Vector2i indexShiftVector;

  for (int i = 0; i < indexShiftVector.size(); i++) {
    indexShiftVector[i] = static_cast(indexShiftVectorTemp[i] + 0.5 * (indexShiftVectorTemp[i] > 0 ? 1 : -1));
    //不懂为什么要在对应的方向上再+0.5？
  }
    //indexShiftVector与positionShift在同一个坐标系中，而这个坐标系与indexShift所在的内存坐标系差180度
  indexShift = (getMapFrameToBufferOrderTransformation() * indexShiftVector).array();
  return true;
}

bool getPositionShiftFromIndexShift(Vector& positionShift,
                                    const Index& indexShift,
                                    const double& resolution)
{
    //坐标系差180度
  positionShift = (getBufferOrderToMapFrameTransformation() * indexShift.matrix()).cast() * resolution;
  return true;
}

//index和bufferSize都对应内存
bool checkIfIndexInRange(const Index& index, const Size& bufferSize)
{
  if (index[0] >= 0 && index[1] >= 0 && index[0] < bufferSize[0] && index[1] < bufferSize[1])
  {
    return true;
  }
  return false;
}

void boundIndexToRange(Index& index, const Size& bufferSize)
{
  for (int i = 0; i < index.size(); i++) {
    boundIndexToRange(index[i], bufferSize[i]);
  }
}

void boundIndexToRange(int& index, const int& bufferSize)
{
  if (index < 0) index = 0;
  else if (index >= bufferSize) index = bufferSize - 1;
}

void wrapIndexToRange(Index& index, const Size& bufferSize)
{
  for (int i = 0; i < index.size(); i++) {
    wrapIndexToRange(index[i], bufferSize[i]);
  }
}

void wrapIndexToRange(int& index, const int& bufferSize)
{
  if (index < 0) index += ((-index / bufferSize) + 1) * bufferSize;
    //当坐标系以左上角为原点时，只有在bufferIndex(wrapped)->position(world)时，本函数中的index才会是<0的
    //这个index表示wrapped后的cell与bufferStartIndex的偏移，<0说明发生了wrap，>=0说明没有wrap
  index = index % bufferSize;
}

void boundPositionToRange(Position& position, const Length& mapLength, const Position& mapPosition)
{
  Vector vectorToOrigin;
  getVectorToOrigin(vectorToOrigin, mapLength);
  Position positionShifted = position - mapPosition + vectorToOrigin;
    //positionShifted是从world系到以map右下角为原点，坐标轴方向不变的坐标系的转换

  // We have to make sure to stay inside the map.
  for (int i = 0; i < positionShifted.size(); i++) {

    double epsilon = 10.0 * numeric_limits::epsilon(); // TODO Why is the factor 10 necessary.
      //epsilon():最小非零浮点数
    if (std::fabs(position(i)) > 1.0) epsilon *= std::fabs(position(i));

    if (positionShifted(i) <= 0) {
      positionShifted(i) = epsilon;
      continue;
    }
    if (positionShifted(i) >= mapLength(i)) {
      positionShifted(i) = mapLength(i) - epsilon;
      continue;
    }
  }

  position = positionShifted + mapPosition - vectorToOrigin;
}

const Eigen::Matrix2i getBufferOrderToMapFrameAlignment()
{
  return getBufferOrderToMapFrameTransformation().array().abs().matrix();
}

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();

    // 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() * 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& 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是=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是 indices)
{
//  for (int i = line.index_; col < line.endIndex(); col++) {
//    for (int i = 0; i < getSize()(0); i++) {
//
//    }
//  }
}

void getIndicesForRegions(const std::vector& regionIndeces, const Size& regionSizes,
                          std::vector indices)
{
}

bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3i& colorVector)
{
  colorVector(0) = (colorValue >> 16) & 0x0000ff;
  colorVector(1) = (colorValue >> 8) & 0x0000ff;
  colorVector(2) =  colorValue & 0x0000ff;
  return true;
}

bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3f& colorVector)
{
  Eigen::Vector3i tempColorVector;
  colorValueToVector(colorValue, tempColorVector);
  colorVector = ((tempColorVector.cast()).array() / 255.0).matrix();
  return true;
}

bool colorValueToVector(const float& colorValue, Eigen::Vector3f& colorVector)
{
  // cppcheck-suppress invalidPointerCast
  const unsigned long tempColorValue = *reinterpret_cast(&colorValue);
  colorValueToVector(tempColorValue, colorVector);
  return true;
}

bool colorVectorToValue(const Eigen::Vector3i& colorVector, unsigned long& colorValue)
{
  colorValue = ((int)colorVector(0)) << 16 | ((int)colorVector(1)) << 8 | ((int)colorVector(2));
  return true;
}

void colorVectorToValue(const Eigen::Vector3i& colorVector, float& colorValue)
{
  unsigned long color = (colorVector(0) << 16) + (colorVector(1) << 8) + colorVector(2);
  // cppcheck-suppress invalidPointerCast
  colorValue = *reinterpret_cast(&color);
}

void colorVectorToValue(const Eigen::Vector3f& colorVector, float& colorValue)
{
  Eigen::Vector3i tempColorVector = (colorVector * 255.0).cast();
  colorVectorToValue(tempColorVector, colorValue);
}

}  // namespace