大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。
做占用栅格地图的话，相对于ROS中采用的costmap_2d，本文介绍的grid_map的兼容性更强。（https://github.com/ethz-asl/grid_map）库的基本特性请自行查阅。本文重点记录我在读该库grid_map_core工程中GridMapMath文件时的理解。
GridMapMath文件比较底层的描述了在世界坐标系中建图和在内存（二维环形缓冲区）中对应存储的变换关系。该文件中使用了4个坐标系：
- 世界坐标系（world，唯一固定，X轴正方向向上，Y轴正方向向左）
- 地图坐标系（map frame，坐标系原点即map center，其在world中的位置就是mapPosition，X正向上，Y正向左)
- 中间过程坐标系（原点为地图左上角的cell，这个cell存储在内存中的坐标就是bufferStartIndex，X正向上，Y正向左。经过180度旋转变换后，就可以与内存对应上）
- 内存坐标系（原点为内存块的左上角，即第一个存储的cell，可以近似理解为图像坐标系，X正向下，Y正向右）。
GridMapMath文件中，position是在世界坐标系，单位米（m)，bufferIndex是内存块范围内的cell坐标，即满足bufferIndex<bufferSize，index一般是指在内存坐标系中没有wrap的坐标（cell），此时假设内存坐标系的XY轴是无限延伸的。但现实是内存块是有限大小的，故index经wrap后就得到了bufferIndex。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<bufferSize
unwrappedIndex = getIndexFromBufferIndex(index, bufferSize, bufferStartIndex);
//unwrappedIndex是没有wrap的bufferStartIndex=（0,0）情况下的坐标（cell）
//即，unwrappedIndex是cell到内存坐标系原点（左上角，X正向下，Y正向右）的偏移（cell）
return (getBufferOrderToMapFrameTransformation() * unwrappedIndex.matrix()).cast<double>();
//返回值是内存中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<int>()).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<bufferSize
if (!checkIfPositionWithinMap(position, mapLength, mapPosition)) return false;
return true;
}
bool checkIfPositionWithinMap(const Position& position,
const Length& mapLength,
const Position& mapPosition)
{
Vector offset;
getVectorToOrigin(offset, mapLength);
Position positionTransformed = getMapFrameToBufferOrderTransformation().cast<double>() * (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<int>(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<double>() * 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<double>::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<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;
}
size_t getLinearIndexFromIndex(const Index& index, const Size& bufferSize, const bool rowMajor)
{
if (!rowMajor) return index(1) * bufferSize(0) + index(0);
return index(0) * bufferSize(1) + index(1);
}
Index getIndexFromLinearIndex(const size_t linearIndex, const Size& bufferSize, const bool rowMajor)
{
if (!rowMajor) return Index((int)linearIndex % bufferSize(0), (int)linearIndex / bufferSize(0));
return Index((int)linearIndex / bufferSize(1), (int)linearIndex % bufferSize(1));
}
void getIndicesForRegion(const Index& regionIndex, const Size& regionSize,
std::vector<Index> indices)
{
// for (int i = line.index_; col < line.endIndex(); col++) {
// for (int i = 0; i < getSize()(0); i++) {
//
// }
// }
}
void getIndicesForRegions(const std::vector<Index>& regionIndeces, const Size& regionSizes,
std::vector<Index> 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<float>()).array() / 255.0).matrix();
return true;
}
bool colorValueToVector(const float& colorValue, Eigen::Vector3f& colorVector)
{
// cppcheck-suppress invalidPointerCast
const unsigned long tempColorValue = *reinterpret_cast<const unsigned long*>(&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<float*>(&color);
}
void colorVectorToValue(const Eigen::Vector3f& colorVector, float& colorValue)
{
Eigen::Vector3i tempColorVector = (colorVector * 255.0).cast<int>();
colorVectorToValue(tempColorVector, colorValue);
}
} // namespace
4304
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
