octomap 1.9.8
OccupancyOcTreeBase.hxx
Go to the documentation of this file.
1/*
2 * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3 * https://octomap.github.io/
4 *
5 * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
6 * All rights reserved.
7 * License: New BSD
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions 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 copyright
15 * notice, this list of conditions and the following disclaimer in the
16 * documentation and/or other materials provided with the distribution.
17 * * Neither the name of the University of Freiburg nor the names of its
18 * contributors may be used to endorse or promote products derived from
19 * this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 * POSSIBILITY OF SUCH DAMAGE.
32 */
33
34#include <bitset>
35#include <algorithm>
36
37#include <octomap/MCTables.h>
38
39namespace octomap {
40
41 template <class NODE>
43 : OcTreeBaseImpl<NODE,AbstractOccupancyOcTree>(in_resolution), use_bbx_limit(false), use_change_detection(false)
44 {
45
46 }
47
48 template <class NODE>
49 OccupancyOcTreeBase<NODE>::OccupancyOcTreeBase(double in_resolution, unsigned int in_tree_depth, unsigned int in_tree_max_val)
50 : OcTreeBaseImpl<NODE,AbstractOccupancyOcTree>(in_resolution, in_tree_depth, in_tree_max_val), use_bbx_limit(false), use_change_detection(false)
51 {
52
53 }
54
55 template <class NODE>
57 }
58
59 template <class NODE>
61 OcTreeBaseImpl<NODE,AbstractOccupancyOcTree>(rhs), use_bbx_limit(rhs.use_bbx_limit),
62 bbx_min(rhs.bbx_min), bbx_max(rhs.bbx_max),
63 bbx_min_key(rhs.bbx_min_key), bbx_max_key(rhs.bbx_max_key),
64 use_change_detection(rhs.use_change_detection), changed_keys(rhs.changed_keys)
65 {
68 this->prob_hit_log = rhs.prob_hit_log;
69 this->prob_miss_log = rhs.prob_miss_log;
71
72
73 }
75 template <class NODE>
76 void OccupancyOcTreeBase<NODE>::insertPointCloud(const ScanNode& scan, double maxrange, bool lazy_eval, bool discretize) {
77 // performs transformation to data and sensor origin first
78 Pointcloud& cloud = *(scan.scan);
79 pose6d frame_origin = scan.pose;
80 point3d sensor_origin = frame_origin.inv().transform(scan.pose.trans());
81 insertPointCloud(cloud, sensor_origin, frame_origin, maxrange, lazy_eval, discretize);
82 }
83
84
85 template <class NODE>
87 double maxrange, bool lazy_eval, bool discretize) {
88
89 KeySet free_cells, occupied_cells;
90 if (discretize)
91 computeDiscreteUpdate(scan, sensor_origin, free_cells, occupied_cells, maxrange);
92 else
93 computeUpdate(scan, sensor_origin, free_cells, occupied_cells, maxrange);
94
95 // insert data into tree -----------------------
96 for (KeySet::iterator it = free_cells.begin(); it != free_cells.end(); ++it) {
97 updateNode(*it, false, lazy_eval);
98 }
99 for (KeySet::iterator it = occupied_cells.begin(); it != occupied_cells.end(); ++it) {
100 updateNode(*it, true, lazy_eval);
101 }
102 }
103
104 template <class NODE>
105 void OccupancyOcTreeBase<NODE>::insertPointCloud(const Pointcloud& pc, const point3d& sensor_origin, const pose6d& frame_origin,
106 double maxrange, bool lazy_eval, bool discretize) {
107 // performs transformation to data and sensor origin first
108 Pointcloud transformed_scan (pc);
109 transformed_scan.transform(frame_origin);
110 point3d transformed_sensor_origin = frame_origin.transform(sensor_origin);
111 insertPointCloud(transformed_scan, transformed_sensor_origin, maxrange, lazy_eval, discretize);
112 }
113
114
115 template <class NODE>
116 void OccupancyOcTreeBase<NODE>::insertPointCloudRays(const Pointcloud& pc, const point3d& origin, double /* maxrange */, bool lazy_eval) {
117 if (pc.size() < 1)
118 return;
119
120#ifdef _OPENMP
121 omp_set_num_threads(this->keyrays.size());
122 #pragma omp parallel for
123#endif
124 for (int i = 0; i < (int)pc.size(); ++i) {
125 const point3d& p = pc[i];
126 unsigned threadIdx = 0;
127#ifdef _OPENMP
128 threadIdx = omp_get_thread_num();
129#endif
130 KeyRay* keyray = &(this->keyrays.at(threadIdx));
131
132 if (this->computeRayKeys(origin, p, *keyray)){
133#ifdef _OPENMP
134 #pragma omp critical
135#endif
136 {
137 for(KeyRay::iterator it=keyray->begin(); it != keyray->end(); it++) {
138 updateNode(*it, false, lazy_eval); // insert freespace measurement
139 }
140 updateNode(p, true, lazy_eval); // update endpoint to be occupied
141 }
142 }
143
144 }
145 }
147 template <class NODE>
149 KeySet& free_cells, KeySet& occupied_cells,
150 double maxrange)
151 {
152 Pointcloud discretePC;
153 discretePC.reserve(scan.size());
154 KeySet endpoints;
155
156 for (int i = 0; i < (int)scan.size(); ++i) {
157 OcTreeKey k = this->coordToKey(scan[i]);
158 std::pair<KeySet::iterator,bool> ret = endpoints.insert(k);
159 if (ret.second){ // insertion took place => k was not in set
160 discretePC.push_back(this->keyToCoord(k));
161 }
162 }
163
164 computeUpdate(discretePC, origin, free_cells, occupied_cells, maxrange);
165 }
166
167
168 template <class NODE>
170 KeySet& free_cells, KeySet& occupied_cells,
171 double maxrange)
172 {
173
174
175
176#ifdef _OPENMP
177 omp_set_num_threads(this->keyrays.size());
178 #pragma omp parallel for schedule(guided)
179#endif
180 for (int i = 0; i < (int)scan.size(); ++i) {
181 const point3d& p = scan[i];
182 unsigned threadIdx = 0;
183#ifdef _OPENMP
184 threadIdx = omp_get_thread_num();
185#endif
186 KeyRay* keyray = &(this->keyrays.at(threadIdx));
187
188
189 if (!use_bbx_limit) { // no BBX specified
190 if ((maxrange < 0.0) || ((p - origin).norm() <= maxrange) ) { // is not maxrange meas.
191 // free cells
192 if (this->computeRayKeys(origin, p, *keyray)){
193#ifdef _OPENMP
194 #pragma omp critical (free_insert)
195#endif
197 free_cells.insert(keyray->begin(), keyray->end());
198 }
199 }
200 // occupied endpoint
201 OcTreeKey key;
202 if (this->coordToKeyChecked(p, key)){
203#ifdef _OPENMP
204 #pragma omp critical (occupied_insert)
205#endif
206 {
207 occupied_cells.insert(key);
209 }
210 } else { // user set a maxrange and length is above
211 point3d direction = (p - origin).normalized ();
212 point3d new_end = origin + direction * (float) maxrange;
213 if (this->computeRayKeys(origin, new_end, *keyray)){
214#ifdef _OPENMP
215 #pragma omp critical (free_insert)
216#endif
217 {
218 free_cells.insert(keyray->begin(), keyray->end());
219 }
220 }
221 } // end if maxrange
222 } else { // BBX was set
223 // endpoint in bbx and not maxrange?
224 if ( inBBX(p) && ((maxrange < 0.0) || ((p - origin).norm () <= maxrange) ) ) {
225
226 // occupied endpoint
227 OcTreeKey key;
228 if (this->coordToKeyChecked(p, key)){
229#ifdef _OPENMP
230 #pragma omp critical (occupied_insert)
231#endif
232 {
233 occupied_cells.insert(key);
234 }
235 }
236 } // end if in BBX and not maxrange
237
238 // truncate the end point to the max range if the max range is exceeded
239 point3d new_end = p;
240 if ((maxrange >= 0.0) && ((p - origin).norm() > maxrange)) {
241 const point3d direction = (p - origin).normalized();
242 new_end = origin + direction * (float) maxrange;
243 }
244
245 // update freespace, break as soon as bbx limit is reached
246 if (this->computeRayKeys(origin, new_end, *keyray)){
247 for(KeyRay::iterator it=keyray->begin(); it != keyray->end(); it++) {
248 if (inBBX(*it)) {
249#ifdef _OPENMP
250 #pragma omp critical (free_insert)
251#endif
252 {
253 free_cells.insert(*it);
254 }
255 }
256 else break;
257 }
258 } // end if compute ray
259 } // end bbx case
260 } // end for all points, end of parallel OMP loop
261
262 // prefer occupied cells over free ones (and make sets disjunct)
263 for(KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ){
264 if (occupied_cells.find(*it) != occupied_cells.end()){
265 it = free_cells.erase(it);
266 } else {
267 ++it;
268 }
269 }
270 }
271
272 template <class NODE>
273 NODE* OccupancyOcTreeBase<NODE>::setNodeValue(const OcTreeKey& key, float log_odds_value, bool lazy_eval) {
274 // clamp log odds within range:
275 log_odds_value = std::min(std::max(log_odds_value, this->clamping_thres_min), this->clamping_thres_max);
276
277 bool createdRoot = false;
278 if (this->root == NULL){
279 this->root = new NODE();
280 this->tree_size++;
281 createdRoot = true;
283
284 return setNodeValueRecurs(this->root, createdRoot, key, 0, log_odds_value, lazy_eval);
285 }
286
287 template <class NODE>
288 NODE* OccupancyOcTreeBase<NODE>::setNodeValue(const point3d& value, float log_odds_value, bool lazy_eval) {
289 OcTreeKey key;
290 if (!this->coordToKeyChecked(value, key))
291 return NULL;
292
293 return setNodeValue(key, log_odds_value, lazy_eval);
294 }
295
296 template <class NODE>
297 NODE* OccupancyOcTreeBase<NODE>::setNodeValue(double x, double y, double z, float log_odds_value, bool lazy_eval) {
298 OcTreeKey key;
299 if (!this->coordToKeyChecked(x, y, z, key))
300 return NULL;
301
302 return setNodeValue(key, log_odds_value, lazy_eval);
303 }
304
305
306 template <class NODE>
307 NODE* OccupancyOcTreeBase<NODE>::updateNode(const OcTreeKey& key, float log_odds_update, bool lazy_eval) {
308 // early abort (no change will happen).
309 // may cause an overhead in some configuration, but more often helps
310 NODE* leaf = this->search(key);
311 // no change: node already at threshold
312 if (leaf
313 && ((log_odds_update >= 0 && leaf->getLogOdds() >= this->clamping_thres_max)
314 || ( log_odds_update <= 0 && leaf->getLogOdds() <= this->clamping_thres_min)))
315 {
316 return leaf;
317 }
318
319 bool createdRoot = false;
320 if (this->root == NULL){
321 this->root = new NODE();
322 this->tree_size++;
323 createdRoot = true;
324 }
325
326 return updateNodeRecurs(this->root, createdRoot, key, 0, log_odds_update, lazy_eval);
327 }
328
329 template <class NODE>
330 NODE* OccupancyOcTreeBase<NODE>::updateNode(const point3d& value, float log_odds_update, bool lazy_eval) {
331 OcTreeKey key;
332 if (!this->coordToKeyChecked(value, key))
333 return NULL;
334
335 return updateNode(key, log_odds_update, lazy_eval);
336 }
338 template <class NODE>
339 NODE* OccupancyOcTreeBase<NODE>::updateNode(double x, double y, double z, float log_odds_update, bool lazy_eval) {
340 OcTreeKey key;
341 if (!this->coordToKeyChecked(x, y, z, key))
342 return NULL;
343
344 return updateNode(key, log_odds_update, lazy_eval);
346
347 template <class NODE>
348 NODE* OccupancyOcTreeBase<NODE>::updateNode(const OcTreeKey& key, bool occupied, bool lazy_eval) {
349 float logOdds = this->prob_miss_log;
350 if (occupied)
351 logOdds = this->prob_hit_log;
352
353 return updateNode(key, logOdds, lazy_eval);
354 }
355
356 template <class NODE>
357 NODE* OccupancyOcTreeBase<NODE>::updateNode(const point3d& value, bool occupied, bool lazy_eval) {
358 OcTreeKey key;
359 if (!this->coordToKeyChecked(value, key))
360 return NULL;
361 return updateNode(key, occupied, lazy_eval);
362 }
363
364 template <class NODE>
365 NODE* OccupancyOcTreeBase<NODE>::updateNode(double x, double y, double z, bool occupied, bool lazy_eval) {
366 OcTreeKey key;
367 if (!this->coordToKeyChecked(x, y, z, key))
368 return NULL;
369 return updateNode(key, occupied, lazy_eval);
370 }
371
372 template <class NODE>
373 NODE* OccupancyOcTreeBase<NODE>::updateNodeRecurs(NODE* node, bool node_just_created, const OcTreeKey& key,
374 unsigned int depth, const float& log_odds_update, bool lazy_eval) {
375 bool created_node = false;
376
377 assert(node);
378
379 // follow down to last level
380 if (depth < this->tree_depth) {
381 unsigned int pos = computeChildIdx(key, this->tree_depth -1 - depth);
382 if (!this->nodeChildExists(node, pos)) {
383 // child does not exist, but maybe it's a pruned node?
384 if (!this->nodeHasChildren(node) && !node_just_created ) {
385 // current node does not have children AND it is not a new node
386 // -> expand pruned node
387 this->expandNode(node);
388 }
389 else {
390 // not a pruned node, create requested child
391 this->createNodeChild(node, pos);
392 created_node = true;
393 }
394 }
395
396 if (lazy_eval)
397 return updateNodeRecurs(this->getNodeChild(node, pos), created_node, key, depth+1, log_odds_update, lazy_eval);
398 else {
399 NODE* retval = updateNodeRecurs(this->getNodeChild(node, pos), created_node, key, depth+1, log_odds_update, lazy_eval);
400 // prune node if possible, otherwise set own probability
401 // note: combining both did not lead to a speedup!
402 if (this->pruneNode(node)){
403 // return pointer to current parent (pruned), the just updated node no longer exists
404 retval = node;
405 } else{
406 node->updateOccupancyChildren();
407 }
408
409 return retval;
410 }
411 }
412
413 // at last level, update node, end of recursion
414 else {
415 if (use_change_detection) {
416 bool occBefore = this->isNodeOccupied(node);
417 updateNodeLogOdds(node, log_odds_update);
418
419 if (node_just_created){ // new node
420 changed_keys.insert(std::pair<OcTreeKey,bool>(key, true));
421 } else if (occBefore != this->isNodeOccupied(node)) { // occupancy changed, track it
422 KeyBoolMap::iterator it = changed_keys.find(key);
423 if (it == changed_keys.end())
424 changed_keys.insert(std::pair<OcTreeKey,bool>(key, false));
425 else if (it->second == false)
426 changed_keys.erase(it);
427 }
428 } else {
429 updateNodeLogOdds(node, log_odds_update);
430 }
431 return node;
432 }
433 }
434
435 // TODO: mostly copy of updateNodeRecurs => merge code or general tree modifier / traversal
436 template <class NODE>
437 NODE* OccupancyOcTreeBase<NODE>::setNodeValueRecurs(NODE* node, bool node_just_created, const OcTreeKey& key,
438 unsigned int depth, const float& log_odds_value, bool lazy_eval) {
439 bool created_node = false;
440
441 assert(node);
443 // follow down to last level
444 if (depth < this->tree_depth) {
445 unsigned int pos = computeChildIdx(key, this->tree_depth -1 - depth);
446 if (!this->nodeChildExists(node, pos)) {
447 // child does not exist, but maybe it's a pruned node?
448 if (!this->nodeHasChildren(node) && !node_just_created ) {
449 // current node does not have children AND it is not a new node
450 // -> expand pruned node
451 this->expandNode(node);
452 }
453 else {
454 // not a pruned node, create requested child
455 this->createNodeChild(node, pos);
456 created_node = true;
457 }
459
460 if (lazy_eval)
461 return setNodeValueRecurs(this->getNodeChild(node, pos), created_node, key, depth+1, log_odds_value, lazy_eval);
462 else {
463 NODE* retval = setNodeValueRecurs(this->getNodeChild(node, pos), created_node, key, depth+1, log_odds_value, lazy_eval);
464 // prune node if possible, otherwise set own probability
465 // note: combining both did not lead to a speedup!
466 if (this->pruneNode(node)){
467 // return pointer to current parent (pruned), the just updated node no longer exists
468 retval = node;
469 } else{
470 node->updateOccupancyChildren();
471 }
472
473 return retval;
475 }
476
477 // at last level, update node, end of recursion
478 else {
479 if (use_change_detection) {
480 bool occBefore = this->isNodeOccupied(node);
481 node->setLogOdds(log_odds_value);
483 if (node_just_created){ // new node
484 changed_keys.insert(std::pair<OcTreeKey,bool>(key, true));
485 } else if (occBefore != this->isNodeOccupied(node)) { // occupancy changed, track it
486 KeyBoolMap::iterator it = changed_keys.find(key);
487 if (it == changed_keys.end())
488 changed_keys.insert(std::pair<OcTreeKey,bool>(key, false));
489 else if (it->second == false)
490 changed_keys.erase(it);
491 }
492 } else {
493 node->setLogOdds(log_odds_value);
494 }
495 return node;
496 }
497 }
498
499 template <class NODE>
501 if (this->root)
502 this->updateInnerOccupancyRecurs(this->root, 0);
503 }
504
505 template <class NODE>
506 void OccupancyOcTreeBase<NODE>::updateInnerOccupancyRecurs(NODE* node, unsigned int depth){
507 assert(node);
508
509 // only recurse and update for inner nodes:
510 if (this->nodeHasChildren(node)){
511 // return early for last level:
512 if (depth < this->tree_depth){
513 for (unsigned int i=0; i<8; i++) {
514 if (this->nodeChildExists(node, i)) {
515 updateInnerOccupancyRecurs(this->getNodeChild(node, i), depth+1);
516 }
517 }
518 }
519 node->updateOccupancyChildren();
520 }
521 }
522
523 template <class NODE>
525 if (this->root == NULL)
526 return;
527
528 // convert bottom up
529 for (unsigned int depth=this->tree_depth; depth>0; depth--) {
530 toMaxLikelihoodRecurs(this->root, 0, depth);
531 }
532
533 // convert root
534 nodeToMaxLikelihood(this->root);
535 }
536
537 template <class NODE>
538 void OccupancyOcTreeBase<NODE>::toMaxLikelihoodRecurs(NODE* node, unsigned int depth,
539 unsigned int max_depth) {
540
541 assert(node);
542
543 if (depth < max_depth) {
544 for (unsigned int i=0; i<8; i++) {
545 if (this->nodeChildExists(node, i)) {
546 toMaxLikelihoodRecurs(this->getNodeChild(node, i), depth+1, max_depth);
547 }
548 }
549 }
550
551 else { // max level reached
552 nodeToMaxLikelihood(node);
553 }
554 }
555
556 template <class NODE>
557 bool OccupancyOcTreeBase<NODE>::getNormals(const point3d& point, std::vector<point3d>& normals,
558 bool unknownStatus) const {
559 normals.clear();
560
561 OcTreeKey init_key;
563 OCTOMAP_WARNING_STR("Voxel out of bounds");
564 return false;
565 }
566
567 // OCTOMAP_WARNING("Normal for %f, %f, %f\n", point.x(), point.y(), point.z());
568
569 int vertex_values[8];
570
571 OcTreeKey current_key;
572 NODE* current_node;
573
574 // There is 8 neighbouring sets
575 // The current cube can be at any of the 8 vertex
576 int x_index[4][4] = {{1, 1, 0, 0}, {1, 1, 0, 0}, {0, 0 -1, -1}, {0, 0 -1, -1}};
577 int y_index[4][4] = {{1, 0, 0, 1}, {0, -1, -1, 0}, {0, -1, -1, 0}, {1, 0, 0, 1}};
578 int z_index[2][2] = {{0, 1}, {-1, 0}};
579
580 // Iterate over the 8 neighboring sets
581 for(int m = 0; m < 2; ++m){
582 for(int l = 0; l < 4; ++l){
583
584 int k = 0;
585 // Iterate over the cubes
586 for(int j = 0; j < 2; ++j){
587 for(int i = 0; i < 4; ++i){
588 current_key[0] = init_key[0] + x_index[l][i];
589 current_key[1] = init_key[1] + y_index[l][i];
590 current_key[2] = init_key[2] + z_index[m][j];
591 current_node = this->search(current_key);
592
593 if(current_node){
594 vertex_values[k] = this->isNodeOccupied(current_node);
595
596 // point3d coord = this->keyToCoord(current_key);
597 // OCTOMAP_WARNING_STR("vertex " << k << " at " << coord << "; value " << vertex_values[k]);
598 }else{
599 // Occupancy of unknown cells
600 vertex_values[k] = unknownStatus;
601 }
602 ++k;
603 }
604 }
605
606 int cube_index = 0;
607 if (vertex_values[0]) cube_index |= 1;
608 if (vertex_values[1]) cube_index |= 2;
609 if (vertex_values[2]) cube_index |= 4;
610 if (vertex_values[3]) cube_index |= 8;
611 if (vertex_values[4]) cube_index |= 16;
612 if (vertex_values[5]) cube_index |= 32;
613 if (vertex_values[6]) cube_index |= 64;
614 if (vertex_values[7]) cube_index |= 128;
615
616 // OCTOMAP_WARNING_STR("cubde_index: " << cube_index);
617
618 // All vertices are occupied or free resulting in no normal
619 if (edgeTable[cube_index] == 0)
620 return true;
621
622 // No interpolation is done yet, we use vertexList in <MCTables.h>.
623 for(int i = 0; triTable[cube_index][i] != -1; i += 3){
624 point3d p1 = vertexList[triTable[cube_index][i ]];
625 point3d p2 = vertexList[triTable[cube_index][i+1]];
626 point3d p3 = vertexList[triTable[cube_index][i+2]];
627 point3d v1 = p2 - p1;
628 point3d v2 = p3 - p1;
629
630 // OCTOMAP_WARNING("Vertex p1 %f, %f, %f\n", p1.x(), p1.y(), p1.z());
631 // OCTOMAP_WARNING("Vertex p2 %f, %f, %f\n", p2.x(), p2.y(), p2.z());
632 // OCTOMAP_WARNING("Vertex p3 %f, %f, %f\n", p3.x(), p3.y(), p3.z());
633
634 // Right hand side cross product to retrieve the normal in the good
635 // direction (pointing to the free nodes).
636 normals.push_back(v1.cross(v2).normalize());
637 }
638 }
639 }
640
641 return true;
642 }
643
644 template <class NODE>
645 bool OccupancyOcTreeBase<NODE>::castRay(const point3d& origin, const point3d& directionP, point3d& end,
646 bool ignoreUnknown, double maxRange) const {
647
649
650 // Initialization phase -------------------------------------------------------
651 OcTreeKey current_key;
653 OCTOMAP_WARNING_STR("Coordinates out of bounds during ray casting");
654 return false;
655 }
656
657 NODE* startingNode = this->search(current_key);
658 if (startingNode){
659 if (this->isNodeOccupied(startingNode)){
660 // Occupied node found at origin
661 // (need to convert from key, since origin does not need to be a voxel center)
662 end = this->keyToCoord(current_key);
663 return true;
664 }
665 } else if(!ignoreUnknown){
666 end = this->keyToCoord(current_key);
667 return false;
668 }
669
670 point3d direction = directionP.normalized();
671 bool max_range_set = (maxRange > 0.0);
672
673 int step[3];
674 double tMax[3];
675 double tDelta[3];
676
677 for(unsigned int i=0; i < 3; ++i) {
678 // compute step direction
679 if (direction(i) > 0.0) step[i] = 1;
680 else if (direction(i) < 0.0) step[i] = -1;
681 else step[i] = 0;
682
683 // compute tMax, tDelta
684 if (step[i] != 0) {
685 // corner point of voxel (in direction of ray)
686 double voxelBorder = this->keyToCoord(current_key[i]);
687 voxelBorder += double(step[i] * this->resolution * 0.5);
688
689 tMax[i] = ( voxelBorder - origin(i) ) / direction(i);
690 tDelta[i] = this->resolution / fabs( direction(i) );
691 }
692 else {
693 tMax[i] = std::numeric_limits<double>::max();
694 tDelta[i] = std::numeric_limits<double>::max();
695 }
696 }
697
698 if (step[0] == 0 && step[1] == 0 && step[2] == 0){
699 OCTOMAP_ERROR("Raycasting in direction (0,0,0) is not possible!");
700 return false;
701 }
702
703 // for speedup:
704 double maxrange_sq = maxRange *maxRange;
705
706 // Incremental phase ---------------------------------------------------------
707
708 bool done = false;
709
710 while (!done) {
711 unsigned int dim;
712
713 // find minimum tMax:
714 if (tMax[0] < tMax[1]){
715 if (tMax[0] < tMax[2]) dim = 0;
716 else dim = 2;
717 }
718 else {
719 if (tMax[1] < tMax[2]) dim = 1;
720 else dim = 2;
721 }
722
723 // check for overflow:
724 if ((step[dim] < 0 && current_key[dim] == 0)
725 || (step[dim] > 0 && current_key[dim] == 2* this->tree_max_val-1))
726 {
727 OCTOMAP_WARNING("Coordinate hit bounds in dim %d, aborting raycast\n", dim);
728 // return border point nevertheless:
729 end = this->keyToCoord(current_key);
730 return false;
731 }
732
733 // advance in direction "dim"
734 current_key[dim] += step[dim];
735 tMax[dim] += tDelta[dim];
736
737
738 // generate world coords from key
739 end = this->keyToCoord(current_key);
740
741 // check for maxrange:
742 if (max_range_set){
743 double dist_from_origin_sq(0.0);
744 for (unsigned int j = 0; j < 3; j++) {
745 dist_from_origin_sq += ((end(j) - origin(j)) * (end(j) - origin(j)));
746 }
747 if (dist_from_origin_sq > maxrange_sq)
748 return false;
749
750 }
751
752 NODE* currentNode = this->search(current_key);
753 if (currentNode){
754 if (this->isNodeOccupied(currentNode)) {
755 done = true;
756 break;
757 }
758 // otherwise: node is free and valid, raycasting continues
759 } else if (!ignoreUnknown){ // no node found, this usually means we are in "unknown" areas
760 return false;
761 }
762 } // end while
763
764 return true;
765 }
766
767 template <class NODE>
768 bool OccupancyOcTreeBase<NODE>::getRayIntersection (const point3d& origin, const point3d& direction, const point3d& center,
769 point3d& intersection, double delta/*=0.0*/) const {
770 // We only need three normals for the six planes
771 octomap::point3d normalX(1, 0, 0);
772 octomap::point3d normalY(0, 1, 0);
773 octomap::point3d normalZ(0, 0, 1);
774
775 // One point on each plane, let them be the center for simplicity
776 octomap::point3d pointXNeg(center(0) - float(this->resolution / 2.0), center(1), center(2));
777 octomap::point3d pointXPos(center(0) + float(this->resolution / 2.0), center(1), center(2));
778 octomap::point3d pointYNeg(center(0), center(1) - float(this->resolution / 2.0), center(2));
779 octomap::point3d pointYPos(center(0), center(1) + float(this->resolution / 2.0), center(2));
780 octomap::point3d pointZNeg(center(0), center(1), center(2) - float(this->resolution / 2.0));
781 octomap::point3d pointZPos(center(0), center(1), center(2) + float(this->resolution / 2.0));
782
783 double lineDotNormal = 0.0;
784 double d = 0.0;
785 double outD = std::numeric_limits<double>::max();
786 octomap::point3d intersect;
787 bool found = false;
788
789 // Find the intersection (if any) with each place
790 // Line dot normal will be zero if they are parallel, in which case no intersection can be the entry one
791 // if there is an intersection does it occur in the bounded plane of the voxel
792 // if yes keep only the closest (smallest distance to sensor origin).
793 if((lineDotNormal = normalX.dot(direction)) != 0.0){ // Ensure lineDotNormal is non-zero (assign and test)
794 d = (pointXNeg - origin).dot(normalX) / lineDotNormal;
795 intersect = direction * float(d) + origin;
796 if(!(intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6) ||
797 intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
798 outD = std::min(outD, d);
799 found = true;
800 }
801
802 d = (pointXPos - origin).dot(normalX) / lineDotNormal;
803 intersect = direction * float(d) + origin;
804 if(!(intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6) ||
805 intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
806 outD = std::min(outD, d);
807 found = true;
808 }
809 }
810
811 if((lineDotNormal = normalY.dot(direction)) != 0.0){ // Ensure lineDotNormal is non-zero (assign and test)
812 d = (pointYNeg - origin).dot(normalY) / lineDotNormal;
813 intersect = direction * float(d) + origin;
814 if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
815 intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
816 outD = std::min(outD, d);
817 found = true;
818 }
819
820 d = (pointYPos - origin).dot(normalY) / lineDotNormal;
821 intersect = direction * float(d) + origin;
822 if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
823 intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
824 outD = std::min(outD, d);
825 found = true;
826 }
827 }
828
829 if((lineDotNormal = normalZ.dot(direction)) != 0.0){ // Ensure lineDotNormal is non-zero (assign and test)
830 d = (pointZNeg - origin).dot(normalZ) / lineDotNormal;
831 intersect = direction * float(d) + origin;
832 if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
833 intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6))){
834 outD = std::min(outD, d);
835 found = true;
836 }
837
838 d = (pointZPos - origin).dot(normalZ) / lineDotNormal;
839 intersect = direction * float(d) + origin;
840 if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
841 intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6))){
842 outD = std::min(outD, d);
843 found = true;
844 }
845 }
846
847 // Substract (add) a fraction to ensure no ambiguity on the starting voxel
848 // Don't start on a boundary.
849 if(found)
850 intersection = direction * float(outD + delta) + origin;
851
852 return found;
853 }
854
855
856 template <class NODE> inline bool
857 OccupancyOcTreeBase<NODE>::integrateMissOnRay(const point3d& origin, const point3d& end, bool lazy_eval) {
858
859 if (!this->computeRayKeys(origin, end, this->keyrays.at(0))) {
860 return false;
861 }
862
863 for(KeyRay::iterator it=this->keyrays[0].begin(); it != this->keyrays[0].end(); it++) {
864 updateNode(*it, false, lazy_eval); // insert freespace measurement
865 }
866
867 return true;
868 }
869
870 template <class NODE> bool
871 OccupancyOcTreeBase<NODE>::insertRay(const point3d& origin, const point3d& end, double maxrange, bool lazy_eval)
872 {
873 // cut ray at maxrange
874 if ((maxrange > 0) && ((end - origin).norm () > maxrange))
875 {
876 point3d direction = (end - origin).normalized ();
877 point3d new_end = origin + direction * (float) maxrange;
878 return integrateMissOnRay(origin, new_end,lazy_eval);
879 }
880 // insert complete ray
881 else
882 {
883 if (!integrateMissOnRay(origin, end,lazy_eval))
884 return false;
885 updateNode(end, true, lazy_eval); // insert hit cell
886 return true;
887 }
888 }
889
890 template <class NODE>
892 bbx_min = min;
893 if (!this->coordToKeyChecked(bbx_min, bbx_min_key)) {
894 OCTOMAP_ERROR("ERROR while generating bbx min key.\n");
895 }
896 }
897
898 template <class NODE>
900 bbx_max = max;
901 if (!this->coordToKeyChecked(bbx_max, bbx_max_key)) {
902 OCTOMAP_ERROR("ERROR while generating bbx max key.\n");
903 }
904 }
905
906
907 template <class NODE>
909 return ((p.x() >= bbx_min.x()) && (p.y() >= bbx_min.y()) && (p.z() >= bbx_min.z()) &&
910 (p.x() <= bbx_max.x()) && (p.y() <= bbx_max.y()) && (p.z() <= bbx_max.z()) );
911 }
912
913
914 template <class NODE>
916 return ((key[0] >= bbx_min_key[0]) && (key[1] >= bbx_min_key[1]) && (key[2] >= bbx_min_key[2]) &&
917 (key[0] <= bbx_max_key[0]) && (key[1] <= bbx_max_key[1]) && (key[2] <= bbx_max_key[2]) );
918 }
919
920 template <class NODE>
922 octomap::point3d obj_bounds = (bbx_max - bbx_min);
923 obj_bounds /= 2.;
924 return obj_bounds;
925 }
926
927 template <class NODE>
929 octomap::point3d obj_bounds = (bbx_max - bbx_min);
930 obj_bounds /= 2.;
931 return bbx_min + obj_bounds;
932 }
933
934 // -- I/O -----------------------------------------
935
936 template <class NODE>
937 std::istream& OccupancyOcTreeBase<NODE>::readBinaryData(std::istream &s){
938 // tree needs to be newly created or cleared externally
939 if (this->root) {
940 OCTOMAP_ERROR_STR("Trying to read into an existing tree.");
941 return s;
942 }
943
944 this->root = new NODE();
945 this->readBinaryNode(s, this->root);
946 this->size_changed = true;
947 this->tree_size = OcTreeBaseImpl<NODE,AbstractOccupancyOcTree>::calcNumNodes(); // compute number of nodes
948 return s;
949 }
950
951 template <class NODE>
952 std::ostream& OccupancyOcTreeBase<NODE>::writeBinaryData(std::ostream &s) const{
953 OCTOMAP_DEBUG("Writing %zu nodes to output stream...", this->size());
954 if (this->root)
955 this->writeBinaryNode(s, this->root);
956 return s;
957 }
958
959 template <class NODE>
960 std::istream& OccupancyOcTreeBase<NODE>::readBinaryNode(std::istream &s, NODE* node){
961
962 assert(node);
963
964 char child1to4_char;
965 char child5to8_char;
966 s.read((char*)&child1to4_char, sizeof(char));
967 s.read((char*)&child5to8_char, sizeof(char));
968
969 std::bitset<8> child1to4 ((unsigned long long) child1to4_char);
970 std::bitset<8> child5to8 ((unsigned long long) child5to8_char);
971
972 // std::cout << "read: "
973 // << child1to4.to_string<char,std::char_traits<char>,std::allocator<char> >() << " "
974 // << child5to8.to_string<char,std::char_traits<char>,std::allocator<char> >() << std::endl;
975
976
977 // inner nodes default to occupied
978 node->setLogOdds(this->clamping_thres_max);
979
980 for (unsigned int i=0; i<4; i++) {
981 if ((child1to4[i*2] == 1) && (child1to4[i*2+1] == 0)) {
982 // child is free leaf
983 this->createNodeChild(node, i);
984 this->getNodeChild(node, i)->setLogOdds(this->clamping_thres_min);
985 }
986 else if ((child1to4[i*2] == 0) && (child1to4[i*2+1] == 1)) {
987 // child is occupied leaf
988 this->createNodeChild(node, i);
989 this->getNodeChild(node, i)->setLogOdds(this->clamping_thres_max);
990 }
991 else if ((child1to4[i*2] == 1) && (child1to4[i*2+1] == 1)) {
992 // child has children
993 this->createNodeChild(node, i);
994 this->getNodeChild(node, i)->setLogOdds(-200.); // child is unkown, we leave it uninitialized
995 }
996 }
997 for (unsigned int i=0; i<4; i++) {
998 if ((child5to8[i*2] == 1) && (child5to8[i*2+1] == 0)) {
999 // child is free leaf
1000 this->createNodeChild(node, i+4);
1001 this->getNodeChild(node, i+4)->setLogOdds(this->clamping_thres_min);
1002 }
1003 else if ((child5to8[i*2] == 0) && (child5to8[i*2+1] == 1)) {
1004 // child is occupied leaf
1005 this->createNodeChild(node, i+4);
1006 this->getNodeChild(node, i+4)->setLogOdds(this->clamping_thres_max);
1007 }
1008 else if ((child5to8[i*2] == 1) && (child5to8[i*2+1] == 1)) {
1009 // child has children
1010 this->createNodeChild(node, i+4);
1011 this->getNodeChild(node, i+4)->setLogOdds(-200.); // set occupancy when all children have been read
1012 }
1013 // child is unkown, we leave it uninitialized
1014 }
1015
1016 // read children's children and set the label
1017 for (unsigned int i=0; i<8; i++) {
1018 if (this->nodeChildExists(node, i)) {
1019 NODE* child = this->getNodeChild(node, i);
1020 if (fabs(child->getLogOdds() + 200.)<1e-3) {
1021 readBinaryNode(s, child);
1022 child->setLogOdds(child->getMaxChildLogOdds());
1023 }
1024 } // end if child exists
1025 } // end for children
1026
1027 return s;
1028 }
1029
1030 template <class NODE>
1031 std::ostream& OccupancyOcTreeBase<NODE>::writeBinaryNode(std::ostream &s, const NODE* node) const{
1032
1033 assert(node);
1034
1035 // 2 bits for each children, 8 children per node -> 16 bits
1036 std::bitset<8> child1to4;
1037 std::bitset<8> child5to8;
1038
1039 // 10 : child is free node
1040 // 01 : child is occupied node
1041 // 00 : child is unkown node
1042 // 11 : child has children
1043
1044
1045 // speedup: only set bits to 1, rest is init with 0 anyway,
1046 // can be one logic expression per bit
1047
1048 for (unsigned int i=0; i<4; i++) {
1049 if (this->nodeChildExists(node, i)) {
1050 const NODE* child = this->getNodeChild(node, i);
1051 if (this->nodeHasChildren(child)) { child1to4[i*2] = 1; child1to4[i*2+1] = 1; }
1052 else if (this->isNodeOccupied(child)) { child1to4[i*2] = 0; child1to4[i*2+1] = 1; }
1053 else { child1to4[i*2] = 1; child1to4[i*2+1] = 0; }
1054 }
1055 else {
1056 child1to4[i*2] = 0; child1to4[i*2+1] = 0;
1057 }
1058 }
1059
1060 for (unsigned int i=0; i<4; i++) {
1061 if (this->nodeChildExists(node, i+4)) {
1062 const NODE* child = this->getNodeChild(node, i+4);
1063 if (this->nodeHasChildren(child)) { child5to8[i*2] = 1; child5to8[i*2+1] = 1; }
1064 else if (this->isNodeOccupied(child)) { child5to8[i*2] = 0; child5to8[i*2+1] = 1; }
1065 else { child5to8[i*2] = 1; child5to8[i*2+1] = 0; }
1066 }
1067 else {
1068 child5to8[i*2] = 0; child5to8[i*2+1] = 0;
1069 }
1070 }
1071 // std::cout << "wrote: "
1072 // << child1to4.to_string<char,std::char_traits<char>,std::allocator<char> >() << " "
1073 // << child5to8.to_string<char,std::char_traits<char>,std::allocator<char> >() << std::endl;
1074
1075 char child1to4_char = (char) child1to4.to_ulong();
1076 char child5to8_char = (char) child5to8.to_ulong();
1077
1078 s.write((char*)&child1to4_char, sizeof(char));
1079 s.write((char*)&child5to8_char, sizeof(char));
1080
1081 // write children's children
1082 for (unsigned int i=0; i<8; i++) {
1083 if (this->nodeChildExists(node, i)) {
1084 const NODE* child = this->getNodeChild(node, i);
1085 if (this->nodeHasChildren(child)) {
1086 writeBinaryNode(s, child);
1087 }
1088 }
1089 }
1090
1091 return s;
1092 }
1093
1094 //-- Occupancy queries on nodes:
1095
1096 template <class NODE>
1097 void OccupancyOcTreeBase<NODE>::updateNodeLogOdds(NODE* occupancyNode, const float& update) const {
1098 occupancyNode->addValue(update);
1099 if (occupancyNode->getLogOdds() < this->clamping_thres_min) {
1100 occupancyNode->setLogOdds(this->clamping_thres_min);
1101 return;
1102 }
1103 if (occupancyNode->getLogOdds() > this->clamping_thres_max) {
1104 occupancyNode->setLogOdds(this->clamping_thres_max);
1105 }
1106 }
1107
1108 template <class NODE>
1109 void OccupancyOcTreeBase<NODE>::integrateHit(NODE* occupancyNode) const {
1110 updateNodeLogOdds(occupancyNode, this->prob_hit_log);
1111 }
1112
1113 template <class NODE>
1114 void OccupancyOcTreeBase<NODE>::integrateMiss(NODE* occupancyNode) const {
1115 updateNodeLogOdds(occupancyNode, this->prob_miss_log);
1116 }
1117
1118 template <class NODE>
1119 void OccupancyOcTreeBase<NODE>::nodeToMaxLikelihood(NODE* occupancyNode) const{
1120 if (this->isNodeOccupied(occupancyNode))
1121 occupancyNode->setLogOdds(this->clamping_thres_max);
1122 else
1123 occupancyNode->setLogOdds(this->clamping_thres_min);
1124 }
1125
1126 template <class NODE>
1127 void OccupancyOcTreeBase<NODE>::nodeToMaxLikelihood(NODE& occupancyNode) const{
1128 if (this->isNodeOccupied(occupancyNode))
1129 occupancyNode.setLogOdds(this->clamping_thres_max);
1130 else
1131 occupancyNode.setLogOdds(this->clamping_thres_min);
1132 }
1133
1134} // namespace
Interface class for all octree types that store occupancy.
Definition: AbstractOccupancyOcTree.h:52
float prob_miss_log
Definition: AbstractOccupancyOcTree.h:232
float clamping_thres_min
Definition: AbstractOccupancyOcTree.h:229
float prob_hit_log
Definition: AbstractOccupancyOcTree.h:231
float clamping_thres_max
Definition: AbstractOccupancyOcTree.h:230
float occ_prob_thres_log
Definition: AbstractOccupancyOcTree.h:233
Definition: OcTreeKey.h:139
iterator begin()
Definition: OcTreeKey.h:170
std::vector< OcTreeKey >::iterator iterator
Definition: OcTreeKey.h:166
iterator end()
Definition: OcTreeKey.h:171
OcTree base class, to be used with with any kind of OcTreeDataNode.
Definition: OcTreeBaseImpl.h:75
size_t calcNumNodes() const
Traverses the tree to calculate the total number of nodes.
Definition: OcTreeBaseImpl.hxx:1016
OcTreeKey is a container class for internal key addressing.
Definition: OcTreeKey.h:70
Base implementation for Occupancy Octrees (e.g.
Definition: OccupancyOcTreeBase.h:69
point3d getBBXCenter() const
Definition: OccupancyOcTreeBase.hxx:928
virtual ~OccupancyOcTreeBase()
Definition: OccupancyOcTreeBase.hxx:56
void updateInnerOccupancy()
Updates the occupancy of all inner nodes to reflect their children's occupancy.
Definition: OccupancyOcTreeBase.hxx:500
void computeDiscreteUpdate(const Pointcloud &scan, const octomap::point3d &origin, KeySet &free_cells, KeySet &occupied_cells, double maxrange)
Helper for insertPointCloud().
Definition: OccupancyOcTreeBase.hxx:148
virtual void integrateHit(NODE *occupancyNode) const
integrate a "hit" measurement according to the tree's sensor model
Definition: OccupancyOcTreeBase.hxx:1109
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
Manipulate log_odds value of a voxel by changing it by log_odds_update (relative).
Definition: OccupancyOcTreeBase.hxx:307
virtual void updateNodeLogOdds(NODE *occupancyNode, const float &update) const
update logodds value of node by adding to the current value.
Definition: OccupancyOcTreeBase.hxx:1097
virtual bool insertRay(const point3d &origin, const point3d &end, double maxrange=-1.0, bool lazy_eval=false)
Insert one ray between origin and end into the tree.
Definition: OccupancyOcTreeBase.hxx:871
virtual void insertPointCloud(const Pointcloud &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false, bool discretize=false)
Integrate a Pointcloud (in global reference frame), parallelized with OpenMP.
Definition: OccupancyOcTreeBase.hxx:86
point3d getBBXBounds() const
Definition: OccupancyOcTreeBase.hxx:921
std::istream & readBinaryNode(std::istream &s, NODE *node)
Read node from binary stream (max-likelihood value), recursively continue with all children.
Definition: OccupancyOcTreeBase.hxx:960
void computeUpdate(const Pointcloud &scan, const octomap::point3d &origin, KeySet &free_cells, KeySet &occupied_cells, double maxrange)
Helper for insertPointCloud().
Definition: OccupancyOcTreeBase.hxx:169
virtual bool getRayIntersection(const point3d &origin, const point3d &direction, const point3d &center, point3d &intersection, double delta=0.0) const
Retrieves the entry point of a ray into a voxel.
Definition: OccupancyOcTreeBase.hxx:768
virtual bool castRay(const point3d &origin, const point3d &direction, point3d &end, bool ignoreUnknownCells=false, double maxRange=-1.0) const
Performs raycasting in 3d, similar to computeRay().
Definition: OccupancyOcTreeBase.hxx:645
virtual NODE * setNodeValue(const OcTreeKey &key, float log_odds_value, bool lazy_eval=false)
Set log_odds value of voxel to log_odds_value.
Definition: OccupancyOcTreeBase.hxx:273
std::ostream & writeBinaryNode(std::ostream &s, const NODE *node) const
Write node to binary stream (max-likelihood value), recursively continue with all children.
Definition: OccupancyOcTreeBase.hxx:1031
virtual void integrateMiss(NODE *occupancyNode) const
integrate a "miss" measurement according to the tree's sensor model
Definition: OccupancyOcTreeBase.hxx:1114
virtual void insertPointCloudRays(const Pointcloud &scan, const point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false)
Integrate a Pointcloud (in global reference frame), parallelized with OpenMP.
Definition: OccupancyOcTreeBase.hxx:116
std::ostream & writeBinaryData(std::ostream &s) const
Writes the data of the tree (without header) to the stream, recursively calling writeBinaryNode (star...
Definition: OccupancyOcTreeBase.hxx:952
bool inBBX(const point3d &p) const
Definition: OccupancyOcTreeBase.hxx:908
bool integrateMissOnRay(const point3d &origin, const point3d &end, bool lazy_eval=false)
Traces a ray from origin to end and updates all voxels on the way as free.
Definition: OccupancyOcTreeBase.hxx:857
void setBBXMax(const point3d &max)
sets the maximum for a query bounding box to use
Definition: OccupancyOcTreeBase.hxx:899
bool getNormals(const point3d &point, std::vector< point3d > &normals, bool unknownStatus=true) const
Performs a step of the marching cubes surface reconstruction algorithm to retrieve the normal of the ...
Definition: OccupancyOcTreeBase.hxx:557
NODE * updateNodeRecurs(NODE *node, bool node_just_created, const OcTreeKey &key, unsigned int depth, const float &log_odds_update, bool lazy_eval=false)
Definition: OccupancyOcTreeBase.hxx:373
std::istream & readBinaryData(std::istream &s)
Reads only the data (=complete tree structure) from the input stream.
Definition: OccupancyOcTreeBase.hxx:937
void setBBXMin(const point3d &min)
sets the minimum for a query bounding box to use
Definition: OccupancyOcTreeBase.hxx:891
virtual void toMaxLikelihood()
Creates the maximum likelihood map by calling toMaxLikelihood on all tree nodes, setting their occupa...
Definition: OccupancyOcTreeBase.hxx:524
virtual void nodeToMaxLikelihood(NODE *occupancyNode) const
converts the node to the maximum likelihood value according to the tree's parameter for "occupancy"
Definition: OccupancyOcTreeBase.hxx:1119
void updateInnerOccupancyRecurs(NODE *node, unsigned int depth)
Definition: OccupancyOcTreeBase.hxx:506
NODE * setNodeValueRecurs(NODE *node, bool node_just_created, const OcTreeKey &key, unsigned int depth, const float &log_odds_value, bool lazy_eval=false)
Definition: OccupancyOcTreeBase.hxx:437
void toMaxLikelihoodRecurs(NODE *node, unsigned int depth, unsigned int max_depth)
Definition: OccupancyOcTreeBase.hxx:538
OccupancyOcTreeBase(double resolution)
Default constructor, sets resolution of leafs.
Definition: OccupancyOcTreeBase.hxx:42
A collection of 3D coordinates (point3d), which are regarded as endpoints of a 3D laser scan.
Definition: Pointcloud.h:47
size_t size() const
Definition: Pointcloud.h:57
void reserve(size_t size)
Definition: Pointcloud.h:59
void push_back(float x, float y, float z)
Definition: Pointcloud.h:61
void transform(pose6d transform)
Apply transform to each point.
Definition: Pointcloud.cpp:102
A 3D scan as Pointcloud, performed from a Pose6D.
Definition: ScanGraph.h:52
Pointcloud * scan
Definition: ScanGraph.h:73
pose6d pose
6D pose from which the scan was performed
Definition: ScanGraph.h:74
This class represents a tree-dimensional pose of an object.
Definition: Pose6D.h:49
Pose6D inv() const
Inversion.
Definition: Pose6D.cpp:70
Vector3 & trans()
Translational component.
Definition: Pose6D.h:82
Vector3 transform(const Vector3 &v) const
Transformation of a vector.
Definition: Pose6D.cpp:85
This class represents a three-dimensional vector.
Definition: Vector3.h:50
Vector3 & normalize()
normalizes this vector, so that it has norm=1.0
Definition: Vector3.h:270
float & x()
Definition: Vector3.h:131
Vector3 cross(const Vector3 &other) const
Three-dimensional vector (cross) product.
Definition: Vector3.h:107
double dot(const Vector3 &other) const
dot product
Definition: Vector3.h:117
float & y()
Definition: Vector3.h:136
float & z()
Definition: Vector3.h:141
Vector3 normalized() const
Definition: Vector3.h:278
Namespace the OctoMap library and visualization tools.
static const int triTable[256][16]
Definition: MCTables.h:78
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:49
static const int edgeTable[256]
Definition: MCTables.h:44
uint8_t computeChildIdx(const OcTreeKey &key, int depth)
generate child index (between 0 and 7) from key at given tree depth
Definition: OcTreeKey.h:207
unordered_ns::unordered_set< OcTreeKey, OcTreeKey::KeyHash > KeySet
Data structure to efficiently compute the nodes to update from a scan insertion using a hash set.
Definition: OcTreeKey.h:129
static const point3d vertexList[12]
Definition: MCTables.h:336
#define OCTOMAP_WARNING(...)
Definition: octomap_types.h:76
#define OCTOMAP_ERROR_STR(args)
Definition: octomap_types.h:79
#define OCTOMAP_DEBUG(...)
Definition: octomap_types.h:72
#define OCTOMAP_WARNING_STR(args)
Definition: octomap_types.h:77
#define OCTOMAP_ERROR(...)
Definition: octomap_types.h:78