Program Listing for File GridMapMath.hpp

Return to documentation for file (/tmp/ws/src/grid_map/grid_map_core/include/grid_map_core/GridMapMath.hpp)

/*
 * GridMapMath.hpp
 *
 *  Created on: Dec 2, 2013
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich, ANYbotics
 */

#ifndef GRID_MAP_CORE__GRIDMAPMATH_HPP_
#define GRID_MAP_CORE__GRIDMAPMATH_HPP_

#include <Eigen/Core>
#include <vector>
#include <map>

#include "grid_map_core/TypeDefs.hpp"
#include "grid_map_core/BufferRegion.hpp"

namespace grid_map
{

bool getPositionFromIndex(
  Position & position,
  const Index & index,
  const Length & mapLength,
  const Position & mapPosition,
  const double & resolution,
  const Size & bufferSize,
  const Index & bufferStartIndex = Index::Zero());

bool getIndexFromPosition(
  Index & index,
  const Position & position,
  const Length & mapLength,
  const Position & mapPosition,
  const double & resolution,
  const Size & bufferSize,
  const Index & bufferStartIndex = Index::Zero());

bool checkIfPositionWithinMap(
  const Position & position,
  const Length & mapLength,
  const Position & mapPosition);

void getPositionOfDataStructureOrigin(
  const Position & position,
  const Length & mapLength,
  Position & positionOfOrigin);

bool getIndexShiftFromPositionShift(
  Index & indexShift,
  const Vector & positionShift,
  const double & resolution);

bool getPositionShiftFromIndexShift(
  Vector & positionShift,
  const Index & indexShift,
  const double & resolution);

bool checkIfIndexInRange(const Index & index, const Size & bufferSize);

void boundIndexToRange(Index & index, const Size & bufferSize);

void boundIndexToRange(int & index, const int & bufferSize);

void wrapIndexToRange(Index & index, const Size & bufferSize);

void wrapIndexToRange(int & index, int bufferSize);

void boundPositionToRange(
  Position & position, const Length & mapLength,
  const Position & mapPosition);

const Eigen::Matrix2i getBufferOrderToMapFrameAlignment();

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 = Index::Zero());

Size getSubmapSizeFromCornerIndeces(
  const Index & topLeftIndex, const Index & bottomRightIndex,
  const Size & bufferSize, const Index & bufferStartIndex);

bool getBufferRegionsForSubmap(
  std::vector<BufferRegion> & submapBufferRegions,
  const Index & submapIndex,
  const Size & submapBufferSize,
  const Size & bufferSize,
  const Index & bufferStartIndex = Index::Zero());

bool incrementIndex(
  Index & index, const Size & bufferSize,
  const Index & bufferStartIndex = Index::Zero());

bool incrementIndexForSubmap(
  Index & submapIndex, Index & index, const Index & submapTopLeftIndex,
  const Size & submapBufferSize, const Size & bufferSize,
  const Index & bufferStartIndex = Index::Zero());

Index getIndexFromBufferIndex(
  const Index & bufferIndex, const Size & bufferSize,
  const Index & bufferStartIndex);

Index getBufferIndexFromIndex(
  const Index & index, const Size & bufferSize,
  const Index & bufferStartIndex);

size_t getLinearIndexFromIndex(
  const Index & index, const Size & bufferSize,
  const bool rowMajor = false);

Index getIndexFromLinearIndex(
  const size_t linearIndex, const Size & bufferSize,
  const bool rowMajor = false);

void getIndicesForRegion(
  const Index & regionIndex, const Size & regionSize,
  std::vector<Index> indices);

void getIndicesForRegions(
  const std::vector<Index> & regionIndeces, const Size & regionSizes,
  std::vector<Index> indices);

bool colorValueToVector(const uint64_t & colorValue, Eigen::Vector3i & colorVector);

bool colorValueToVector(const uint64_t & colorValue, Eigen::Vector3f & colorVector);

bool colorValueToVector(const float & colorValue, Eigen::Vector3f & colorVector);

bool colorVectorToValue(const Eigen::Vector3i & colorVector, uint64_t & colorValue);

void colorVectorToValue(const Eigen::Vector3i & colorVector, float & colorValue);

void colorVectorToValue(const Eigen::Vector3f & colorVector, float & colorValue);

}  // namespace grid_map
#endif  // GRID_MAP_CORE__GRIDMAPMATH_HPP_