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 
38 #ifndef HPP_FCL_OCTREE_H
39 #define HPP_FCL_OCTREE_H
40 
41 #include <boost/array.hpp>
42 
43 #include <octomap/octomap.h>
44 #include <hpp/fcl/fwd.hh>
45 #include <hpp/fcl/BV/AABB.h>
47 
48 namespace hpp {
49 namespace fcl {
50 
53 class HPP_FCL_DLLAPI OcTree : public CollisionGeometry {
54  private:
55  shared_ptr<const octomap::OcTree> tree;
56 
58 
61 
62  public:
64 
66  explicit OcTree(FCL_REAL resolution)
67  : tree(shared_ptr<const octomap::OcTree>(
68  new octomap::OcTree(resolution))) {
69  default_occupancy = tree->getOccupancyThres();
70 
71  // default occupancy/free threshold is consistent with default setting from
72  // octomap
73  occupancy_threshold = tree->getOccupancyThres();
74  free_threshold = 0;
75  }
76 
78  explicit OcTree(const shared_ptr<const octomap::OcTree>& tree_)
79  : tree(tree_) {
80  default_occupancy = tree->getOccupancyThres();
81 
82  // default occupancy/free threshold is consistent with default setting from
83  // octomap
84  occupancy_threshold = tree->getOccupancyThres();
85  free_threshold = 0;
86  }
87 
88  OcTree(const OcTree& other)
89  : CollisionGeometry(other),
90  tree(other.tree),
91  default_occupancy(other.default_occupancy),
92  occupancy_threshold(other.occupancy_threshold),
93  free_threshold(other.free_threshold) {}
94 
95  OcTree* clone() const { return new OcTree(*this); }
96 
97  void exportAsObjFile(const std::string& filename) const;
98 
101  aabb_local = getRootBV();
102  aabb_center = aabb_local.center();
103  aabb_radius = (aabb_local.min_ - aabb_center).norm();
104  }
105 
107  AABB getRootBV() const {
108  FCL_REAL delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2;
109 
110  // std::cout << "octree size " << delta << std::endl;
111  return AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta));
112  }
113 
115  unsigned int getTreeDepth() const { return tree->getTreeDepth(); }
116 
118  OcTreeNode* getRoot() const { return tree->getRoot(); }
119 
121  bool isNodeOccupied(const OcTreeNode* node) const {
122  // return tree->isNodeOccupied(node);
123  return node->getOccupancy() >= occupancy_threshold;
124  }
125 
127  bool isNodeFree(const OcTreeNode* node) const {
128  // return false; // default no definitely free node
129  return node->getOccupancy() <= free_threshold;
130  }
131 
133  bool isNodeUncertain(const OcTreeNode* node) const {
134  return (!isNodeOccupied(node)) && (!isNodeFree(node));
135  }
136 
140  std::vector<boost::array<FCL_REAL, 6> > toBoxes() const {
141  std::vector<boost::array<FCL_REAL, 6> > boxes;
142  boxes.reserve(tree->size() / 2);
144  it = tree->begin((unsigned char)tree->getTreeDepth()),
145  end = tree->end();
146  it != end; ++it) {
147  // if(tree->isNodeOccupied(*it))
148  if (isNodeOccupied(&*it)) {
149  FCL_REAL size = it.getSize();
150  FCL_REAL x = it.getX();
151  FCL_REAL y = it.getY();
152  FCL_REAL z = it.getZ();
153  FCL_REAL c = (*it).getOccupancy();
154  FCL_REAL t = tree->getOccupancyThres();
155 
156  boost::array<FCL_REAL, 6> box = {{x, y, z, size, c, t}};
157  boxes.push_back(box);
158  }
159  }
160  return boxes;
161  }
162 
165  FCL_REAL getOccupancyThres() const { return occupancy_threshold; }
166 
169  FCL_REAL getFreeThres() const { return free_threshold; }
170 
171  FCL_REAL getDefaultOccupancy() const { return default_occupancy; }
172 
173  void setCellDefaultOccupancy(FCL_REAL d) { default_occupancy = d; }
174 
175  void setOccupancyThres(FCL_REAL d) { occupancy_threshold = d; }
176 
177  void setFreeThres(FCL_REAL d) { free_threshold = d; }
178 
180  OcTreeNode* getNodeChild(OcTreeNode* node, unsigned int childIdx) {
181 #if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
182  return tree->getNodeChild(node, childIdx);
183 #else
184  return node->getChild(childIdx);
185 #endif
186  }
187 
189  const OcTreeNode* getNodeChild(const OcTreeNode* node,
190  unsigned int childIdx) const {
191 #if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
192  return tree->getNodeChild(node, childIdx);
193 #else
194  return node->getChild(childIdx);
195 #endif
196  }
197 
199  bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const {
200 #if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
201  return tree->nodeChildExists(node, childIdx);
202 #else
203  return node->childExists(childIdx);
204 #endif
205  }
206 
208  bool nodeHasChildren(const OcTreeNode* node) const {
209 #if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
210  return tree->nodeHasChildren(node);
211 #else
212  return node->hasChildren();
213 #endif
214  }
215 
217  OBJECT_TYPE getObjectType() const { return OT_OCTREE; }
218 
220  NODE_TYPE getNodeType() const { return GEOM_OCTREE; }
221 
222  private:
223  virtual bool isEqual(const CollisionGeometry& _other) const {
224  const OcTree* other_ptr = dynamic_cast<const OcTree*>(&_other);
225  if (other_ptr == nullptr) return false;
226  const OcTree& other = *other_ptr;
227 
228  return tree.get() == other.tree.get() &&
229  default_occupancy == other.default_occupancy &&
230  occupancy_threshold == other.occupancy_threshold &&
231  free_threshold == other.free_threshold;
232  }
233 
234  public:
235  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
236 };
237 
239 static inline void computeChildBV(const AABB& root_bv, unsigned int i,
240  AABB& child_bv) {
241  if (i & 1) {
242  child_bv.min_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
243  child_bv.max_[0] = root_bv.max_[0];
244  } else {
245  child_bv.min_[0] = root_bv.min_[0];
246  child_bv.max_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
247  }
248 
249  if (i & 2) {
250  child_bv.min_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
251  child_bv.max_[1] = root_bv.max_[1];
252  } else {
253  child_bv.min_[1] = root_bv.min_[1];
254  child_bv.max_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
255  }
256 
257  if (i & 4) {
258  child_bv.min_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
259  child_bv.max_[2] = root_bv.max_[2];
260  } else {
261  child_bv.min_[2] = root_bv.min_[2];
262  child_bv.max_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
263  }
264 }
265 
274 HPP_FCL_DLLAPI OcTreePtr_t
275 makeOctree(const Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 3>& point_cloud,
276  const FCL_REAL resolution);
277 
278 } // namespace fcl
279 
280 } // namespace hpp
281 
282 #endif
bool isNodeFree(const OcTreeNode *node) const
whether one node is completely free
Definition: octree.h:127
OcTreeNode * getRoot() const
get the root node of the octree
Definition: octree.h:118
FCL_REAL getOccupancyThres() const
the threshold used to decide whether one node is occupied, this is NOT the octree occupied_thresold ...
Definition: octree.h:165
Vec3f min_
The min point in the AABB.
Definition: BV/AABB.h:57
y
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:140
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Main namespace.
t
double getOccupancy() const
OcTree(FCL_REAL resolution)
construct octree with a given resolution
Definition: octree.h:66
FCL_REAL getDefaultOccupancy() const
Definition: octree.h:171
AABB getRootBV() const
get the bounding volume for the root
Definition: octree.h:107
static void computeChildBV(const AABB &root_bv, unsigned int i, AABB &child_bv)
compute the bounding volume of an octree node&#39;s i-th child
Definition: octree.h:239
unsigned int getTreeDepth() const
Returns the depth of octree.
Definition: octree.h:115
HPP_FCL_DLLAPI 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.
Definition: src/octree.cpp:185
FCL_REAL getFreeThres() const
the threshold used to decide whether one node is free, this is NOT the octree free_threshold ...
Definition: octree.h:169
FCL_REAL default_occupancy
Definition: octree.h:57
void setOccupancyThres(FCL_REAL d)
Definition: octree.h:175
c
Octree is one type of collision geometry which can encode uncertainty information in the sensor data...
Definition: octree.h:53
octomap::OcTreeNode OcTreeNode
Definition: octree.h:63
bool isNodeUncertain(const OcTreeNode *node) const
whether one node is uncertain
Definition: octree.h:133
double FCL_REAL
Definition: data_types.h:65
shared_ptr< OcTree > OcTreePtr_t
OcTree * clone() const
Clone *this into a new CollisionGeometry.
Definition: octree.h:95
x
OcTreeNode * getNodeChild(OcTreeNode *node, unsigned int childIdx)
Definition: octree.h:180
const OcTreeNode * getNodeChild(const OcTreeNode *node, unsigned int childIdx) const
Definition: octree.h:189
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: BV/AABB.h:54
Vec3f max_
The max point in the AABB.
Definition: BV/AABB.h:59
shared_ptr< const octomap::OcTree > tree
Definition: octree.h:55
OBJECT_TYPE getObjectType() const
return object type, it is an octree
Definition: octree.h:217
void setCellDefaultOccupancy(FCL_REAL d)
Definition: octree.h:173
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, cylinder, convex, plane, triangle), and octree
OcTree(const shared_ptr< const octomap::OcTree > &tree_)
construct octree from octomap
Definition: octree.h:78
FCL_REAL free_threshold
Definition: octree.h:60
NODE_TYPE getNodeType() const
return node type, it is an octree
Definition: octree.h:220
void computeLocalAABB()
compute the AABB for the octree in its local coordinate system
Definition: octree.h:100
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
The geometry for the object for collision or distance computation.
bool isNodeOccupied(const OcTreeNode *node) const
whether one node is completely occupied
Definition: octree.h:121
virtual bool isEqual(const CollisionGeometry &_other) const
equal operator with another object of derived type.
Definition: octree.h:223
FCL_REAL occupancy_threshold
Definition: octree.h:59
bool nodeChildExists(const OcTreeNode *node, unsigned int childIdx) const
return true if the child at childIdx exists
Definition: octree.h:199
void setFreeThres(FCL_REAL d)
Definition: octree.h:177
OcTree(const OcTree &other)
Definition: octree.h:88
bool nodeHasChildren(const OcTreeNode *node) const
return true if node has at least one child
Definition: octree.h:208


hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:01