GridMapMath.hpp

Go to the documentation of this file.

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

#pragma once

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

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

namespace grid_map {

union Color
{
    unsigned long longColor_;
    float floatColor_;
};

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);

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 getSubmapSizeFromCornerIndices(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, bool rowMajor = false);

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

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

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

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

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

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

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

}  // namespace grid_map