GridMap2D.h
Go to the documentation of this file.
00001 /*
00002  * A simple 2D gridmap structure
00003  *
00004  * Copyright 2011 Armin Hornung, University of Freiburg
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of the University of Freiburg nor the names of its
00015  *       contributors may be used to endorse or promote products derived from
00016  *       this software without specific prior written permission.
00017  *
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 
00032 #ifndef GRIDMAP2D_GRIDMAP2D_H_
00033 #define GRIDMAP2D_GRIDMAP2D_H_
00034 
00035 #include <opencv2/core/core.hpp>
00036 #include <opencv2/imgproc/imgproc.hpp>
00037 #include <nav_msgs/OccupancyGrid.h>
00038 
00039 
00040 
00041 
00042 
00043 namespace gridmap_2d{
00049 class GridMap2D {
00050 public:
00051   GridMap2D();
00052   GridMap2D(const nav_msgs::OccupancyGridConstPtr& gridMap);
00053   virtual ~GridMap2D();
00054 
00055   void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const;
00056   bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const;
00057   void worldToMapNoBounds(double wx, double wy, unsigned int& mx, unsigned int& my) const;
00058 
00060   bool inMapBounds(double wx, double wy) const;
00061 
00065   void inflateMap(double inflationRaduis);
00066 
00068   inline double worldDist(unsigned x1, unsigned y1, unsigned x2, unsigned y2){
00069     return worldDist(cv::Point(x1, y1), cv::Point(x2, y2));
00070   }
00071 
00072   inline double worldDist(const cv::Point& p1, const cv::Point& p2){
00073     return GridMap2D::pointDist(p1, p2) * m_mapInfo.resolution;
00074   }
00075 
00077   static inline double pointDist(const cv::Point& p1, const cv::Point& p2){
00078     return sqrt(pointDist2(p1, p2));
00079   }
00080 
00082   static inline double pointDist2(const cv::Point& p1, const cv::Point& p2){
00083     return (p1.x - p2.x)*(p1.x - p2.x) + (p1.y - p2.y)*(p1.y - p2.y);
00084   }
00085 
00087   float distanceMapAt(double wx, double wy) const;
00088 
00090   float distanceMapAtCell(unsigned int mx, unsigned int my) const;
00091 
00093   uchar binaryMapAt(double wx, double wy) const;
00094 
00096   uchar binaryMapAtCell(unsigned int mx, unsigned int my) const;
00097 
00100   bool isOccupiedAt(double wx, double wy) const;
00101 
00103   bool isOccupiedAtCell(unsigned int mx, unsigned int my) const;
00104 
00106   void setMap(const nav_msgs::OccupancyGridConstPtr& gridMap);
00107 
00109   void setMap(const cv::Mat& binaryMap);
00110 
00111   inline const nav_msgs::MapMetaData& getInfo() const {return m_mapInfo;}
00112   inline float getResolution() const {return m_mapInfo.resolution; };
00114   inline const std::string getFrameID() const {return m_frameId;}
00116   const cv::Mat& distanceMap() const {return m_distMap;}
00118   const cv::Mat& binaryMap() const {return m_binaryMap;}
00120   inline const CvSize size() const {return m_binaryMap.size();};
00121 
00122 
00123 protected:
00124   cv::Mat m_binaryMap;  
00125   cv::Mat m_distMap;            
00126   nav_msgs::MapMetaData m_mapInfo;
00127   std::string m_frameId;        
00128 
00129 };
00130 
00131 typedef boost::shared_ptr< GridMap2D> GridMap2DPtr;
00132 }
00133 
00134 #endif /* GRIDMAP2D_H_ */


gridmap_2d
Author(s): Armin Hornung
autogenerated on Mon Oct 6 2014 00:41:23