grid_map.h
Go to the documentation of this file.
1 // -*- mode: C++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 #ifndef JSK_RECOGNITION_UTILS_GRID_MAP_H_
36 #define JSK_RECOGNITION_UTILS_GRID_MAP_H_
37 #include <jsk_recognition_msgs/SparseOccupancyGrid.h>
38 
41 #include <pcl/point_types.h>
42 #include <pcl/point_cloud.h>
43 #include <map>
44 #include <set>
45 #include <Eigen/Geometry>
46 #include <boost/tuple/tuple.hpp>
47 
49 #include <opencv2/opencv.hpp>
50 
51 namespace jsk_recognition_utils
52 {
53 
54  // infinity range, might be slow...
55  class GridMap
56  {
57  public:
59  typedef std::set<int> RowIndices;
60  typedef std::map<int, RowIndices> Columns;
61  typedef Columns::iterator ColumnIterator;
62  typedef std::set<int>::iterator RowIterator;
63  GridMap(double resolution, const std::vector<float>& coefficients);
64  virtual ~GridMap();
65  virtual void registerPoint(const pcl::PointXYZRGB& point);
66  virtual std::vector<GridIndex::Ptr> registerLine(const pcl::PointXYZRGB& from, const pcl::PointXYZRGB& to);
67  virtual void removeIndex(const GridIndex::Ptr& index);
68  virtual void registerPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud);
69  virtual GridIndex::Ptr registerIndex(const GridIndex::Ptr& index);
70  virtual GridIndex::Ptr registerIndex(const int x, const int y);
71  virtual void pointToIndex(const pcl::PointXYZRGB& point, GridIndex::Ptr index);
72  virtual void pointToIndex(const Eigen::Vector3f& point, GridIndex::Ptr index);
73  virtual void indicesToPointCloud(const std::vector<GridIndex::Ptr>& indices,
74  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud);
75  virtual bool getValue(const GridIndex::Ptr& index);
76  virtual bool getValue(const GridIndex& index);
77  virtual bool getValue(const int x, const int y);
78  virtual void gridToPoint(GridIndex::Ptr index, Eigen::Vector3f& pos);
79  virtual void gridToPoint(const GridIndex& index, Eigen::Vector3f& pos);
80  virtual void gridToPoint2(const GridIndex& index, Eigen::Vector3f& pos);
81  virtual void fillRegion(const Eigen::Vector3f& start, std::vector<GridIndex::Ptr>& output);
82  virtual void fillRegion(const GridIndex::Ptr start, std::vector<GridIndex::Ptr>& output);
83  // toMsg does not fill header, be carefull
84  virtual void originPose(Eigen::Affine3f& output);
85  virtual void originPose(Eigen::Affine3d& output);
86  virtual void toMsg(jsk_recognition_msgs::SparseOccupancyGrid& grid);
87  virtual Plane toPlane();
88  virtual Plane::Ptr toPlanePtr();
89  virtual void vote();
90  virtual unsigned int getVoteNum();
91  virtual void setGeneration(unsigned int generation);
92  virtual unsigned int getGeneration();
93  virtual std::vector<float> getCoefficients();
94  virtual bool isBinsOccupied(const Eigen::Vector3f& p);
95  virtual int normalizedWidth();
96  virtual int normalizedHeight();
97  virtual boost::tuple<int, int> minMaxX();
98  virtual boost::tuple<int, int> minMaxY();
99  virtual int widthOffset();
100  virtual int heightOffset();
101  virtual int normalizedIndex(int width_offset, int height_offset,
102  int step,
103  int elem_size,
104  int original_x, int original_y);
105  virtual cv::Mat toImage();
106  virtual bool check4Neighbor(int x, int y);
108  virtual pcl::PointCloud<pcl::PointXYZ>::Ptr toPointCloud();
109  virtual void decrease(int i);
110  virtual void add(GridMap& other);
111  protected:
112  virtual void decreaseOne();
113 
114  double resolution_;
115  Eigen::Vector3f O_;
116 
117  // plane parameter
118  Eigen::Vector3f normal_;
119  double d_;
120 
121  Eigen::Vector3f ex_, ey_;
122 
123  std::vector<GridLine::Ptr> lines_;
124  Columns data_;
125  unsigned int vote_;
126  unsigned int generation_;
127  private:
128  };
129 
130 }
131 
132 #endif
virtual boost::tuple< int, int > minMaxX()
Definition: grid_map.cpp:466
virtual void add(GridMap &other)
Definition: grid_map.cpp:608
GridMap(double resolution, const std::vector< float > &coefficients)
Definition: grid_map.cpp:49
virtual void registerPoint(const pcl::PointXYZRGB &point)
Definition: grid_map.cpp:95
virtual void decrease(int i)
Definition: grid_map.cpp:601
virtual void indicesToPointCloud(const std::vector< GridIndex::Ptr > &indices, pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud)
Definition: grid_map.cpp:344
virtual std::vector< GridIndex::Ptr > registerLine(const pcl::PointXYZRGB &from, const pcl::PointXYZRGB &to)
Definition: grid_map.cpp:103
Columns::iterator ColumnIterator
Definition: grid_map.h:61
virtual void removeIndex(const GridIndex::Ptr &index)
Definition: grid_map.cpp:446
virtual bool getValue(const GridIndex::Ptr &index)
Definition: grid_map.cpp:288
virtual cv::Mat toImage()
Definition: grid_map.cpp:544
std::set< int >::iterator RowIterator
Definition: grid_map.h:62
virtual void pointToIndex(const pcl::PointXYZRGB &point, GridIndex::Ptr index)
Definition: grid_map.cpp:218
virtual pcl::PointCloud< pcl::PointXYZ >::Ptr toPointCloud()
Definition: grid_map.cpp:629
virtual void registerPointCloud(pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud)
Definition: grid_map.cpp:210
virtual bool check4Neighbor(int x, int y)
Definition: grid_map.cpp:568
virtual bool isBinsOccupied(const Eigen::Vector3f &p)
Definition: grid_map.cpp:458
virtual std::vector< float > getCoefficients()
Definition: grid_map.cpp:417
virtual void gridToPoint(GridIndex::Ptr index, Eigen::Vector3f &pos)
Definition: grid_map.cpp:229
boost::shared_ptr< GridMap > Ptr
Definition: grid_map.h:58
virtual Plane::Ptr toPlanePtr()
Definition: grid_map.cpp:410
std::map< int, RowIndices > Columns
Definition: grid_map.h:60
virtual void fillRegion(const Eigen::Vector3f &start, std::vector< GridIndex::Ptr > &output)
Definition: grid_map.cpp:337
virtual void setGeneration(unsigned int generation)
Definition: grid_map.cpp:437
virtual ConvexPolygon::Ptr toConvexPolygon()
Definition: grid_map.cpp:652
virtual unsigned int getGeneration()
Definition: grid_map.cpp:441
virtual unsigned int getVoteNum()
Definition: grid_map.cpp:432
std::set< int > RowIndices
Definition: grid_map.h:59
virtual void originPose(Eigen::Affine3f &output)
Definition: grid_map.cpp:359
virtual void gridToPoint2(const GridIndex &index, Eigen::Vector3f &pos)
Definition: grid_map.cpp:240
virtual int normalizedIndex(int width_offset, int height_offset, int step, int elem_size, int original_x, int original_y)
Definition: grid_map.cpp:533
virtual void toMsg(jsk_recognition_msgs::SparseOccupancyGrid &grid)
Definition: grid_map.cpp:379
std::vector< GridLine::Ptr > lines_
Definition: grid_map.h:123
virtual GridIndex::Ptr registerIndex(const GridIndex::Ptr &index)
Definition: grid_map.cpp:90
virtual boost::tuple< int, int > minMaxY()
Definition: grid_map.cpp:484


jsk_recognition_utils
Author(s):
autogenerated on Tue Mar 3 2020 03:59:38