GridMap.hpp
Go to the documentation of this file.
00001 /*
00002  * GridMap.hpp
00003  *
00004  *  Created on: Jul 14, 2014
00005  *      Author: Péter Fankhauser
00006  *       Institute: ETH Zurich, ANYbotics
00007  */
00008 
00009 #pragma once
00010 
00011 #include "grid_map_core/TypeDefs.hpp"
00012 #include "grid_map_core/SubmapGeometry.hpp"
00013 #include "grid_map_core/BufferRegion.hpp"
00014 
00015 // STL
00016 #include <vector>
00017 #include <unordered_map>
00018 
00019 // Eigen
00020 #include <Eigen/Core>
00021 #include <Eigen/Geometry>
00022 
00023 namespace grid_map {
00024 
00025 class SubmapGeometry;
00026 
00040 class GridMap
00041 {
00042  public:
00043   // Type traits for use with template methods/classes using GridMap as a template parameter.
00044   typedef grid_map::DataType DataType;
00045   typedef grid_map::Matrix Matrix;
00046 
00051   GridMap(const std::vector<std::string>& layers);
00052 
00056   GridMap();
00057 
00061   GridMap(const GridMap&) = default;
00062   GridMap& operator=(const GridMap&) = default;
00063   GridMap(GridMap&&) = default;
00064   GridMap& operator=(GridMap&&) = default;
00065 
00069   virtual ~GridMap();
00070 
00077   void setGeometry(const Length& length, const double resolution,
00078                    const Position& position = Position::Zero());
00079 
00084   void setGeometry(const SubmapGeometry& geometry);
00085 
00091   void add(const std::string& layer, const double value = NAN);
00092 
00098   void add(const std::string& layer, const Matrix& data);
00099 
00105   bool exists(const std::string& layer) const;
00106 
00113   const Matrix& get(const std::string& layer) const;
00114 
00122   Matrix& get(const std::string& layer);
00123 
00130   const Matrix& operator [](const std::string& layer) const;
00131 
00139   Matrix& operator [](const std::string& layer);
00140 
00146   bool erase(const std::string& layer);
00147 
00152   const std::vector<std::string>& getLayers() const;
00153 
00160   void setBasicLayers(const std::vector<std::string>& basicLayers);
00161 
00166   const std::vector<std::string>& getBasicLayers() const;
00167 
00172   bool hasBasicLayers() const;
00173 
00181   bool hasSameLayers(const grid_map::GridMap& other) const;
00182 
00190   float& atPosition(const std::string& layer, const Position& position);
00191 
00200   float atPosition(const std::string& layer, const Position& position,
00201                    InterpolationMethods interpolationMethod = InterpolationMethods::INTER_NEAREST) const;
00202 
00210   float& at(const std::string& layer, const Index& index);
00211 
00219   float at(const std::string& layer, const Index& index) const;
00220 
00227   bool getIndex(const Position& position, Index& index) const;
00228 
00236   bool getPosition(const Index& index, Position& position) const;
00237 
00243   bool isInside(const Position& position) const;
00244 
00251   bool isValid(const Index& index) const;
00252 
00259   bool isValid(const Index& index, const std::string& layer) const;
00260 
00267   bool isValid(const Index& index, const std::vector<std::string>& layers) const;
00268 
00277   bool getPosition3(const std::string& layer, const Index& index, Position3& position) const;
00278 
00286   bool getVector(const std::string& layerPrefix, const Index& index,
00287                  Eigen::Vector3d& vector) const;
00288 
00299   GridMap getSubmap(const Position& position, const Length& length, bool& isSuccess) const;
00300 
00312   GridMap getSubmap(const Position& position, const Length& length, Index& indexInSubmap,
00313                     bool& isSuccess) const;
00314 
00328   GridMap getTransformedMap(const Eigen::Isometry3d& transform, const std::string& heightLayerName,
00329                             const std::string& newFrameId,
00330                             const double sampleRatio = 0.0) const;
00331 
00340    void setPosition(const Position& position);
00341 
00352   bool move(const Position& position, std::vector<BufferRegion>& newRegions);
00353 
00361   bool move(const Position& position);
00362 
00372   bool addDataFrom(const GridMap& other, bool extendMap,
00373                    bool overwriteData, bool copyAllLayers,
00374                    std::vector<std::string> layers = std::vector<std::string>());
00375 
00381   bool extendToInclude(const GridMap& other);
00382 
00387   void clear(const std::string& layer);
00388 
00393   void clearBasic();
00394 
00400   void clearAll();
00401 
00406   void setTimestamp(const Time timestamp);
00407 
00412   Time getTimestamp() const;
00413 
00417   void resetTimestamp();
00418 
00423   void setFrameId(const std::string& frameId);
00424 
00429   const std::string& getFrameId() const;
00430 
00435   const Length& getLength() const;
00436 
00441   const Position& getPosition() const;
00442 
00447   double getResolution() const;
00448 
00453   const Size& getSize() const;
00454 
00460   void setStartIndex(const Index& startIndex);
00461 
00466   const Index& getStartIndex() const;
00467 
00472   bool isDefaultStartIndex() const;
00473 
00477   void convertToDefaultStartIndex();
00478 
00479  private:
00480 
00486   void clearCols(unsigned int index, unsigned int nCols);
00487 
00493   void clearRows(unsigned int index, unsigned int nRows);
00494 
00502   bool atPositionLinearInterpolated(const std::string& layer, const Position& position, float& value) const;
00503 
00508   void resize(const Index& bufferSize);
00509 
00511   std::string frameId_;
00512 
00514   Time timestamp_;
00515 
00517   std::unordered_map<std::string, Matrix> data_;
00518 
00520   std::vector<std::string> layers_;
00521 
00525   std::vector<std::string> basicLayers_;
00526 
00528   Length length_;
00529 
00531   double resolution_;
00532 
00534   Position position_;
00535 
00537   Size size_;
00538 
00540   Index startIndex_;
00541 
00542  public:
00543   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00544 };
00545 
00546 } /* namespace */


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Tue Jul 9 2019 05:06:13