hpp-fcl: include/hpp/fcl/octree.h Source File

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>
48#include <hpp/fcl/collision_object.h>
49
50namespace hpp
51{
52namespace fcl
53{
54
56 class HPP_FCL_DLLAPI OcTree : public CollisionGeometry
57{
58private:
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
66public:
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
106 void computeLocalAABB()
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
183 FCL_REAL getOccupancyThres() const
184 {
185 return occupancy_threshold;
186 }
187
189 FCL_REAL getFreeThres() const
190 {
191 return free_threshold;
192 }
193
194 FCL_REAL getDefaultOccupancy() const
195 {
196 return default_occupancy;
197 }
198
199 void setCellDefaultOccupancy(FCL_REAL d)
200 {
201 default_occupancy = d;
202 }
203
204 void setOccupancyThres(FCL_REAL d)
205 {
206 occupancy_threshold = d;
207 }
208
209 void setFreeThres(FCL_REAL d)
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
262static 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
AABB.h
hpp::fcl::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:56
hpp::fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_object.h:67
hpp::fcl::OcTree
Octree is one type of collision geometry which can encode uncertainty information in the sensor data.
Definition: octree.h:57
hpp::fcl::OcTree::getRootBV
AABB getRootBV() const
get the bounding volume for the root
Definition: octree.h:114
hpp::fcl::OcTree::OcTreeNode
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef octomap::OcTreeNode OcTreeNode
Definition: octree.h:70
hpp::fcl::OcTree::toBoxes
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
hpp::fcl::OcTree::OcTree
OcTree(FCL_REAL resolution)
construct octree with a given resolution
Definition: octree.h:73
hpp::fcl::OcTree::getObjectType
OBJECT_TYPE getObjectType() const
return object type, it is an octree
Definition: octree.h:255
hpp::fcl::OcTree::isNodeOccupied
bool isNodeOccupied(const OcTreeNode *node) const
whether one node is completely occupied
Definition: octree.h:135
hpp::fcl::OcTree::getNodeChild
OcTreeNode * getNodeChild(OcTreeNode *node, unsigned int childIdx)
Definition: octree.h:215
hpp::fcl::OcTree::getFreeThres
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
hpp::fcl::OcTree::getDefaultOccupancy
FCL_REAL getDefaultOccupancy() const
Definition: octree.h:194
hpp::fcl::OcTree::OcTree
OcTree(const OcTree &other)
Definition: octree.h:92
hpp::fcl::OcTree::clone
OcTree * clone() const
Clone *this into a new CollisionGeometry.
Definition: octree.h:101
hpp::fcl::OcTree::setCellDefaultOccupancy
void setCellDefaultOccupancy(FCL_REAL d)
Definition: octree.h:199
hpp::fcl::OcTree::exportAsObjFile
void exportAsObjFile(const std::string &filename) const
hpp::fcl::OcTree::getNodeChild
const OcTreeNode * getNodeChild(const OcTreeNode *node, unsigned int childIdx) const
Definition: octree.h:225
hpp::fcl::OcTree::getTreeDepth
unsigned int getTreeDepth() const
Returns the depth of octree.
Definition: octree.h:123
hpp::fcl::OcTree::setFreeThres
void setFreeThres(FCL_REAL d)
Definition: octree.h:209
hpp::fcl::OcTree::isNodeUncertain
bool isNodeUncertain(const OcTreeNode *node) const
whether one node is uncertain
Definition: octree.h:149
hpp::fcl::OcTree::setOccupancyThres
void setOccupancyThres(FCL_REAL d)
Definition: octree.h:204
hpp::fcl::OcTree::nodeChildExists
bool nodeChildExists(const OcTreeNode *node, unsigned int childIdx) const
return true if the child at childIdx exists
Definition: octree.h:235
hpp::fcl::OcTree::getOccupancyThres
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
hpp::fcl::OcTree::getRoot
OcTreeNode * getRoot() const
get the root node of the octree
Definition: octree.h:129
hpp::fcl::OcTree::computeLocalAABB
void computeLocalAABB()
compute the AABB for the octree in its local coordinate system
Definition: octree.h:106
hpp::fcl::OcTree::nodeHasChildren
bool nodeHasChildren(const OcTreeNode *node) const
return true if node has at least one child
Definition: octree.h:245
hpp::fcl::OcTree::OcTree
OcTree(const shared_ptr< const octomap::OcTree > &tree_)
construct octree from octomap
Definition: octree.h:83
hpp::fcl::OcTree::getNodeType
NODE_TYPE getNodeType() const
return node type, it is an octree
Definition: octree.h:258
hpp::fcl::OcTree::isNodeFree
bool isNodeFree(const OcTreeNode *node) const
whether one node is completely free
Definition: octree.h:142
collision_object.h
fwd.hh
hpp::fcl::OT_OCTREE
@ OT_OCTREE
Definition: collision_object.h:55
hpp::fcl::GEOM_OCTREE
@ GEOM_OCTREE
Definition: collision_object.h:59
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::OBJECT_TYPE
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:55
hpp::fcl::NODE_TYPE
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_object.h:58
hpp::fcl::OcTreePtr_t
shared_ptr< OcTree > OcTreePtr_t
Definition: fwd.hh:85
hpp::fcl::makeOctree
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.
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
hpp
Main namespace.
Definition: AABB.h:44