Program Listing for File GridMap.hpp

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

/*
 * GridMap.hpp
 *
 *  Created on: Jul 14, 2014
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich, ANYbotics
 */

#ifndef GRID_MAP_CORE__GRIDMAP_HPP_
#define GRID_MAP_CORE__GRIDMAP_HPP_

// Eigen
#include <Eigen/Core>
#include <Eigen/Geometry>

// STL
#include <vector>
#include <string>
#include <utility>
#include <unordered_map>

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

namespace grid_map
{

class SubmapGeometry;

class GridMap
{
public:
  // Type traits for use with template methods/classes using GridMap as a template parameter.
  typedef grid_map::DataType DataType;
  typedef grid_map::Matrix Matrix;

  explicit GridMap(const std::vector<std::string> & layers);

  GridMap();

  GridMap(const GridMap &) = default;
  GridMap & operator=(const GridMap &) = default;
  GridMap(GridMap &&) = default;
  GridMap & operator=(GridMap &&) = default;

  virtual ~GridMap() = default;

  void setGeometry(
    const Length & length, const double resolution,
    const Position & position = Position::Zero());

  void setGeometry(const SubmapGeometry & geometry);

  void add(const std::string & layer, const double value = NAN);

  void add(const std::string & layer, const Matrix & data);

  bool exists(const std::string & layer) const;

  const Matrix & get(const std::string & layer) const;

  Matrix & get(const std::string & layer);

  const Matrix & operator[](const std::string & layer) const;

  Matrix & operator[](const std::string & layer);

  bool erase(const std::string & layer);

  const std::vector<std::string> & getLayers() const;

  void setBasicLayers(const std::vector<std::string> & basicLayers);

  const std::vector<std::string> & getBasicLayers() const;

  bool hasBasicLayers() const;

  bool hasSameLayers(const grid_map::GridMap & other) const;

  float & atPosition(const std::string & layer, const Position & position);

  float atPosition(
    const std::string & layer, const Position & position,
    InterpolationMethods interpolationMethod = InterpolationMethods::INTER_NEAREST) const;

  float & at(const std::string & layer, const Index & index);

  float at(const std::string & layer, const Index & index) const;

  bool getIndex(const Position & position, Index & index) const;

  bool getPosition(const Index & index, Position & position) const;

  bool isInside(const Position & position) const;

  bool isValid(const Index & index) const;

  bool isValid(const Index & index, const std::string & layer) const;

  bool isValid(const Index & index, const std::vector<std::string> & layers) const;

  bool getPosition3(const std::string & layer, const Index & index, Position3 & position) const;

  bool getVector(
    const std::string & layerPrefix, const Index & index,
    Eigen::Vector3d & vector) const;

  GridMap getSubmap(const Position & position, const Length & length, bool & isSuccess) const;

  GridMap getTransformedMap(
    const Eigen::Isometry3d & transform, const std::string & heightLayerName,
    const std::string & newFrameId,
    const double sampleRatio = 0.0) const;

  void setPosition(const Position & position);

  bool move(const Position & position, std::vector<BufferRegion> & newRegions);

  bool move(const Position & position);

  bool addDataFrom(
    const GridMap & other, bool extendMap,
    bool overwriteData, bool copyAllLayers,
    std::vector<std::string> layers = std::vector<std::string>());

  bool extendToInclude(const GridMap & other);

  void clear(const std::string & layer);

  void clearBasic();

  void clearAll();

  void setTimestamp(const Time timestamp);

  Time getTimestamp() const;

  void resetTimestamp();

  void setFrameId(const std::string & frameId);

  const std::string & getFrameId() const;

  const Length & getLength() const;

  const Position & getPosition() const;

  double getResolution() const;

  const Size & getSize() const;

  void setStartIndex(const Index & startIndex);

  const Index & getStartIndex() const;

  bool isDefaultStartIndex() const;

  void convertToDefaultStartIndex();

  Position getClosestPositionInMap(const Position & position) const;

private:
  bool isValid(DataType value) const;

  void clearCols(unsigned int index, unsigned int nCols);

  void clearRows(unsigned int index, unsigned int nRows);

  bool atPositionLinearInterpolated(
    const std::string & layer, const Position & position,
    float & value) const;


  bool atPositionBicubicConvolutionInterpolated(
    const std::string & layer, const Position & position,
    float & value) const;

  bool atPositionBicubicInterpolated(
    const std::string & layer, const Position & position,
    float & value) const;


  void resize(const Index & bufferSize);

  std::string frameId_;

  Time timestamp_;

  std::unordered_map<std::string, Matrix> data_;

  std::vector<std::string> layers_;

  std::vector<std::string> basicLayers_;

  Length length_;

  double resolution_;

  Position position_;

  Size size_;

  Index startIndex_;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

}  // namespace grid_map
#endif  // GRID_MAP_CORE__GRIDMAP_HPP_