GridMapMath.cpp
Go to the documentation of this file.
00001 /*
00002  * GridMapMath.cpp
00003  *
00004  *  Created on: Dec 2, 2013
00005  *      Author: Péter Fankhauser
00006  *       Institute: ETH Zurich, Autonomous Systems Lab
00007  */
00008 
00009 #include "grid_map_core/GridMapMath.hpp"
00010 
00011 // fabs
00012 #include <cmath>
00013 
00014 // Limits
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   // Vector to center of cell.
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 } // namespace
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   // We have to make sure to stay inside the map.
00222   for (int i = 0; i < positionShifted.size(); i++) {
00223 
00224     double epsilon = 10.0 * numeric_limits<double>::epsilon(); // TODO Why is the factor 10 necessary.
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   // (Top left / bottom right corresponds to the position in the matrix, not the map frame)
00259   Eigen::Matrix2d transform = getMapFrameToBufferOrderTransformation().cast<double>();
00260 
00261   // Corners of submap.
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   // Get the position of the top left corner of the generated submap.
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   // Size of submap.
00280   submapBufferSize = bottomRightIndex - topLeftIndex + Index::Ones();
00281 
00282   // Length of the submap.
00283   submapLength = submapBufferSize.cast<double>() * resolution;
00284 
00285   // Position of submap.
00286   Vector vectorToSubmapOrigin;
00287   getVectorToOrigin(vectorToSubmapOrigin, submapLength);
00288   submapPosition = topLeftCorner - vectorToSubmapOrigin;
00289 
00290   // Get the index of the cell which corresponds the requested
00291   // position of the submap.
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   // Increment index.
00418   if (unwrappedIndex(1) + 1 < bufferSize(1)) {
00419     // Same row.
00420     unwrappedIndex[1]++;
00421   } else {
00422     // Next row.
00423     unwrappedIndex[0]++;
00424     unwrappedIndex[1] = 0;
00425   }
00426 
00427   // End of iterations reached.
00428   if (!checkIfIndexInRange(unwrappedIndex, bufferSize)) return false;
00429 
00430   // Return true iterated index.
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   // Copy the data first, only copy it back if everything is within range.
00440   Index tempIndex = index;
00441   Index tempSubmapIndex = submapIndex;
00442 
00443   // Increment submap index.
00444   if (tempSubmapIndex[1] + 1 < submapBufferSize[1]) {
00445     // Same row.
00446     tempSubmapIndex[1]++;
00447   } else {
00448     // Next row.
00449     tempSubmapIndex[0]++;
00450     tempSubmapIndex[1] = 0;
00451   }
00452 
00453   // End of iterations reached.
00454   if (!checkIfIndexInRange(tempSubmapIndex, submapBufferSize)) return false;
00455 
00456   // Get corresponding index in map.
00457   Index unwrappedSubmapTopLeftIndex = getIndexFromBufferIndex(submapTopLeftIndex, bufferSize, bufferStartIndex);
00458   tempIndex = getBufferIndexFromIndex(unwrappedSubmapTopLeftIndex + tempSubmapIndex, bufferSize, bufferStartIndex);
00459 
00460   // Copy data back.
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 //  for (int i = line.index_; col < line.endIndex(); col++) {
00500 //    for (int i = 0; i < getSize()(0); i++) {
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   // cppcheck-suppress invalidPointerCast
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   // cppcheck-suppress invalidPointerCast
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 }  // namespace
00555 


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Mon Oct 9 2017 03:09:16