OctoMap.h
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #ifndef SRC_OCTOMAP_H_
29 #define SRC_OCTOMAP_H_
30 
31 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
32 
33 #include <octomap/ColorOcTree.h>
34 #include <octomap/OcTreeKey.h>
35 
36 #include <pcl/pcl_base.h>
37 #include <pcl/point_types.h>
38 
39 #include <rtabmap/core/Transform.h>
41 
42 #include <map>
43 #include <string>
44 
45 namespace rtabmap {
46 
47 // forward declaraton for "friend"
48 class RtabmapColorOcTree;
49 
51 {
52 public:
54 
55 public:
56  friend class RtabmapColorOcTree; // needs access to node children (inherited)
57 
60 
61  void setNodeRefId(int nodeRefId) {nodeRefId_ = nodeRefId;}
62  void setOccupancyType(char type) {type_=type;}
63  void setPointRef(const octomap::point3d & point) {pointRef_ = point;}
64  int getNodeRefId() const {return nodeRefId_;}
65  int getOccupancyType() const {return type_;}
66  const octomap::point3d & getPointRef() const {return pointRef_;}
67 
68  // following methods defined for octomap < 1.8 compatibility
69  RtabmapColorOcTreeNode* getChild(unsigned int i);
70  const RtabmapColorOcTreeNode* getChild(unsigned int i) const;
71  bool pruneNode();
72  void expandNode();
73  bool createChild(unsigned int i);
74 
75 private:
77  int type_; // -1=undefined, 0=empty, 100=obstacle, 1=ground
79 };
80 
81 // Same as official ColorOctree but using RtabmapColorOcTreeNode, which is inheriting ColorOcTreeNode
82 class RtabmapColorOcTree : public octomap::OccupancyOcTreeBase <RtabmapColorOcTreeNode> {
83 
84  public:
86  RtabmapColorOcTree(double resolution);
87  virtual ~RtabmapColorOcTree() {}
88 
91  RtabmapColorOcTree* create() const {return new RtabmapColorOcTree(resolution); }
92 
93  std::string getTreeType() const {return "ColorOcTree";} // same type as ColorOcTree to be compatible with ROS OctoMap msg
94 
101  virtual bool pruneNode(RtabmapColorOcTreeNode* node);
102 
103  virtual bool isNodeCollapsible(const RtabmapColorOcTreeNode* node) const;
104 
105  // set node color at given key or coordinate. Replaces previous color.
106  RtabmapColorOcTreeNode* setNodeColor(const octomap::OcTreeKey& key, uint8_t r,
107  uint8_t g, uint8_t b);
108 
110  float z, uint8_t r,
111  uint8_t g, uint8_t b) {
112  octomap::OcTreeKey key;
113  if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
114  return setNodeColor(key,r,g,b);
115  }
116 
117  // integrate color measurement at given key or coordinate. Average with previous color
118  RtabmapColorOcTreeNode* averageNodeColor(const octomap::OcTreeKey& key, uint8_t r,
119  uint8_t g, uint8_t b);
120 
122  float z, uint8_t r,
123  uint8_t g, uint8_t b) {
125  if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
126  return averageNodeColor(key,r,g,b);
127  }
128 
129  // integrate color measurement at given key or coordinate. Average with previous color
130  RtabmapColorOcTreeNode* integrateNodeColor(const octomap::OcTreeKey& key, uint8_t r,
131  uint8_t g, uint8_t b);
132 
134  float z, uint8_t r,
135  uint8_t g, uint8_t b) {
136  octomap::OcTreeKey key;
137  if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
138  return integrateNodeColor(key,r,g,b);
139  }
140 
141  // update inner nodes, sets color to average child color
142  void updateInnerOccupancy();
143 
144  protected:
145  void updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node, unsigned int depth);
146 
155  public:
157 
163  void ensureLinking() {};
164  };
167 
168  };
169 
171 public:
172  OctoMap(const ParametersMap & parameters);
173  OctoMap(float cellSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false, float updateError=0.01f);
174 
175  const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
176  void addToCache(int nodeId,
177  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
178  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
179  const pcl::PointXYZ & viewPoint);
180  void addToCache(int nodeId,
181  const cv::Mat & ground,
182  const cv::Mat & obstacles,
183  const cv::Mat & empty,
184  const cv::Point3f & viewPoint);
185  bool update(const std::map<int, Transform> & poses); // return true if map has changed
186 
187  const RtabmapColorOcTree * octree() const {return octree_;}
188 
189  pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
190  unsigned int treeDepth = 0,
191  std::vector<int> * obstacleIndices = 0,
192  std::vector<int> * emptyIndices = 0,
193  std::vector<int> * groundIndices = 0,
194  bool originalRefPoints = true,
195  std::vector<int> * frontierIndices = 0,
196  std::vector<double> * cloudProb = 0) const;
197 
198  cv::Mat createProjectionMap(
199  float & xMin,
200  float & yMin,
201  float & gridCellSize,
202  float minGridSize = 0.0f,
203  unsigned int treeDepth = 0);
204 
205  bool writeBinary(const std::string & path);
206 
207  virtual ~OctoMap();
208  void clear();
209 
210  void getGridMin(double & x, double & y, double & z) const {x=minValues_[0];y=minValues_[1];z=minValues_[2];}
211  void getGridMax(double & x, double & y, double & z) const {x=maxValues_[0];y=maxValues_[1];z=maxValues_[2];}
212 
213  void setMaxRange(float value) {rangeMax_ = value;}
214  void setRayTracing(bool enabled) {rayTracing_ = enabled;}
215  bool hasColor() const {return hasColor_;}
216 
217 private:
218  void updateMinMax(const octomap::point3d & point);
219 
220 private:
221  std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > cache_; // [id: < <ground, obstacles>, empty>]
222  std::map<int, std::pair<const pcl::PointCloud<pcl::PointXYZRGB>::Ptr, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cacheClouds_; // [id: <ground, obstacles>]
223  std::map<int, cv::Point3f> cacheViewPoints_;
225  std::map<int, Transform> addedNodes_;
226  bool hasColor_;
229  float rangeMax_;
231  double minValues_[3];
232  double maxValues_[3];
233 };
234 
235 } /* namespace rtabmap */
236 
237 #endif /* SRC_OCTOMAP_H_ */
#define NULL
RtabmapColorOcTreeNode * integrateNodeColor(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b)
Definition: OctoMap.h:133
RtabmapColorOcTree * create() const
Definition: OctoMap.h:91
f
virtual ~RtabmapColorOcTree()
Definition: OctoMap.h:87
octomap::point3d pointRef_
Definition: OctoMap.h:78
static StaticMemberInitializer RtabmapColorOcTreeMemberInit
static member to ensure static initialization (only once)
Definition: OctoMap.h:166
std::map< int, Transform > addedNodes_
Definition: OctoMap.h:225
bool createChild(unsigned int i)
Definition: OctoMap.cpp:97
bool fullUpdate_
Definition: OctoMap.h:227
std::map< int, cv::Point3f > cacheViewPoints_
Definition: OctoMap.h:223
RtabmapColorOcTreeNode * getChild(unsigned int i)
Definition: OctoMap.cpp:44
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
RtabmapColorOcTreeNode(const RtabmapColorOcTreeNode &rhs)
Definition: OctoMap.h:59
void setPointRef(const octomap::point3d &point)
Definition: OctoMap.h:63
void setNodeRefId(int nodeRefId)
Definition: OctoMap.h:61
RtabmapColorOcTreeNode * setNodeColor(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b)
Definition: OctoMap.h:109
bool hasColor() const
Definition: OctoMap.h:215
detail::uint8 uint8_t
Definition: fwd.hpp:908
std::string getTreeType() const
Definition: OctoMap.h:93
#define RTABMAP_EXP
Definition: RtabmapExp.h:38
const std::map< int, Transform > & addedNodes() const
Definition: OctoMap.h:175
std::map< int, std::pair< std::pair< cv::Mat, cv::Mat >, cv::Mat > > cache_
Definition: OctoMap.h:221
const octomap::point3d & getPointRef() const
Definition: OctoMap.h:66
int getOccupancyType() const
Definition: OctoMap.h:65
std::map< int, std::pair< const pcl::PointCloud< pcl::PointXYZRGB >::Ptr, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr > > cacheClouds_
Definition: OctoMap.h:222
friend class RtabmapColorOcTree
Definition: OctoMap.h:56
void getGridMax(double &x, double &y, double &z) const
Definition: OctoMap.h:211
RtabmapColorOcTree * octree_
Definition: OctoMap.h:224
const RtabmapColorOcTree * octree() const
Definition: OctoMap.h:187
void setOccupancyType(char type)
Definition: OctoMap.h:62
void setMaxRange(float value)
Definition: OctoMap.h:213
void getGridMin(double &x, double &y, double &z) const
Definition: OctoMap.h:210
float rangeMax_
Definition: OctoMap.h:229
bool rayTracing_
Definition: OctoMap.h:230
RtabmapColorOcTreeNode * averageNodeColor(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b)
Definition: OctoMap.h:121
void setRayTracing(bool enabled)
Definition: OctoMap.h:214
float updateError_
Definition: OctoMap.h:228


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59