global_map/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/rtabmap_core_export.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 #include <rtabmap/core/GlobalMap.h>
42 
43 #include <map>
44 #include <unordered_set>
45 #include <string>
46 #include <queue>
47 
48 namespace rtabmap {
49 
50 // forward declaraton for "friend"
51 class RtabmapColorOcTree;
52 
53 class RtabmapColorOcTreeNode : public octomap::ColorOcTreeNode
54 {
55 public:
57 
58 public:
59  friend class RtabmapColorOcTree; // needs access to node children (inherited)
60 
61  RtabmapColorOcTreeNode() : ColorOcTreeNode(), nodeRefId_(0), type_(kTypeUnknown) {}
62  RtabmapColorOcTreeNode(const RtabmapColorOcTreeNode& rhs) : ColorOcTreeNode(rhs), nodeRefId_(rhs.nodeRefId_), type_(rhs.type_) {}
63 
64  void setNodeRefId(int nodeRefId) {nodeRefId_ = nodeRefId;}
66  void setPointRef(const octomap::point3d & point) {pointRef_ = point;}
67  int getNodeRefId() const {return nodeRefId_;}
68  int getOccupancyType() const {return type_;}
69  const octomap::point3d & getPointRef() const {return pointRef_;}
70 
71  // following methods defined for octomap < 1.8 compatibility
72  RtabmapColorOcTreeNode* getChild(unsigned int i);
73  const RtabmapColorOcTreeNode* getChild(unsigned int i) const;
74  bool pruneNode();
75  void expandNode();
76  bool createChild(unsigned int i);
77 
79 
80 private:
82  int type_; // -1=undefined, 0=empty, 100=obstacle, 1=ground
83  octomap::point3d pointRef_;
84 };
85 
86 // Same as official ColorOctree but using RtabmapColorOcTreeNode, which is inheriting ColorOcTreeNode
87 class RtabmapColorOcTree : public octomap::OccupancyOcTreeBase <RtabmapColorOcTreeNode> {
88 
89  public:
91  RtabmapColorOcTree(double resolution);
92  virtual ~RtabmapColorOcTree() {}
93 
96  RtabmapColorOcTree* create() const {return new RtabmapColorOcTree(resolution); }
97 
98  std::string getTreeType() const {return "ColorOcTree";} // same type as ColorOcTree to be compatible with ROS OctoMap msg
99 
106  virtual bool pruneNode(RtabmapColorOcTreeNode* node);
107 
108  virtual bool isNodeCollapsible(const RtabmapColorOcTreeNode* node) const;
109 
110  // set node color at given key or coordinate. Replaces previous color.
111  RtabmapColorOcTreeNode* setNodeColor(const octomap::OcTreeKey& key, uint8_t r,
112  uint8_t g, uint8_t b);
113 
115  float z, uint8_t r,
116  uint8_t g, uint8_t b) {
117  octomap::OcTreeKey key;
118  if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
119  return setNodeColor(key,r,g,b);
120  }
121 
122  // integrate color measurement at given key or coordinate. Average with previous color
123  RtabmapColorOcTreeNode* averageNodeColor(const octomap::OcTreeKey& key, uint8_t r,
124  uint8_t g, uint8_t b);
125 
127  float z, uint8_t r,
128  uint8_t g, uint8_t b) {
129  octomap:: OcTreeKey key;
130  if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
131  return averageNodeColor(key,r,g,b);
132  }
133 
134  // integrate color measurement at given key or coordinate. Average with previous color
135  RtabmapColorOcTreeNode* integrateNodeColor(const octomap::OcTreeKey& key, uint8_t r,
136  uint8_t g, uint8_t b);
137 
139  float z, uint8_t r,
140  uint8_t g, uint8_t b) {
141  octomap::OcTreeKey key;
142  if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
143  return integrateNodeColor(key,r,g,b);
144  }
145 
146  // update inner nodes, sets color to average child color
147  void updateInnerOccupancy();
148 
149  protected:
150  void updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node, unsigned int depth);
151 
160  public:
162 
168  void ensureLinking() {};
169  };
172 
173  };
174 
175 class RTABMAP_CORE_EXPORT OctoMap : public GlobalMap {
176 public:
177  OctoMap(const LocalGridCache * cache, const ParametersMap & parameters = ParametersMap());
178 
179  const RtabmapColorOcTree * octree() const {return octree_;}
180 
181  pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
182  unsigned int treeDepth = 0,
183  std::vector<int> * obstacleIndices = 0,
184  std::vector<int> * emptyIndices = 0,
185  std::vector<int> * groundIndices = 0,
186  bool originalRefPoints = true,
187  std::vector<int> * frontierIndices = 0,
188  std::vector<double> * cloudProb = 0) const;
189 
190  cv::Mat createProjectionMap(
191  float & xMin,
192  float & yMin,
193  float & gridCellSize,
194  float minGridSize = 0.0f,
195  unsigned int treeDepth = 0);
196 
197  bool writeBinary(const std::string & path);
198 
199  virtual ~OctoMap();
200  virtual void clear();
201  virtual unsigned long getMemoryUsed() const;
202 
203  bool hasColor() const {return hasColor_;}
204 
205  static std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> findEmptyNode(RtabmapColorOcTree* octree_, unsigned int treeDepth, octomap::point3d startPosition);
206  static void floodFill(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition, std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> & EmptyNodes,std::queue<octomap::point3d>& positionToExplore);
207  static bool isNodeVisited(std::unordered_set<octomap::OcTreeKey,octomap::OcTreeKey::KeyHash> const & EmptyNodes,octomap::OcTreeKey const key);
208  static octomap::point3d findCloseEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition);
209  static bool isValidEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition);
210 
211 protected:
212  virtual void assemble(const std::list<std::pair<int, Transform> > & newPoses);
213 
214 private:
215  void updateMinMax(const octomap::point3d & point);
216 
217 private:
219  bool hasColor_;
220  float rangeMax_;
222  unsigned int emptyFloodFillDepth_;
223 };
224 
225 } /* namespace rtabmap */
226 
227 #endif /* SRC_OCTOMAP_H_ */
rtabmap::RtabmapColorOcTree::integrateNodeColor
RtabmapColorOcTreeNode * integrateNodeColor(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b)
Definition: global_map/OctoMap.h:138
rtabmap::OctoMap::rangeMax_
float rangeMax_
Definition: global_map/OctoMap.h:220
rtabmap::RtabmapColorOcTreeNode::getPointRef
const octomap::point3d & getPointRef() const
Definition: global_map/OctoMap.h:69
rtabmap::RtabmapColorOcTreeNode::pruneNode
bool pruneNode()
Definition: OctoMap.cpp:63
b
Array< int, 3, 1 > b
rtabmap::RtabmapColorOcTreeNode::setNodeRefId
void setNodeRefId(int nodeRefId)
Definition: global_map/OctoMap.h:64
rtabmap::RtabmapColorOcTreeNode::expandNode
void expandNode()
Definition: OctoMap.cpp:84
rtabmap::RtabmapColorOcTreeNode::getNodeRefId
int getNodeRefId() const
Definition: global_map/OctoMap.h:67
rtabmap::RtabmapColorOcTreeNode::createChild
bool createChild(unsigned int i)
Definition: OctoMap.cpp:98
rtabmap::OctoMap::octree_
RtabmapColorOcTree * octree_
Definition: global_map/OctoMap.h:218
rtabmap::RtabmapColorOcTreeNode::kTypeEmpty
@ kTypeEmpty
Definition: global_map/OctoMap.h:56
rtabmap::RtabmapColorOcTreeNode::kTypeObstacle
@ kTypeObstacle
Definition: global_map/OctoMap.h:56
Transform.h
rtabmap::OctoMap::emptyFloodFillDepth_
unsigned int emptyFloodFillDepth_
Definition: global_map/OctoMap.h:222
type
y
Matrix3f y
rtabmap::LocalGridCache
Definition: LocalGrid.h:56
rtabmap::OctoMap::octree
const RtabmapColorOcTree * octree() const
Definition: global_map/OctoMap.h:179
rtabmap::RtabmapColorOcTreeNode::getChild
RtabmapColorOcTreeNode * getChild(unsigned int i)
Definition: OctoMap.cpp:45
rtabmap::RtabmapColorOcTree::RtabmapColorOcTreeMemberInit
static StaticMemberInitializer RtabmapColorOcTreeMemberInit
static member to ensure static initialization (only once)
Definition: global_map/OctoMap.h:171
rtabmap::RtabmapColorOcTreeNode::kTypeUnknown
@ kTypeUnknown
Definition: global_map/OctoMap.h:56
rtabmap::RtabmapColorOcTree::~RtabmapColorOcTree
virtual ~RtabmapColorOcTree()
Definition: global_map/OctoMap.h:92
rtabmap::RtabmapColorOcTreeNode::OccupancyType
OccupancyType
Definition: global_map/OctoMap.h:56
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::RtabmapColorOcTree::setNodeColor
RtabmapColorOcTreeNode * setNodeColor(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b)
Definition: global_map/OctoMap.h:114
GlobalMap.h
Parameters.h
rtabmap::RtabmapColorOcTree::StaticMemberInitializer::StaticMemberInitializer
StaticMemberInitializer()
Definition: OctoMap.cpp:271
point
point
rtabmap::RtabmapColorOcTreeNode::setOccupancyType
void setOccupancyType(char type)
Definition: global_map/OctoMap.h:65
rtabmap::RtabmapColorOcTree::updateInnerOccupancy
void updateInnerOccupancy()
Definition: OctoMap.cpp:233
g
float g
rtabmap::GlobalMap
Definition: GlobalMap.h:40
rtabmap::RtabmapColorOcTreeNode::RtabmapColorOcTreeNode
RtabmapColorOcTreeNode()
Definition: global_map/OctoMap.h:61
rtabmap::RtabmapColorOcTree::create
RtabmapColorOcTree * create() const
Definition: global_map/OctoMap.h:96
rtabmap::OctoMap::rayTracing_
bool rayTracing_
Definition: global_map/OctoMap.h:221
rtabmap::RtabmapColorOcTree::isNodeCollapsible
virtual bool isNodeCollapsible(const RtabmapColorOcTreeNode *node) const
Definition: OctoMap.cpp:167
rtabmap::RtabmapColorOcTree::updateInnerOccupancyRecurs
void updateInnerOccupancyRecurs(RtabmapColorOcTreeNode *node, unsigned int depth)
Definition: OctoMap.cpp:237
rtabmap::RtabmapColorOcTree::integrateNodeColor
RtabmapColorOcTreeNode * integrateNodeColor(const octomap::OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
Definition: OctoMap.cpp:208
z
z
x
x
rtabmap::RtabmapColorOcTreeNode::kTypeGround
@ kTypeGround
Definition: global_map/OctoMap.h:56
rtabmap::OctoMap::hasColor_
bool hasColor_
Definition: global_map/OctoMap.h:219
rtabmap::OctoMap::hasColor
bool hasColor() const
Definition: global_map/OctoMap.h:203
key
const gtsam::Symbol key( 'X', 0)
rtabmap::RtabmapColorOcTreeNode::pointRef_
octomap::point3d pointRef_
Definition: global_map/OctoMap.h:83
rtabmap::RtabmapColorOcTreeNode::updateOccupancyTypeChildren
void updateOccupancyTypeChildren()
Definition: OctoMap.cpp:109
rtabmap::RtabmapColorOcTree
Definition: global_map/OctoMap.h:87
rtabmap::RtabmapColorOcTreeNode::type_
int type_
Definition: global_map/OctoMap.h:82
rtabmap::RtabmapColorOcTree::StaticMemberInitializer::ensureLinking
void ensureLinking()
Definition: global_map/OctoMap.h:168
rtabmap::RtabmapColorOcTree::RtabmapColorOcTree
RtabmapColorOcTree(double resolution)
Default constructor, sets resolution of leafs.
Definition: OctoMap.cpp:126
rtabmap::RtabmapColorOcTree::getTreeType
std::string getTreeType() const
Definition: global_map/OctoMap.h:98
rtabmap::RtabmapColorOcTree::setNodeColor
RtabmapColorOcTreeNode * setNodeColor(const octomap::OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
Definition: OctoMap.cpp:131
uint8_t
::uint8_t uint8_t
rtabmap::RtabmapColorOcTreeNode::RtabmapColorOcTreeNode
RtabmapColorOcTreeNode(const RtabmapColorOcTreeNode &rhs)
Definition: global_map/OctoMap.h:62
rtabmap::RtabmapColorOcTreeNode::getOccupancyType
int getOccupancyType() const
Definition: global_map/OctoMap.h:68
rtabmap::RtabmapColorOcTree::averageNodeColor
RtabmapColorOcTreeNode * averageNodeColor(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b)
Definition: global_map/OctoMap.h:126
rtabmap::RtabmapColorOcTreeNode
Definition: global_map/OctoMap.h:53
rtabmap::RtabmapColorOcTree::pruneNode
virtual bool pruneNode(RtabmapColorOcTreeNode *node)
Definition: OctoMap.cpp:142
rtabmap::OctoMap
Definition: global_map/OctoMap.h:175
rtabmap::RtabmapColorOcTreeNode::nodeRefId_
int nodeRefId_
Definition: global_map/OctoMap.h:81
NULL
#define NULL
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::RtabmapColorOcTree::StaticMemberInitializer
Definition: global_map/OctoMap.h:159
rtabmap::RtabmapColorOcTree::averageNodeColor
RtabmapColorOcTreeNode * averageNodeColor(const octomap::OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
Definition: OctoMap.cpp:191
rtabmap::RtabmapColorOcTreeNode::setPointRef
void setPointRef(const octomap::point3d &point)
Definition: global_map/OctoMap.h:66


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:13