hpp-fcl 2.3.6
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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
48namespace hpp {
49namespace fcl {
50
54 private:
55 shared_ptr<const octomap::OcTree> tree;
56
57 FCL_REAL default_occupancy;
58
59 FCL_REAL occupancy_threshold;
60 FCL_REAL free_threshold;
61
62 public:
63 typedef octomap::OcTreeNode OcTreeNode;
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);
143 for (octomap::OcTree::iterator
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
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
218
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
239static 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
275makeOctree(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
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:54
The geometry for the object for collision or distance computation.
Definition: collision_object.h:95
Octree is one type of collision geometry which can encode uncertainty information in the sensor data.
Definition: octree.h:53
AABB getRootBV() const
get the bounding volume for the root
Definition: octree.h:107
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
OcTree(FCL_REAL resolution)
construct octree with a given resolution
Definition: octree.h:66
OBJECT_TYPE getObjectType() const
return object type, it is an octree
Definition: octree.h:217
bool isNodeOccupied(const OcTreeNode *node) const
whether one node is completely occupied
Definition: octree.h:121
OcTreeNode * getNodeChild(OcTreeNode *node, unsigned int childIdx)
Definition: octree.h:180
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 getDefaultOccupancy() const
Definition: octree.h:171
OcTree(const OcTree &other)
Definition: octree.h:88
octomap::OcTreeNode OcTreeNode
Definition: octree.h:63
OcTree * clone() const
Clone *this into a new CollisionGeometry.
Definition: octree.h:95
void setCellDefaultOccupancy(FCL_REAL d)
Definition: octree.h:173
void exportAsObjFile(const std::string &filename) const
const OcTreeNode * getNodeChild(const OcTreeNode *node, unsigned int childIdx) const
Definition: octree.h:189
unsigned int getTreeDepth() const
Returns the depth of octree.
Definition: octree.h:115
void setFreeThres(FCL_REAL d)
Definition: octree.h:177
bool isNodeUncertain(const OcTreeNode *node) const
whether one node is uncertain
Definition: octree.h:133
void setOccupancyThres(FCL_REAL d)
Definition: octree.h:175
bool nodeChildExists(const OcTreeNode *node, unsigned int childIdx) const
return true if the child at childIdx exists
Definition: octree.h:199
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
OcTreeNode * getRoot() const
get the root node of the octree
Definition: octree.h:118
void computeLocalAABB()
compute the AABB for the octree in its local coordinate system
Definition: octree.h:100
bool nodeHasChildren(const OcTreeNode *node) const
return true if node has at least one child
Definition: octree.h:208
OcTree(const shared_ptr< const octomap::OcTree > &tree_)
construct octree from octomap
Definition: octree.h:78
NODE_TYPE getNodeType() const
return node type, it is an octree
Definition: octree.h:220
bool isNodeFree(const OcTreeNode *node) const
whether one node is completely free
Definition: octree.h:127
#define HPP_FCL_DLLAPI
Definition: config.hh:64
@ OT_OCTREE
Definition: collision_object.h:57
@ GEOM_OCTREE
Definition: collision_object.h:84
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:53
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_object.h:65
shared_ptr< OcTree > OcTreePtr_t
Definition: fwd.hh:107
bool isEqual(const Eigen::MatrixBase< Derived > &lhs, const Eigen::MatrixBase< OtherDerived > &rhs, const FCL_REAL tol=std::numeric_limits< FCL_REAL >::epsilon() *100)
Definition: tools.h:209
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.
double FCL_REAL
Definition: data_types.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44