GridMap.hpp

Go to the documentation of this file.

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

#pragma once

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

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

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

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;

  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 getSubmap(const Position& position, const Length& length, Index& indexInSubmap,
                    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 */