hpp-fcl
1.4.4
HPP fork of FCL -- The Flexible Collision Library
|
Go to the documentation of this file.
39 #ifndef HPP_FCL_OCTREE_H
40 #define HPP_FCL_OCTREE_H
43 #include <boost/shared_ptr.hpp>
44 #include <boost/array.hpp>
46 #include <octomap/octomap.h>
59 boost::shared_ptr<const octomap::OcTree> tree;
73 default_occupancy = tree->getOccupancyThres();
76 occupancy_threshold = tree->getOccupancyThres();
81 OcTree(
const boost::shared_ptr<const octomap::OcTree>& tree_) : tree(tree_)
83 default_occupancy = tree->getOccupancyThres();
86 occupancy_threshold = tree->getOccupancyThres();
101 FCL_REAL delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2;
104 return AABB(
Vec3f(-delta, -delta, -delta),
Vec3f(delta, delta, delta));
110 return tree->getRoot();
117 return node->getOccupancy() >= occupancy_threshold;
124 return node->getOccupancy() <= free_threshold;
135 std::vector<boost::array<FCL_REAL, 6> >
toBoxes()
const
137 std::vector<boost::array<FCL_REAL, 6> > boxes;
138 boxes.reserve(tree->size() / 2);
139 for(octomap::OcTree::iterator it =
140 tree->begin((
unsigned char) tree->getTreeDepth()), end = tree->end();
152 FCL_REAL t = tree->getOccupancyThres();
154 boost::array<FCL_REAL, 6> box = {{x, y, z, size, c, t}};
155 boxes.push_back(box);
164 return occupancy_threshold;
170 return free_threshold;
175 return default_occupancy;
180 default_occupancy = d;
185 occupancy_threshold = d;
196 #if OCTOMAP_VERSION_AT_LEAST(1,8,0)
197 return tree->getNodeChild(node, childIdx);
199 return node->getChild(childIdx);
206 #if OCTOMAP_VERSION_AT_LEAST(1,8,0)
207 return tree->getNodeChild(node, childIdx);
209 return node->getChild(childIdx);
216 #if OCTOMAP_VERSION_AT_LEAST(1,8,0)
217 return tree->nodeChildExists(node, childIdx);
219 return node->childExists(childIdx);
226 #if OCTOMAP_VERSION_AT_LEAST(1,8,0)
227 return tree->nodeHasChildren(node);
229 return node->hasChildren();
241 static inline void computeChildBV(
const AABB& root_bv,
unsigned int i, AABB& child_bv)
245 child_bv.min_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
246 child_bv.max_[0] = root_bv.max_[0];
250 child_bv.min_[0] = root_bv.min_[0];
251 child_bv.max_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
256 child_bv.min_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
257 child_bv.max_[1] = root_bv.max_[1];
261 child_bv.min_[1] = root_bv.min_[1];
262 child_bv.max_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
267 child_bv.min_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
268 child_bv.max_[2] = root_bv.max_[2];
272 child_bv.min_[2] = root_bv.min_[2];
273 child_bv.max_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:73
bool nodeHasChildren(const OcTreeNode *node) const
return true if node has at least one child
Definition: octree.h:224
FCL_REAL getFreeThres() const
the threshold used to decide whether one node is occupied, this is NOT the octree free_threshold
Definition: octree.h:168
OcTree(const boost::shared_ptr< const octomap::OcTree > &tree_)
construct octree from octomap
Definition: octree.h:81
void setFreeThres(FCL_REAL d)
Definition: octree.h:188
Vec3f min_
The min point in the AABB.
Definition: AABB.h:59
void setCellDefaultOccupancy(FCL_REAL d)
Definition: octree.h:178
FCL_REAL getOccupancyThres() const
the threshold used to decide whether one node is occupied, this is NOT the octree occupied_thresold
Definition: octree.h:162
void computeLocalAABB()
compute the AABB for the octree in its local coordinate system
Definition: octree.h:91
AABB aabb_local
AABB in local coordinate, used for tight AABB when only translation transform.
Definition: collision_object.h:113
The geometry for the object for collision or distance computation.
Definition: collision_object.h:63
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_object.h:56
OcTreeNode * getRoot() const
get the root node of the octree
Definition: octree.h:108
octomap::OcTreeNode OcTreeNode
Definition: octree.h:68
double FCL_REAL
Definition: data_types.h:68
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:53
AABB getRootBV() const
get the bounding volume for the root
Definition: octree.h:99
bool isNodeUncertain(const OcTreeNode *node) const
whether one node is uncertain
Definition: octree.h:128
OcTreeNode * getNodeChild(OcTreeNode *node, unsigned int childIdx)
Definition: octree.h:194
Main namespace.
Definition: AABB.h:43
@ OT_OCTREE
Definition: collision_object.h:53
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:55
bool nodeChildExists(const OcTreeNode *node, unsigned int childIdx) const
return true if the child at childIdx exists
Definition: octree.h:214
@ GEOM_OCTREE
Definition: collision_object.h:57
NODE_TYPE getNodeType() const
return node type, it is an octree
Definition: octree.h:237
bool isNodeOccupied(const OcTreeNode *node) const
whether one node is completely occupied
Definition: octree.h:114
void setOccupancyThres(FCL_REAL d)
Definition: octree.h:183
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:135
bool isNodeFree(const OcTreeNode *node) const
whether one node is completely free
Definition: octree.h:121
FCL_REAL getDefaultOccupancy() const
Definition: octree.h:173
const OcTreeNode * getNodeChild(const OcTreeNode *node, unsigned int childIdx) const
Definition: octree.h:204
OBJECT_TYPE getObjectType() const
return object type, it is an octree
Definition: octree.h:234
Octree is one type of collision geometry which can encode uncertainty information in the sensor data.
Definition: octree.h:56
OcTree(FCL_REAL resolution)
construct octree with a given resolution
Definition: octree.h:71
FCL_REAL aabb_radius
AABB radius.
Definition: collision_object.h:110
Vec3f center() const
Center of the AABB.
Definition: AABB.h:157
Vec3f aabb_center
AABB center in local coordinate.
Definition: collision_object.h:107