601 lines
21 KiB
C++
601 lines
21 KiB
C++
/*
|
|
* GridMapMath.cpp
|
|
*
|
|
* Created on: Dec 2, 2013
|
|
* Author: Péter Fankhauser
|
|
* Institute: ETH Zurich, ANYbotics
|
|
*/
|
|
|
|
#include "robot_grid_map_core/GridMapMath.hpp"
|
|
|
|
// fabs
|
|
#include <cmath>
|
|
|
|
// Limits
|
|
#include <limits>
|
|
|
|
using std::numeric_limits;
|
|
|
|
namespace grid_map {
|
|
|
|
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();
|
|
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 = (vectorToOrigin.array() - 0.5 * resolution).matrix();
|
|
return true;
|
|
}
|
|
|
|
inline Eigen::Matrix2i getBufferOrderToMapFrameTransformation()
|
|
{
|
|
return -Eigen::Matrix2i::Identity();
|
|
}
|
|
|
|
inline Vector transformBufferOrderToMapFrame(const Index& index) {
|
|
return {-index[0], -index[1]};
|
|
}
|
|
|
|
inline Eigen::Matrix2i getMapFrameToBufferOrderTransformation()
|
|
{
|
|
return getBufferOrderToMapFrameTransformation().transpose();
|
|
}
|
|
|
|
inline Index transformMapFrameToBufferOrder(const Vector& vector) {
|
|
return {-vector[0], -vector[1]};
|
|
}
|
|
|
|
inline Index transformMapFrameToBufferOrder(const Eigen::Vector2i& vector) {
|
|
return {-vector[0], -vector[1]};
|
|
}
|
|
|
|
inline bool checkIfStartIndexAtDefaultPosition(const Index& bufferStartIndex)
|
|
{
|
|
return ((bufferStartIndex == 0).all());
|
|
}
|
|
|
|
inline Vector getIndexVectorFromIndex(
|
|
const Index& index,
|
|
const Size& bufferSize,
|
|
const Index& bufferStartIndex)
|
|
{
|
|
Index unwrappedIndex;
|
|
unwrappedIndex = getIndexFromBufferIndex(index, bufferSize, bufferStartIndex);
|
|
return transformBufferOrderToMapFrame(unwrappedIndex);
|
|
}
|
|
|
|
inline Index getIndexFromIndexVector(
|
|
const Vector& indexVector,
|
|
const Size& bufferSize,
|
|
const Index& bufferStartIndex)
|
|
{
|
|
Index index = transformMapFrameToBufferOrder(indexVector);
|
|
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 internal
|
|
|
|
using internal::checkIfStartIndexAtDefaultPosition;
|
|
using internal::getBufferOrderToMapFrameTransformation;
|
|
using internal::getIndexFromIndexVector;
|
|
using internal::getIndexVectorFromIndex;
|
|
using internal::getMapFrameToBufferOrderTransformation;
|
|
using internal::getQuadrant;
|
|
using internal::getVectorToFirstCell;
|
|
using internal::getVectorToOrigin;
|
|
using internal::transformBufferOrderToMapFrame;
|
|
using internal::transformMapFrameToBufferOrder;
|
|
|
|
bool getPositionFromIndex(Position& position,
|
|
const Index& index,
|
|
const Length& mapLength,
|
|
const Position& mapPosition,
|
|
const double& resolution,
|
|
const Size& bufferSize,
|
|
const Index& bufferStartIndex)
|
|
{
|
|
if (!checkIfIndexInRange(index, bufferSize)) {
|
|
return false;
|
|
}
|
|
Vector offset;
|
|
getVectorToFirstCell(offset, mapLength, resolution);
|
|
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);
|
|
Vector indexVector = ((position - offset - mapPosition).array() / resolution).matrix();
|
|
index = getIndexFromIndexVector(indexVector, bufferSize, bufferStartIndex);
|
|
return checkIfPositionWithinMap(position, mapLength, mapPosition) && checkIfIndexInRange(index, bufferSize);
|
|
}
|
|
|
|
bool checkIfPositionWithinMap(const Position& position,
|
|
const Length& mapLength,
|
|
const Position& mapPosition)
|
|
{
|
|
Vector offset;
|
|
getVectorToOrigin(offset, mapLength);
|
|
Position positionTransformed = getMapFrameToBufferOrderTransformation().cast<double>() * (position - mapPosition - offset);
|
|
|
|
return positionTransformed.x() >= 0.0 && positionTransformed.y() >= 0.0
|
|
&& positionTransformed.x() < mapLength(0) && positionTransformed.y() < mapLength(1);
|
|
}
|
|
|
|
void getPositionOfDataStructureOrigin(const Position& position,
|
|
const Length& mapLength,
|
|
Position& positionOfOrigin)
|
|
{
|
|
Vector vectorToOrigin;
|
|
getVectorToOrigin(vectorToOrigin, mapLength);
|
|
positionOfOrigin = position + vectorToOrigin;
|
|
}
|
|
|
|
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));
|
|
}
|
|
|
|
indexShift = transformMapFrameToBufferOrder(indexShiftVector);
|
|
return true;
|
|
}
|
|
|
|
bool getPositionShiftFromIndexShift(Vector& positionShift,
|
|
const Index& indexShift,
|
|
const double& resolution)
|
|
{
|
|
positionShift = transformBufferOrderToMapFrame(indexShift) * resolution;
|
|
return true;
|
|
}
|
|
|
|
bool checkIfIndexInRange(const Index& index, const Size& bufferSize)
|
|
{
|
|
return index[0] >= 0 && index[1] >= 0 && index[0] < bufferSize[0] && index[1] < bufferSize[1];
|
|
}
|
|
|
|
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, int bufferSize)
|
|
{
|
|
// Try shortcuts before resorting to the expensive modulo operation.
|
|
if (index < bufferSize){
|
|
if(index >= 0){ // within the wanted range
|
|
return;
|
|
} else if(index >= -bufferSize){ // Index is below range, but not more than one span of the range.
|
|
index +=bufferSize;
|
|
return;
|
|
}else{ // Index is largely below range.
|
|
index = index % bufferSize;
|
|
index += bufferSize;
|
|
}
|
|
}else if(index < bufferSize*2){ // Index is above range, but not more than one span of the range.
|
|
index -= bufferSize;
|
|
return;
|
|
} else{ // Index is largely above range.
|
|
index = index % bufferSize;
|
|
}
|
|
}
|
|
|
|
void boundPositionToRange(Position& position, const Length& mapLength, const Position& mapPosition)
|
|
{
|
|
Vector vectorToOrigin;
|
|
getVectorToOrigin(vectorToOrigin, mapLength);
|
|
Position positionShifted = position - mapPosition + vectorToOrigin;
|
|
|
|
// 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.
|
|
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;
|
|
}
|
|
|
|
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)
|
|
const Eigen::Matrix2d halfTransform = 0.5 * getMapFrameToBufferOrderTransformation().cast<double>();
|
|
|
|
// Corners of submap.
|
|
Position topLeftPosition = requestedSubmapPosition - halfTransform * 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 + halfTransform * 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 -= halfTransform * Position::Constant(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.
|
|
return getIndexFromPosition(requestedIndexInSubmap, requestedSubmapPosition, submapLength, submapPosition, resolution, submapBufferSize);
|
|
}
|
|
|
|
Size getSubmapSizeFromCornerIndices(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.emplace_back(submapIndex, submapBufferSize, BufferRegion::Quadrant::TopLeft);
|
|
return true;
|
|
}
|
|
|
|
if (quadrantOfBottomRight == BufferRegion::Quadrant::TopRight) {
|
|
Size topLeftSize(submapBufferSize(0), bufferSize(1) - submapIndex(1));
|
|
submapBufferRegions.emplace_back(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft);
|
|
|
|
Index topRightIndex(submapIndex(0), 0);
|
|
Size topRightSize(submapBufferSize(0), submapBufferSize(1) - topLeftSize(1));
|
|
submapBufferRegions.emplace_back(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight);
|
|
return true;
|
|
}
|
|
|
|
if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomLeft) {
|
|
Size topLeftSize(bufferSize(0) - submapIndex(0), submapBufferSize(1));
|
|
submapBufferRegions.emplace_back(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft);
|
|
|
|
Index bottomLeftIndex(0, submapIndex(1));
|
|
Size bottomLeftSize(submapBufferSize(0) - topLeftSize(0), submapBufferSize(1));
|
|
submapBufferRegions.emplace_back(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft);
|
|
return true;
|
|
}
|
|
|
|
if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
|
|
Size topLeftSize(bufferSize(0) - submapIndex(0), bufferSize(1) - submapIndex(1));
|
|
submapBufferRegions.emplace_back(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft);
|
|
|
|
Index topRightIndex(submapIndex(0), 0);
|
|
Size topRightSize(bufferSize(0) - submapIndex(0), submapBufferSize(1) - topLeftSize(1));
|
|
submapBufferRegions.emplace_back(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight);
|
|
|
|
Index bottomLeftIndex(0, submapIndex(1));
|
|
Size bottomLeftSize(submapBufferSize(0) - topLeftSize(0), bufferSize(1) - submapIndex(1));
|
|
submapBufferRegions.emplace_back(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft);
|
|
|
|
Index bottomRightIndex = Index::Zero();
|
|
Size bottomRightSize(bottomLeftSize(0), topRightSize(1));
|
|
submapBufferRegions.emplace_back(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight);
|
|
return true;
|
|
}
|
|
|
|
} else if (quadrantOfTopLeft == BufferRegion::Quadrant::TopRight) {
|
|
|
|
if (quadrantOfBottomRight == BufferRegion::Quadrant::TopRight) {
|
|
submapBufferRegions.emplace_back(submapIndex, submapBufferSize, BufferRegion::Quadrant::TopRight);
|
|
return true;
|
|
}
|
|
|
|
if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
|
|
|
|
Size topRightSize(bufferSize(0) - submapIndex(0), submapBufferSize(1));
|
|
submapBufferRegions.emplace_back(submapIndex, topRightSize, BufferRegion::Quadrant::TopRight);
|
|
|
|
Index bottomRightIndex(0, submapIndex(1));
|
|
Size bottomRightSize(submapBufferSize(0) - topRightSize(0), submapBufferSize(1));
|
|
submapBufferRegions.emplace_back(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight);
|
|
return true;
|
|
}
|
|
|
|
} else if (quadrantOfTopLeft == BufferRegion::Quadrant::BottomLeft) {
|
|
|
|
if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomLeft) {
|
|
submapBufferRegions.emplace_back(submapIndex, submapBufferSize, BufferRegion::Quadrant::BottomLeft);
|
|
return true;
|
|
}
|
|
|
|
if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
|
|
Size bottomLeftSize(submapBufferSize(0), bufferSize(1) - submapIndex(1));
|
|
submapBufferRegions.emplace_back(submapIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft);
|
|
|
|
Index bottomRightIndex(submapIndex(0), 0);
|
|
Size bottomRightSize(submapBufferSize(0), submapBufferSize(1) - bottomLeftSize(1));
|
|
submapBufferRegions.emplace_back(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight);
|
|
return true;
|
|
}
|
|
|
|
} else if (quadrantOfTopLeft == BufferRegion::Quadrant::BottomRight) {
|
|
|
|
if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
|
|
submapBufferRegions.emplace_back(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;
|
|
}
|
|
|
|
Index getIndexFromBufferIndex(const Index& bufferIndex, const Size& bufferSize, const Index& bufferStartIndex)
|
|
{
|
|
if (checkIfStartIndexAtDefaultPosition(bufferStartIndex)) {
|
|
return bufferIndex;
|
|
}
|
|
|
|
Index index = bufferIndex - bufferStartIndex;
|
|
wrapIndexToRange(index, bufferSize);
|
|
return index;
|
|
}
|
|
|
|
Index getBufferIndexFromIndex(const Index& index, const Size& bufferSize, const Index& bufferStartIndex)
|
|
{
|
|
if (checkIfStartIndexAtDefaultPosition(bufferStartIndex)) {
|
|
return index;
|
|
}
|
|
|
|
Index bufferIndex = index + bufferStartIndex;
|
|
wrapIndexToRange(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));
|
|
}
|
|
|
|
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)
|
|
{
|
|
Color colors;
|
|
colors.longColor_ = (colorVector(0) << 16) + (colorVector(1) << 8) + colorVector(2);
|
|
colorValue = colors.floatColor_;
|
|
}
|
|
|
|
void colorVectorToValue(const Eigen::Vector3f& colorVector, float& colorValue)
|
|
{
|
|
Eigen::Vector3i tempColorVector = (colorVector * 255.0).cast<int>();
|
|
colorVectorToValue(tempColorVector, colorValue);
|
|
}
|
|
|
|
} // namespace grid_map
|
|
|