00001
00002
00003
00004
00005
00006
00007
00008
00009 #include "grid_map_core/GridMapMath.hpp"
00010
00011
00012 #include <cmath>
00013
00014
00015 #include <limits>
00016
00017 using namespace std;
00018
00019 namespace grid_map {
00020
00021 namespace internal {
00022
00030 inline bool getVectorToOrigin(Vector& vectorToOrigin, const Length& mapLength)
00031 {
00032 vectorToOrigin = (0.5 * mapLength).matrix();
00033 return true;
00034 }
00035
00044 inline bool getVectorToFirstCell(Vector& vectorToFirstCell,
00045 const Length& mapLength, const double& resolution)
00046 {
00047 Vector vectorToOrigin;
00048 getVectorToOrigin(vectorToOrigin, mapLength);
00049
00050
00051 vectorToFirstCell = (vectorToOrigin.array() - 0.5 * resolution).matrix();
00052 return true;
00053 }
00054
00055 inline Eigen::Matrix2i getBufferOrderToMapFrameTransformation()
00056 {
00057 return -Eigen::Matrix2i::Identity();
00058 }
00059
00060 inline Eigen::Matrix2i getMapFrameToBufferOrderTransformation()
00061 {
00062 return getBufferOrderToMapFrameTransformation().transpose();
00063 }
00064
00065 inline bool checkIfStartIndexAtDefaultPosition(const Index& bufferStartIndex)
00066 {
00067 return ((bufferStartIndex == 0).all());
00068 }
00069
00070 inline Vector getIndexVectorFromIndex(
00071 const Index& index,
00072 const Size& bufferSize,
00073 const Index& bufferStartIndex)
00074 {
00075 Index unwrappedIndex;
00076 unwrappedIndex = getIndexFromBufferIndex(index, bufferSize, bufferStartIndex);
00077 return (getBufferOrderToMapFrameTransformation() * unwrappedIndex.matrix()).cast<double>();
00078 }
00079
00080 inline Index getIndexFromIndexVector(
00081 const Vector& indexVector,
00082 const Size& bufferSize,
00083 const Index& bufferStartIndex)
00084 {
00085 Index index = (getMapFrameToBufferOrderTransformation() * indexVector.cast<int>()).array();
00086 return getBufferIndexFromIndex(index, bufferSize, bufferStartIndex);
00087 }
00088
00089 inline BufferRegion::Quadrant getQuadrant(const Index& index, const Index& bufferStartIndex)
00090 {
00091 if (index[0] >= bufferStartIndex[0] && index[1] >= bufferStartIndex[1]) return BufferRegion::Quadrant::TopLeft;
00092 if (index[0] >= bufferStartIndex[0] && index[1] < bufferStartIndex[1]) return BufferRegion::Quadrant::TopRight;
00093 if (index[0] < bufferStartIndex[0] && index[1] >= bufferStartIndex[1]) return BufferRegion::Quadrant::BottomLeft;
00094 if (index[0] < bufferStartIndex[0] && index[1] < bufferStartIndex[1]) return BufferRegion::Quadrant::BottomRight;
00095 return BufferRegion::Quadrant::Undefined;
00096 }
00097
00098 }
00099
00100 using namespace internal;
00101
00102 bool getPositionFromIndex(Position& position,
00103 const Index& index,
00104 const Length& mapLength,
00105 const Position& mapPosition,
00106 const double& resolution,
00107 const Size& bufferSize,
00108 const Index& bufferStartIndex)
00109 {
00110 if (!checkIfIndexInRange(index, bufferSize)) return false;
00111 Vector offset;
00112 getVectorToFirstCell(offset, mapLength, resolution);
00113 position = mapPosition + offset + resolution * getIndexVectorFromIndex(index, bufferSize, bufferStartIndex);
00114 return true;
00115 }
00116
00117 bool getIndexFromPosition(Index& index,
00118 const Position& position,
00119 const Length& mapLength,
00120 const Position& mapPosition,
00121 const double& resolution,
00122 const Size& bufferSize,
00123 const Index& bufferStartIndex)
00124 {
00125 Vector offset;
00126 getVectorToOrigin(offset, mapLength);
00127 Vector indexVector = ((position - offset - mapPosition).array() / resolution).matrix();
00128 index = getIndexFromIndexVector(indexVector, bufferSize, bufferStartIndex);
00129 if (!checkIfPositionWithinMap(position, mapLength, mapPosition)) return false;
00130 return true;
00131 }
00132
00133 bool checkIfPositionWithinMap(const Position& position,
00134 const Length& mapLength,
00135 const Position& mapPosition)
00136 {
00137 Vector offset;
00138 getVectorToOrigin(offset, mapLength);
00139 Position positionTransformed = getMapFrameToBufferOrderTransformation().cast<double>() * (position - mapPosition - offset);
00140
00141 if (positionTransformed.x() >= 0.0 && positionTransformed.y() >= 0.0
00142 && positionTransformed.x() < mapLength(0) && positionTransformed.y() < mapLength(1)) {
00143 return true;
00144 }
00145 return false;
00146 }
00147
00148 void getPositionOfDataStructureOrigin(const Position& position,
00149 const Length& mapLength,
00150 Position& positionOfOrigin)
00151 {
00152 Vector vectorToOrigin;
00153 getVectorToOrigin(vectorToOrigin, mapLength);
00154 positionOfOrigin = position + vectorToOrigin;
00155 }
00156
00157 bool getIndexShiftFromPositionShift(Index& indexShift,
00158 const Vector& positionShift,
00159 const double& resolution)
00160 {
00161 Vector indexShiftVectorTemp = (positionShift.array() / resolution).matrix();
00162 Eigen::Vector2i indexShiftVector;
00163
00164 for (int i = 0; i < indexShiftVector.size(); i++) {
00165 indexShiftVector[i] = static_cast<int>(indexShiftVectorTemp[i] + 0.5 * (indexShiftVectorTemp[i] > 0 ? 1 : -1));
00166 }
00167
00168 indexShift = (getMapFrameToBufferOrderTransformation() * indexShiftVector).array();
00169 return true;
00170 }
00171
00172 bool getPositionShiftFromIndexShift(Vector& positionShift,
00173 const Index& indexShift,
00174 const double& resolution)
00175 {
00176 positionShift = (getBufferOrderToMapFrameTransformation() * indexShift.matrix()).cast<double>() * resolution;
00177 return true;
00178 }
00179
00180 bool checkIfIndexInRange(const Index& index, const Size& bufferSize)
00181 {
00182 if (index[0] >= 0 && index[1] >= 0 && index[0] < bufferSize[0] && index[1] < bufferSize[1])
00183 {
00184 return true;
00185 }
00186 return false;
00187 }
00188
00189 void boundIndexToRange(Index& index, const Size& bufferSize)
00190 {
00191 for (int i = 0; i < index.size(); i++) {
00192 boundIndexToRange(index[i], bufferSize[i]);
00193 }
00194 }
00195
00196 void boundIndexToRange(int& index, const int& bufferSize)
00197 {
00198 if (index < 0) index = 0;
00199 else if (index >= bufferSize) index = bufferSize - 1;
00200 }
00201
00202 void wrapIndexToRange(Index& index, const Size& bufferSize)
00203 {
00204 for (int i = 0; i < index.size(); i++) {
00205 wrapIndexToRange(index[i], bufferSize[i]);
00206 }
00207 }
00208
00209 void wrapIndexToRange(int& index, const int& bufferSize)
00210 {
00211 if (index < 0) index += ((-index / bufferSize) + 1) * bufferSize;
00212 index = index % bufferSize;
00213 }
00214
00215 void boundPositionToRange(Position& position, const Length& mapLength, const Position& mapPosition)
00216 {
00217 Vector vectorToOrigin;
00218 getVectorToOrigin(vectorToOrigin, mapLength);
00219 Position positionShifted = position - mapPosition + vectorToOrigin;
00220
00221
00222 for (int i = 0; i < positionShifted.size(); i++) {
00223
00224 double epsilon = 10.0 * numeric_limits<double>::epsilon();
00225 if (std::fabs(position(i)) > 1.0) epsilon *= std::fabs(position(i));
00226
00227 if (positionShifted(i) <= 0) {
00228 positionShifted(i) = epsilon;
00229 continue;
00230 }
00231 if (positionShifted(i) >= mapLength(i)) {
00232 positionShifted(i) = mapLength(i) - epsilon;
00233 continue;
00234 }
00235 }
00236
00237 position = positionShifted + mapPosition - vectorToOrigin;
00238 }
00239
00240 const Eigen::Matrix2i getBufferOrderToMapFrameAlignment()
00241 {
00242 return getBufferOrderToMapFrameTransformation().array().abs().matrix();
00243 }
00244
00245 bool getSubmapInformation(Index& submapTopLeftIndex,
00246 Size& submapBufferSize,
00247 Position& submapPosition,
00248 Length& submapLength,
00249 Index& requestedIndexInSubmap,
00250 const Position& requestedSubmapPosition,
00251 const Length& requestedSubmapLength,
00252 const Length& mapLength,
00253 const Position& mapPosition,
00254 const double& resolution,
00255 const Size& bufferSize,
00256 const Index& bufferStartIndex)
00257 {
00258
00259 Eigen::Matrix2d transform = getMapFrameToBufferOrderTransformation().cast<double>();
00260
00261
00262 Position topLeftPosition = requestedSubmapPosition - transform * 0.5 * requestedSubmapLength.matrix();
00263 boundPositionToRange(topLeftPosition, mapLength, mapPosition);
00264 if(!getIndexFromPosition(submapTopLeftIndex, topLeftPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) return false;
00265 Index topLeftIndex;
00266 topLeftIndex = getIndexFromBufferIndex(submapTopLeftIndex, bufferSize, bufferStartIndex);
00267
00268 Position bottomRightPosition = requestedSubmapPosition + transform * 0.5 * requestedSubmapLength.matrix();
00269 boundPositionToRange(bottomRightPosition, mapLength, mapPosition);
00270 Index bottomRightIndex;
00271 if(!getIndexFromPosition(bottomRightIndex, bottomRightPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) return false;
00272 bottomRightIndex = getIndexFromBufferIndex(bottomRightIndex, bufferSize, bufferStartIndex);
00273
00274
00275 Position topLeftCorner;
00276 if(!getPositionFromIndex(topLeftCorner, submapTopLeftIndex, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) return false;
00277 topLeftCorner -= transform * Position::Constant(0.5 * resolution);
00278
00279
00280 submapBufferSize = bottomRightIndex - topLeftIndex + Index::Ones();
00281
00282
00283 submapLength = submapBufferSize.cast<double>() * resolution;
00284
00285
00286 Vector vectorToSubmapOrigin;
00287 getVectorToOrigin(vectorToSubmapOrigin, submapLength);
00288 submapPosition = topLeftCorner - vectorToSubmapOrigin;
00289
00290
00291
00292 if(!getIndexFromPosition(requestedIndexInSubmap, requestedSubmapPosition, submapLength, submapPosition, resolution, submapBufferSize)) return false;
00293
00294 return true;
00295 }
00296
00297 Size getSubmapSizeFromCornerIndeces(const Index& topLeftIndex, const Index& bottomRightIndex,
00298 const Size& bufferSize, const Index& bufferStartIndex)
00299 {
00300 const Index unwrappedTopLeftIndex = getIndexFromBufferIndex(topLeftIndex, bufferSize, bufferStartIndex);
00301 const Index unwrappedBottomRightIndex = getIndexFromBufferIndex(bottomRightIndex, bufferSize, bufferStartIndex);
00302 return Size(unwrappedBottomRightIndex - unwrappedTopLeftIndex + Size::Ones());
00303 }
00304
00305 bool getBufferRegionsForSubmap(std::vector<BufferRegion>& submapBufferRegions,
00306 const Index& submapIndex,
00307 const Size& submapBufferSize,
00308 const Size& bufferSize,
00309 const Index& bufferStartIndex)
00310 {
00311 if ((getIndexFromBufferIndex(submapIndex, bufferSize, bufferStartIndex) + submapBufferSize > bufferSize).any()) return false;
00312
00313 submapBufferRegions.clear();
00314
00315 Index bottomRightIndex = submapIndex + submapBufferSize - Index::Ones();
00316 wrapIndexToRange(bottomRightIndex, bufferSize);
00317
00318 BufferRegion::Quadrant quadrantOfTopLeft = getQuadrant(submapIndex, bufferStartIndex);
00319 BufferRegion::Quadrant quadrantOfBottomRight = getQuadrant(bottomRightIndex, bufferStartIndex);
00320
00321 if (quadrantOfTopLeft == BufferRegion::Quadrant::TopLeft) {
00322
00323 if (quadrantOfBottomRight == BufferRegion::Quadrant::TopLeft) {
00324 submapBufferRegions.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::TopLeft));
00325 return true;
00326 }
00327
00328 if (quadrantOfBottomRight == BufferRegion::Quadrant::TopRight) {
00329 Size topLeftSize(submapBufferSize(0), bufferSize(1) - submapIndex(1));
00330 submapBufferRegions.push_back(BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft));
00331
00332 Index topRightIndex(submapIndex(0), 0);
00333 Size topRightSize(submapBufferSize(0), submapBufferSize(1) - topLeftSize(1));
00334 submapBufferRegions.push_back(BufferRegion(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight));
00335 return true;
00336 }
00337
00338 if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomLeft) {
00339 Size topLeftSize(bufferSize(0) - submapIndex(0), submapBufferSize(1));
00340 submapBufferRegions.push_back(BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft));
00341
00342 Index bottomLeftIndex(0, submapIndex(1));
00343 Size bottomLeftSize(submapBufferSize(0) - topLeftSize(0), submapBufferSize(1));
00344 submapBufferRegions.push_back(BufferRegion(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft));
00345 return true;
00346 }
00347
00348 if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
00349 Size topLeftSize(bufferSize(0) - submapIndex(0), bufferSize(1) - submapIndex(1));
00350 submapBufferRegions.push_back(BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft));
00351
00352 Index topRightIndex(submapIndex(0), 0);
00353 Size topRightSize(bufferSize(0) - submapIndex(0), submapBufferSize(1) - topLeftSize(1));
00354 submapBufferRegions.push_back(BufferRegion(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight));
00355
00356 Index bottomLeftIndex(0, submapIndex(1));
00357 Size bottomLeftSize(submapBufferSize(0) - topLeftSize(0), bufferSize(1) - submapIndex(1));
00358 submapBufferRegions.push_back(BufferRegion(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft));
00359
00360 Index bottomRightIndex = Index::Zero();
00361 Size bottomRightSize(bottomLeftSize(0), topRightSize(1));
00362 submapBufferRegions.push_back(BufferRegion(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight));
00363 return true;
00364 }
00365
00366 } else if (quadrantOfTopLeft == BufferRegion::Quadrant::TopRight) {
00367
00368 if (quadrantOfBottomRight == BufferRegion::Quadrant::TopRight) {
00369 submapBufferRegions.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::TopRight));
00370 return true;
00371 }
00372
00373 if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
00374
00375 Size topRightSize(bufferSize(0) - submapIndex(0), submapBufferSize(1));
00376 submapBufferRegions.push_back(BufferRegion(submapIndex, topRightSize, BufferRegion::Quadrant::TopRight));
00377
00378 Index bottomRightIndex(0, submapIndex(1));
00379 Size bottomRightSize(submapBufferSize(0) - topRightSize(0), submapBufferSize(1));
00380 submapBufferRegions.push_back(BufferRegion(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight));
00381 return true;
00382 }
00383
00384 } else if (quadrantOfTopLeft == BufferRegion::Quadrant::BottomLeft) {
00385
00386 if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomLeft) {
00387 submapBufferRegions.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::BottomLeft));
00388 return true;
00389 }
00390
00391 if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
00392 Size bottomLeftSize(submapBufferSize(0), bufferSize(1) - submapIndex(1));
00393 submapBufferRegions.push_back(BufferRegion(submapIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft));
00394
00395 Index bottomRightIndex(submapIndex(0), 0);
00396 Size bottomRightSize(submapBufferSize(0), submapBufferSize(1) - bottomLeftSize(1));
00397 submapBufferRegions.push_back(BufferRegion(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight));
00398 return true;
00399 }
00400
00401 } else if (quadrantOfTopLeft == BufferRegion::Quadrant::BottomRight) {
00402
00403 if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
00404 submapBufferRegions.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::BottomRight));
00405 return true;
00406 }
00407
00408 }
00409
00410 return false;
00411 }
00412
00413 bool incrementIndex(Index& index, const Size& bufferSize, const Index& bufferStartIndex)
00414 {
00415 Index unwrappedIndex = getIndexFromBufferIndex(index, bufferSize, bufferStartIndex);
00416
00417
00418 if (unwrappedIndex(1) + 1 < bufferSize(1)) {
00419
00420 unwrappedIndex[1]++;
00421 } else {
00422
00423 unwrappedIndex[0]++;
00424 unwrappedIndex[1] = 0;
00425 }
00426
00427
00428 if (!checkIfIndexInRange(unwrappedIndex, bufferSize)) return false;
00429
00430
00431 index = getBufferIndexFromIndex(unwrappedIndex, bufferSize, bufferStartIndex);
00432 return true;
00433 }
00434
00435 bool incrementIndexForSubmap(Index& submapIndex, Index& index, const Index& submapTopLeftIndex,
00436 const Size& submapBufferSize, const Size& bufferSize,
00437 const Index& bufferStartIndex)
00438 {
00439
00440 Index tempIndex = index;
00441 Index tempSubmapIndex = submapIndex;
00442
00443
00444 if (tempSubmapIndex[1] + 1 < submapBufferSize[1]) {
00445
00446 tempSubmapIndex[1]++;
00447 } else {
00448
00449 tempSubmapIndex[0]++;
00450 tempSubmapIndex[1] = 0;
00451 }
00452
00453
00454 if (!checkIfIndexInRange(tempSubmapIndex, submapBufferSize)) return false;
00455
00456
00457 Index unwrappedSubmapTopLeftIndex = getIndexFromBufferIndex(submapTopLeftIndex, bufferSize, bufferStartIndex);
00458 tempIndex = getBufferIndexFromIndex(unwrappedSubmapTopLeftIndex + tempSubmapIndex, bufferSize, bufferStartIndex);
00459
00460
00461 index = tempIndex;
00462 submapIndex = tempSubmapIndex;
00463 return true;
00464 }
00465
00466 Index getIndexFromBufferIndex(const Index& bufferIndex, const Size& bufferSize, const Index& bufferStartIndex)
00467 {
00468 if (checkIfStartIndexAtDefaultPosition(bufferStartIndex)) return bufferIndex;
00469
00470 Index index = bufferIndex - bufferStartIndex;
00471 wrapIndexToRange(index, bufferSize);
00472 return index;
00473 }
00474
00475 Index getBufferIndexFromIndex(const Index& index, const Size& bufferSize, const Index& bufferStartIndex)
00476 {
00477 if (checkIfStartIndexAtDefaultPosition(bufferStartIndex)) return index;
00478
00479 Index bufferIndex = index + bufferStartIndex;
00480 wrapIndexToRange(bufferIndex, bufferSize);
00481 return bufferIndex;
00482 }
00483
00484 size_t getLinearIndexFromIndex(const Index& index, const Size& bufferSize, const bool rowMajor)
00485 {
00486 if (!rowMajor) return index(1) * bufferSize(0) + index(0);
00487 return index(0) * bufferSize(1) + index(1);
00488 }
00489
00490 Index getIndexFromLinearIndex(const size_t linearIndex, const Size& bufferSize, const bool rowMajor)
00491 {
00492 if (!rowMajor) return Index((int)linearIndex % bufferSize(0), (int)linearIndex / bufferSize(0));
00493 return Index((int)linearIndex / bufferSize(1), (int)linearIndex % bufferSize(1));
00494 }
00495
00496 void getIndicesForRegion(const Index& regionIndex, const Size& regionSize,
00497 std::vector<Index> indices)
00498 {
00499
00500
00501
00502
00503
00504 }
00505
00506 void getIndicesForRegions(const std::vector<Index>& regionIndeces, const Size& regionSizes,
00507 std::vector<Index> indices)
00508 {
00509 }
00510
00511 bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3i& colorVector)
00512 {
00513 colorVector(0) = (colorValue >> 16) & 0x0000ff;
00514 colorVector(1) = (colorValue >> 8) & 0x0000ff;
00515 colorVector(2) = colorValue & 0x0000ff;
00516 return true;
00517 }
00518
00519 bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3f& colorVector)
00520 {
00521 Eigen::Vector3i tempColorVector;
00522 colorValueToVector(colorValue, tempColorVector);
00523 colorVector = ((tempColorVector.cast<float>()).array() / 255.0).matrix();
00524 return true;
00525 }
00526
00527 bool colorValueToVector(const float& colorValue, Eigen::Vector3f& colorVector)
00528 {
00529
00530 const unsigned long tempColorValue = *reinterpret_cast<const unsigned long*>(&colorValue);
00531 colorValueToVector(tempColorValue, colorVector);
00532 return true;
00533 }
00534
00535 bool colorVectorToValue(const Eigen::Vector3i& colorVector, unsigned long& colorValue)
00536 {
00537 colorValue = ((int)colorVector(0)) << 16 | ((int)colorVector(1)) << 8 | ((int)colorVector(2));
00538 return true;
00539 }
00540
00541 void colorVectorToValue(const Eigen::Vector3i& colorVector, float& colorValue)
00542 {
00543 unsigned long color = (colorVector(0) << 16) + (colorVector(1) << 8) + colorVector(2);
00544
00545 colorValue = *reinterpret_cast<float*>(&color);
00546 }
00547
00548 void colorVectorToValue(const Eigen::Vector3f& colorVector, float& colorValue)
00549 {
00550 Eigen::Vector3i tempColorVector = (colorVector * 255.0).cast<int>();
00551 colorVectorToValue(tempColorVector, colorValue);
00552 }
00553
00554 }
00555