hpp-fcl  1.8.0
HPP fork of FCL -- The Flexible Collision Library
octree.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2015, Open Source Robotics Foundation
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/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation 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 
39 #ifndef HPP_FCL_OCTREE_H
40 #define HPP_FCL_OCTREE_H
41 
42 
43 #include <boost/array.hpp>
44 
45 #include <octomap/octomap.h>
46 #include <hpp/fcl/fwd.hh>
47 #include <hpp/fcl/BV/AABB.h>
49 
50 namespace hpp
51 {
52 namespace fcl
53 {
54 
57 {
58 private:
59  shared_ptr<const octomap::OcTree> tree;
60 
61  FCL_REAL default_occupancy;
62 
63  FCL_REAL occupancy_threshold;
64  FCL_REAL free_threshold;
65 
66 public:
67 
68  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
69 
70  typedef octomap::OcTreeNode OcTreeNode;
71 
73  OcTree(FCL_REAL resolution) : tree(shared_ptr<const octomap::OcTree>(new octomap::OcTree(resolution)))
74  {
75  default_occupancy = tree->getOccupancyThres();
76 
77  // default occupancy/free threshold is consistent with default setting from octomap
78  occupancy_threshold = tree->getOccupancyThres();
79  free_threshold = 0;
80  }
81 
83  OcTree(const shared_ptr<const octomap::OcTree>& tree_) : tree(tree_)
84  {
85  default_occupancy = tree->getOccupancyThres();
86 
87  // default occupancy/free threshold is consistent with default setting from octomap
88  occupancy_threshold = tree->getOccupancyThres();
89  free_threshold = 0;
90  }
91 
92  OcTree(const OcTree & other)
93  : CollisionGeometry(other)
94  , tree(other.tree)
95  , default_occupancy(other.default_occupancy)
96  , occupancy_threshold(other.occupancy_threshold)
97  , free_threshold(other.free_threshold)
98  {
99  }
100 
101  OcTree * clone() const { return new OcTree(*this); }
102 
103  void exportAsObjFile(const std::string& filename) const;
104 
107  {
108  aabb_local = getRootBV();
109  aabb_center = aabb_local.center();
110  aabb_radius = (aabb_local.min_ - aabb_center).norm();
111  }
112 
114  AABB getRootBV() const
115  {
116  FCL_REAL delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2;
117 
118  // std::cout << "octree size " << delta << std::endl;
119  return AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta));
120  }
121 
123  unsigned int getTreeDepth() const
124  {
125  return tree->getTreeDepth();
126  }
127 
129  OcTreeNode* getRoot() const
130  {
131  return tree->getRoot();
132  }
133 
135  bool isNodeOccupied(const OcTreeNode* node) const
136  {
137  // return tree->isNodeOccupied(node);
138  return node->getOccupancy() >= occupancy_threshold;
139  }
140 
142  bool isNodeFree(const OcTreeNode* node) const
143  {
144  // return false; // default no definitely free node
145  return node->getOccupancy() <= free_threshold;
146  }
147 
149  bool isNodeUncertain(const OcTreeNode* node) const
150  {
151  return (!isNodeOccupied(node)) && (!isNodeFree(node));
152  }
153 
156  std::vector<boost::array<FCL_REAL, 6> > toBoxes() const
157  {
158  std::vector<boost::array<FCL_REAL, 6> > boxes;
159  boxes.reserve(tree->size() / 2);
160  for(octomap::OcTree::iterator it =
161  tree->begin((unsigned char) tree->getTreeDepth()), end = tree->end();
162  it != end;
163  ++it)
164  {
165  // if(tree->isNodeOccupied(*it))
166  if(isNodeOccupied(&*it))
167  {
168  FCL_REAL size = it.getSize();
169  FCL_REAL x = it.getX();
170  FCL_REAL y = it.getY();
171  FCL_REAL z = it.getZ();
172  FCL_REAL c = (*it).getOccupancy();
173  FCL_REAL t = tree->getOccupancyThres();
174 
175  boost::array<FCL_REAL, 6> box = {{x, y, z, size, c, t}};
176  boxes.push_back(box);
177  }
178  }
179  return boxes;
180  }
181 
184  {
185  return occupancy_threshold;
186  }
187 
190  {
191  return free_threshold;
192  }
193 
195  {
196  return default_occupancy;
197  }
198 
200  {
201  default_occupancy = d;
202  }
203 
205  {
206  occupancy_threshold = d;
207  }
208 
210  {
211  free_threshold = d;
212  }
213 
215  OcTreeNode* getNodeChild(OcTreeNode* node, unsigned int childIdx)
216  {
217 #if OCTOMAP_VERSION_AT_LEAST(1,8,0)
218  return tree->getNodeChild(node, childIdx);
219 #else
220  return node->getChild(childIdx);
221 #endif
222  }
223 
225  const OcTreeNode* getNodeChild(const OcTreeNode* node, unsigned int childIdx) const
226  {
227 #if OCTOMAP_VERSION_AT_LEAST(1,8,0)
228  return tree->getNodeChild(node, childIdx);
229 #else
230  return node->getChild(childIdx);
231 #endif
232  }
233 
235  bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const
236  {
237 #if OCTOMAP_VERSION_AT_LEAST(1,8,0)
238  return tree->nodeChildExists(node, childIdx);
239 #else
240  return node->childExists(childIdx);
241 #endif
242  }
243 
245  bool nodeHasChildren(const OcTreeNode* node) const
246  {
247 #if OCTOMAP_VERSION_AT_LEAST(1,8,0)
248  return tree->nodeHasChildren(node);
249 #else
250  return node->hasChildren();
251 #endif
252  }
253 
255  OBJECT_TYPE getObjectType() const { return OT_OCTREE; }
256 
258  NODE_TYPE getNodeType() const { return GEOM_OCTREE; }
259 };
260 
262 static inline void computeChildBV(const AABB& root_bv, unsigned int i, AABB& child_bv)
263 {
264  if(i&1)
265  {
266  child_bv.min_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
267  child_bv.max_[0] = root_bv.max_[0];
268  }
269  else
270  {
271  child_bv.min_[0] = root_bv.min_[0];
272  child_bv.max_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
273  }
274 
275  if(i&2)
276  {
277  child_bv.min_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
278  child_bv.max_[1] = root_bv.max_[1];
279  }
280  else
281  {
282  child_bv.min_[1] = root_bv.min_[1];
283  child_bv.max_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
284  }
285 
286  if(i&4)
287  {
288  child_bv.min_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
289  child_bv.max_[2] = root_bv.max_[2];
290  }
291  else
292  {
293  child_bv.min_[2] = root_bv.min_[2];
294  child_bv.max_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
295  }
296 }
297 
306  HPP_FCL_DLLAPI OcTreePtr_t makeOctree(const Eigen::Matrix<FCL_REAL,Eigen::Dynamic,3> & point_cloud,
307  const FCL_REAL resolution);
308 
309 }
310 
311 } // namespace hpp
312 
313 #endif
bool isNodeFree(const OcTreeNode *node) const
whether one node is completely free
Definition: octree.h:142
OcTreeNode * getRoot() const
get the root node of the octree
Definition: octree.h:129
FCL_REAL getOccupancyThres() const
the threshold used to decide whether one node is occupied, this is NOT the octree occupied_thresold ...
Definition: octree.h:183
Vec3f min_
The min point in the AABB.
Definition: AABB.h:60
std::vector< boost::array< FCL_REAL, 6 > > toBoxes() const
transform the octree into a bunch of boxes; uncertainty information is kept in the boxes...
Definition: octree.h:156
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:55
Main namespace.
Definition: AABB.h:43
OcTree(FCL_REAL resolution)
construct octree with a given resolution
Definition: octree.h:73
FCL_REAL getDefaultOccupancy() const
Definition: octree.h:194
AABB getRootBV() const
get the bounding volume for the root
Definition: octree.h:114
unsigned int getTreeDepth() const
Returns the depth of octree.
Definition: octree.h:123
OcTreePtr_t makeOctree(const Eigen::Matrix< FCL_REAL, Eigen::Dynamic, 3 > &point_cloud, const FCL_REAL resolution)
Build an OcTree from a point cloud and a given resolution.
FCL_REAL getFreeThres() const
the threshold used to decide whether one node is free, this is NOT the octree free_threshold ...
Definition: octree.h:189
Definition: collision_object.h:59
void setOccupancyThres(FCL_REAL d)
Definition: octree.h:204
Octree is one type of collision geometry which can encode uncertainty information in the sensor data...
Definition: octree.h:56
bool isNodeUncertain(const OcTreeNode *node) const
whether one node is uncertain
Definition: octree.h:149
double FCL_REAL
Definition: data_types.h:66
shared_ptr< OcTree > OcTreePtr_t
Definition: fwd.hh:85
OcTree * clone() const
Clone *this into a new CollisionGeometry.
Definition: octree.h:101
OcTreeNode * getNodeChild(OcTreeNode *node, unsigned int childIdx)
Definition: octree.h:215
const OcTreeNode * getNodeChild(const OcTreeNode *node, unsigned int childIdx) const
Definition: octree.h:225
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:55
Vec3f max_
The max point in the AABB.
Definition: AABB.h:62
Definition: collision_object.h:55
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef octomap::OcTreeNode OcTreeNode
Definition: octree.h:70
OBJECT_TYPE getObjectType() const
return object type, it is an octree
Definition: octree.h:255
void setCellDefaultOccupancy(FCL_REAL d)
Definition: octree.h:199
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, capsule, cone, cylinder, convex, plane, triangle), and octree
Definition: collision_object.h:58
OcTree(const shared_ptr< const octomap::OcTree > &tree_)
construct octree from octomap
Definition: octree.h:83
NODE_TYPE getNodeType() const
return node type, it is an octree
Definition: octree.h:258
void computeLocalAABB()
compute the AABB for the octree in its local coordinate system
Definition: octree.h:106
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
The geometry for the object for collision or distance computation.
Definition: collision_object.h:66
bool isNodeOccupied(const OcTreeNode *node) const
whether one node is completely occupied
Definition: octree.h:135
bool nodeChildExists(const OcTreeNode *node, unsigned int childIdx) const
return true if the child at childIdx exists
Definition: octree.h:235
void setFreeThres(FCL_REAL d)
Definition: octree.h:209
OcTree(const OcTree &other)
Definition: octree.h:92
#define HPP_FCL_DLLAPI
Definition: config.hh:64
bool nodeHasChildren(const OcTreeNode *node) const
return true if node has at least one child
Definition: octree.h:245