39 #ifndef HPP_FCL_OCTREE_H 40 #define HPP_FCL_OCTREE_H 43 #include <boost/array.hpp> 45 #include <octomap/octomap.h> 59 shared_ptr<const octomap::OcTree> tree;
68 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
75 default_occupancy = tree->getOccupancyThres();
78 occupancy_threshold = tree->getOccupancyThres();
83 OcTree(
const shared_ptr<const octomap::OcTree>& tree_) : tree(tree_)
85 default_occupancy = tree->getOccupancyThres();
88 occupancy_threshold = tree->getOccupancyThres();
95 , default_occupancy(other.default_occupancy)
96 , occupancy_threshold(other.occupancy_threshold)
97 , free_threshold(other.free_threshold)
103 void exportAsObjFile(
const std::string& filename)
const;
108 aabb_local = getRootBV();
109 aabb_center = aabb_local.center();
110 aabb_radius = (aabb_local.min_ - aabb_center).norm();
116 FCL_REAL delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2;
119 return AABB(
Vec3f(-delta, -delta, -delta),
Vec3f(delta, delta, delta));
125 return tree->getTreeDepth();
131 return tree->getRoot();
138 return node->getOccupancy() >= occupancy_threshold;
145 return node->getOccupancy() <= free_threshold;
151 return (!isNodeOccupied(node)) && (!isNodeFree(node));
156 std::vector<boost::array<FCL_REAL, 6> >
toBoxes()
const 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();
166 if(isNodeOccupied(&*it))
173 FCL_REAL t = tree->getOccupancyThres();
175 boost::array<FCL_REAL, 6> box = {{x, y, z, size, c, t}};
176 boxes.push_back(box);
185 return occupancy_threshold;
191 return free_threshold;
196 return default_occupancy;
201 default_occupancy = d;
206 occupancy_threshold = d;
217 #if OCTOMAP_VERSION_AT_LEAST(1,8,0) 218 return tree->getNodeChild(node, childIdx);
220 return node->getChild(childIdx);
225 const OcTreeNode*
getNodeChild(
const OcTreeNode* node,
unsigned int childIdx)
const 227 #if OCTOMAP_VERSION_AT_LEAST(1,8,0) 228 return tree->getNodeChild(node, childIdx);
230 return node->getChild(childIdx);
237 #if OCTOMAP_VERSION_AT_LEAST(1,8,0) 238 return tree->nodeChildExists(node, childIdx);
240 return node->childExists(childIdx);
247 #if OCTOMAP_VERSION_AT_LEAST(1,8,0) 248 return tree->nodeHasChildren(node);
250 return node->hasChildren();
262 static inline void computeChildBV(
const AABB& root_bv,
unsigned int i,
AABB& child_bv)
266 child_bv.
min_[0] = (root_bv.
min_[0] + root_bv.
max_[0]) * 0.5;
272 child_bv.
max_[0] = (root_bv.
min_[0] + root_bv.
max_[0]) * 0.5;
277 child_bv.
min_[1] = (root_bv.
min_[1] + root_bv.
max_[1]) * 0.5;
283 child_bv.
max_[1] = (root_bv.
min_[1] + root_bv.
max_[1]) * 0.5;
288 child_bv.
min_[2] = (root_bv.
min_[2] + root_bv.
max_[2]) * 0.5;
294 child_bv.
max_[2] = (root_bv.
min_[2] + root_bv.
max_[2]) * 0.5;
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