GCC Code Coverage Report

Directory: ./
File: include/coal/octree.h
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 97 124 78.2%
Branches: 70 218 32.1%
Line Branch Exec Source
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 * Copyright (c) 2022-2024, Inria
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Open Source Robotics Foundation nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 */
36
37 /** \author Jia Pan */
38
39 #ifndef COAL_OCTREE_H
40 #define COAL_OCTREE_H
41
42 #include <algorithm>
43
44 #include <octomap/octomap.h>
45 #include "coal/fwd.hh"
46 #include "coal/BV/AABB.h"
47 #include "coal/collision_object.h"
48
49 namespace coal {
50
51 /// @brief Octree is one type of collision geometry which can encode uncertainty
52 /// information in the sensor data.
53 class COAL_DLLAPI OcTree : public CollisionGeometry {
54 protected:
55 shared_ptr<const octomap::OcTree> tree;
56
57 Scalar default_occupancy;
58
59 Scalar occupancy_threshold;
60 Scalar free_threshold;
61
62 public:
63 typedef octomap::OcTreeNode OcTreeNode;
64
65 /// @brief construct octree with a given resolution
66 3 explicit OcTree(Scalar resolution)
67
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 : tree(shared_ptr<const octomap::OcTree>(
68
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 new octomap::OcTree(resolution))) {
69 3 default_occupancy = Scalar(tree->getOccupancyThres());
70
71 // default occupancy/free threshold is consistent with default setting from
72 // octomap
73 3 occupancy_threshold = Scalar(tree->getOccupancyThres());
74 3 free_threshold = 0;
75 3 }
76
77 /// @brief construct octree from octomap
78 4 explicit OcTree(const shared_ptr<const octomap::OcTree>& tree_)
79 4 : tree(tree_) {
80 4 default_occupancy = Scalar(tree->getOccupancyThres());
81
82 // default occupancy/free threshold is consistent with default setting from
83 // octomap
84 4 occupancy_threshold = Scalar(tree->getOccupancyThres());
85 4 free_threshold = 0;
86 4 }
87
88 ///  \brief Copy constructor
89 2 OcTree(const OcTree& other)
90 2 : CollisionGeometry(other),
91 2 tree(other.tree),
92 2 default_occupancy(other.default_occupancy),
93 2 occupancy_threshold(other.occupancy_threshold),
94 2 free_threshold(other.free_threshold) {}
95
96 /// \brief Clone *this into a new Octree
97 ✗ OcTree* clone() const { return new OcTree(*this); }
98
99 /// \brief Returns the tree associated to the underlying octomap OcTree.
100 7 shared_ptr<const octomap::OcTree> getTree() const { return tree; }
101
102 void exportAsObjFile(const std::string& filename) const;
103
104 /// @brief compute the AABB for the octree in its local coordinate system
105 ✗ void computeLocalAABB() {
106 typedef Eigen::Matrix<float, 3, 1> Vec3float;
107 ✗ Vec3float max_extent, min_extent;
108
109 octomap::OcTree::iterator it =
110 ✗ tree->begin((unsigned char)tree->getTreeDepth());
111 ✗ octomap::OcTree::iterator end = tree->end();
112
113 ✗ if (it == end) return;
114
115 {
116 const octomap::point3d& coord =
117 ✗ it.getCoordinate(); // getCoordinate returns a copy
118 ✗ max_extent = min_extent = Eigen::Map<const Vec3float>(&coord.x());
119 ✗ for (++it; it != end; ++it) {
120 ✗ const octomap::point3d& coord = it.getCoordinate();
121 ✗ const Vec3float pos = Eigen::Map<const Vec3float>(&coord.x());
122 ✗ max_extent = max_extent.array().max(pos.array());
123 ✗ min_extent = min_extent.array().min(pos.array());
124 }
125 }
126
127 // Account for the size of the boxes.
128 ✗ const Scalar resolution = Scalar(tree->getResolution());
129 ✗ max_extent.array() += float(resolution / 2.);
130 ✗ min_extent.array() -= float(resolution / 2.);
131
132 ✗ aabb_local = AABB(min_extent.cast<Scalar>(), max_extent.cast<Scalar>());
133 ✗ aabb_center = aabb_local.center();
134 ✗ aabb_radius = (aabb_local.min_ - aabb_center).norm();
135 }
136
137 /// @brief get the bounding volume for the root
138 3100 AABB getRootBV() const {
139 Scalar delta =
140
1/2
✓ Branch 4 taken 3100 times.
✗ Branch 5 not taken.
 3100 Scalar((1 << tree->getTreeDepth()) * tree->getResolution() / 2);
141
142 // std::cout << "octree size " << delta << std::endl;
143
3/6
✓ Branch 1 taken 3100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3100 times.
✗ Branch 8 not taken.
 6200 return AABB(Vec3s(-delta, -delta, -delta), Vec3s(delta, delta, delta));
144 }
145
146 /// @brief Returns the depth of the octree
147 ✗ unsigned int getTreeDepth() const { return tree->getTreeDepth(); }
148
149 /// @brief Returns the size of the octree
150 ✗ unsigned long size() const { return tree->size(); }
151
152 /// @brief Returns the resolution of the octree
153 3 Scalar getResolution() const { return Scalar(tree->getResolution()); }
154
155 /// @brief get the root node of the octree
156 3201 OcTreeNode* getRoot() const { return tree->getRoot(); }
157
158 /// @brief whether one node is completely occupied
159 2903021 bool isNodeOccupied(const OcTreeNode* node) const {
160 // return tree->isNodeOccupied(node);
161 2903021 return node->getOccupancy() >= occupancy_threshold;
162 }
163
164 /// @brief whether one node is completely free
165 113592 bool isNodeFree(const OcTreeNode* node) const {
166 // return false; // default no definitely free node
167 113592 return node->getOccupancy() <= free_threshold;
168 }
169
170 /// @brief whether one node is uncertain
171 113592 bool isNodeUncertain(const OcTreeNode* node) const {
172
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 113592 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 113592 return (!isNodeOccupied(node)) && (!isNodeFree(node));
173 }
174
175 /// @brief transform the octree into a bunch of boxes; uncertainty information
176 /// is kept in the boxes. However, we only keep the occupied boxes (i.e., the
177 /// boxes whose occupied probability is higher enough).
178 19 std::vector<Vec6s> toBoxes() const {
179 19 std::vector<Vec6s> boxes;
180
2/4
✓ Branch 2 taken 19 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 19 times.
✗ Branch 6 not taken.
 19 boxes.reserve(tree->size() / 2);
181 19 for (octomap::OcTree::iterator
182
1/2
✓ Branch 4 taken 19 times.
✗ Branch 5 not taken.
 19 it = tree->begin((unsigned char)tree->getTreeDepth()),
183
1/2
✓ Branch 2 taken 19 times.
✗ Branch 3 not taken.
 19 end = tree->end();
184
4/6
✓ Branch 1 taken 1403421 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1403440 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1403421 times.
✓ Branch 7 taken 19 times.
 1403440 it != end; ++it) {
185 // if(tree->isNodeOccupied(*it))
186
2/4
✓ Branch 1 taken 1403421 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1403421 times.
✗ Branch 5 not taken.
 1403421 if (isNodeOccupied(&*it)) {
187
1/2
✓ Branch 1 taken 1403421 times.
✗ Branch 2 not taken.
 1403421 Scalar x = Scalar(it.getX());
188
1/2
✓ Branch 1 taken 1403421 times.
✗ Branch 2 not taken.
 1403421 Scalar y = Scalar(it.getY());
189
1/2
✓ Branch 1 taken 1403421 times.
✗ Branch 2 not taken.
 1403421 Scalar z = Scalar(it.getZ());
190
1/2
✓ Branch 1 taken 1403421 times.
✗ Branch 2 not taken.
 1403421 Scalar size = Scalar(it.getSize());
191
1/2
✓ Branch 1 taken 1403421 times.
✗ Branch 2 not taken.
 1403421 Scalar c = Scalar((*it).getOccupancy());
192 1403421 Scalar t = Scalar(tree->getOccupancyThres());
193
194
1/2
✓ Branch 1 taken 1403421 times.
✗ Branch 2 not taken.
 1403421 Vec6s box;
195
6/12
✓ Branch 1 taken 1403421 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1403421 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1403421 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1403421 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1403421 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1403421 times.
✗ Branch 17 not taken.
 1403421 box << x, y, z, size, c, t;
196
1/2
✓ Branch 1 taken 1403421 times.
✗ Branch 2 not taken.
 1403421 boxes.push_back(box);
197 }
198 19 }
199 19 return boxes;
200 }
201
202 /// \brief Returns a byte description of *this
203 1 std::vector<uint8_t> tobytes() const {
204 typedef Eigen::Matrix<float, 3, 1> Vec3sloat;
205 1 const size_t total_size = (tree->size() * sizeof(Scalar) * 3) / 2;
206 1 std::vector<uint8_t> bytes;
207
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bytes.reserve(total_size);
208
209 1 for (octomap::OcTree::iterator
210
1/2
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 it = tree->begin((unsigned char)tree->getTreeDepth()),
211
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 end = tree->end();
212
4/6
✓ Branch 1 taken 1385421 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1385422 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1385421 times.
✓ Branch 7 taken 1 times.
 1385422 it != end; ++it) {
213 const Vec3s box_pos =
214
5/10
✓ Branch 1 taken 1385421 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1385421 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1385421 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1385421 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1385421 times.
✗ Branch 15 not taken.
 1385421 Eigen::Map<Vec3sloat>(&it.getCoordinate().x()).cast<Scalar>();
215
2/4
✓ Branch 1 taken 1385421 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1385421 times.
✗ Branch 5 not taken.
 1385421 if (isNodeOccupied(&*it))
216
4/8
✓ Branch 1 taken 1385421 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1385421 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1385421 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1385421 times.
✗ Branch 11 not taken.
 1385421 std::copy(box_pos.data(), box_pos.data() + sizeof(Scalar) * 3,
217 std::back_inserter(bytes));
218 1 }
219
220 1 return bytes;
221 }
222
223 /// @brief the threshold used to decide whether one node is occupied, this is
224 /// NOT the octree occupied_thresold
225 ✗ Scalar getOccupancyThres() const { return occupancy_threshold; }
226
227 /// @brief the threshold used to decide whether one node is free, this is NOT
228 /// the octree free_threshold
229 ✗ Scalar getFreeThres() const { return free_threshold; }
230
231 ✗ Scalar getDefaultOccupancy() const { return default_occupancy; }
232
233 ✗ void setCellDefaultOccupancy(Scalar d) { default_occupancy = d; }
234
235 ✗ void setOccupancyThres(Scalar d) { occupancy_threshold = d; }
236
237 ✗ void setFreeThres(Scalar d) { free_threshold = d; }
238
239 /// @return ptr to child number childIdx of node
240 OcTreeNode* getNodeChild(OcTreeNode* node, unsigned int childIdx) {
241 #if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
242 return tree->getNodeChild(node, childIdx);
243 #else
244 return node->getChild(childIdx);
245 #endif
246 }
247
248 /// @return const ptr to child number childIdx of node
249 99383 const OcTreeNode* getNodeChild(const OcTreeNode* node,
250 unsigned int childIdx) const {
251 #if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
252 99383 return tree->getNodeChild(node, childIdx);
253 #else
254 return node->getChild(childIdx);
255 #endif
256 }
257
258 /// @brief return true if the child at childIdx exists
259 287163 bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const {
260 #if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
261 287163 return tree->nodeChildExists(node, childIdx);
262 #else
263 return node->childExists(childIdx);
264 #endif
265 }
266
267 /// @brief return true if node has at least one child
268 72745 bool nodeHasChildren(const OcTreeNode* node) const {
269 #if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
270 72745 return tree->nodeHasChildren(node);
271 #else
272 return node->hasChildren();
273 #endif
274 }
275
276 /// @brief return object type, it is an octree
277 3100 OBJECT_TYPE getObjectType() const { return OT_OCTREE; }
278
279 /// @brief return node type, it is an octree
280 3100 NODE_TYPE getNodeType() const { return GEOM_OCTREE; }
281
282 private:
283 18 virtual bool isEqual(const CollisionGeometry& _other) const {
284
1/2
✓ Branch 0 taken 18 times.
✗ Branch 1 not taken.
 18 const OcTree* other_ptr = dynamic_cast<const OcTree*>(&_other);
285
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 18 times.
 18 if (other_ptr == nullptr) return false;
286 18 const OcTree& other = *other_ptr;
287
288
6/14
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✓ Branch 14 taken 10 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
 44 return (tree.get() == other.tree.get() || toBoxes() == other.toBoxes()) &&
289
1/2
✓ Branch 0 taken 18 times.
✗ Branch 1 not taken.
 18 default_occupancy == other.default_occupancy &&
290
3/4
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 10 times.
✓ Branch 3 taken 18 times.
✗ Branch 4 not taken.
 72 occupancy_threshold == other.occupancy_threshold &&
291
3/4
✓ Branch 0 taken 18 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
✓ Branch 3 taken 10 times.
 36 free_threshold == other.free_threshold;
292 }
293
294 public:
295 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
296 };
297
298 /// @brief compute the bounding volume of an octree node's i-th child
299 99383 static inline void computeChildBV(const AABB& root_bv, unsigned int i,
300 AABB& child_bv) {
301 99383 const Scalar half = Scalar(0.5);
302
2/2
✓ Branch 0 taken 46564 times.
✓ Branch 1 taken 52819 times.
 99383 if (i & 1) {
303 46564 child_bv.min_[0] = (root_bv.min_[0] + root_bv.max_[0]) * half;
304 46564 child_bv.max_[0] = root_bv.max_[0];
305 } else {
306 52819 child_bv.min_[0] = root_bv.min_[0];
307 52819 child_bv.max_[0] = (root_bv.min_[0] + root_bv.max_[0]) * half;
308 }
309
310
2/2
✓ Branch 0 taken 49765 times.
✓ Branch 1 taken 49618 times.
 99383 if (i & 2) {
311 49765 child_bv.min_[1] = (root_bv.min_[1] + root_bv.max_[1]) * half;
312 49765 child_bv.max_[1] = root_bv.max_[1];
313 } else {
314 49618 child_bv.min_[1] = root_bv.min_[1];
315 49618 child_bv.max_[1] = (root_bv.min_[1] + root_bv.max_[1]) * half;
316 }
317
318
2/2
✓ Branch 0 taken 60776 times.
✓ Branch 1 taken 38607 times.
 99383 if (i & 4) {
319 60776 child_bv.min_[2] = (root_bv.min_[2] + root_bv.max_[2]) * half;
320 60776 child_bv.max_[2] = root_bv.max_[2];
321 } else {
322 38607 child_bv.min_[2] = root_bv.min_[2];
323 38607 child_bv.max_[2] = (root_bv.min_[2] + root_bv.max_[2]) * half;
324 }
325 99383 }
326
327 ///
328 /// \brief Build an OcTree from a point cloud and a given resolution
329 ///
330 /// \param[in] point_cloud The input points to insert in the OcTree
331 /// \param[in] resolution of the octree.
332 ///
333 /// \returns An OcTree that can be used for collision checking and more.
334 ///
335 COAL_DLLAPI OcTreePtr_t
336 makeOctree(const Eigen::Matrix<Scalar, Eigen::Dynamic, 3>& point_cloud,
337 const Scalar resolution);
338
339 } // namespace coal
340
341 #endif
342