octomap  1.9.7
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 
39 namespace 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  {
66  this->clamping_thres_min = rhs.clamping_thres_min;
67  this->clamping_thres_max = rhs.clamping_thres_max;
68  this->prob_hit_log = rhs.prob_hit_log;
69  this->prob_miss_log = rhs.prob_miss_log;
70  this->occ_prob_thres_log = rhs.occ_prob_thres_log;
71 
72 
73  }
74 
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  }
146 
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
196  {
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);
208  }
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 
237  // update freespace, break as soon as bbx limit is reached
238  if (this->computeRayKeys(origin, p, *keyray)){
239  for(KeyRay::reverse_iterator rit=keyray->rbegin(); rit != keyray->rend(); rit++) {
240  if (inBBX(*rit)) {
241 #ifdef _OPENMP
242  #pragma omp critical (free_insert)
243 #endif
244  {
245  free_cells.insert(*rit);
246  }
247  }
248  else break;
249  }
250  } // end if compute ray
251  } // end if in BBX and not maxrange
252  } // end bbx case
253 
254  } // end for all points, end of parallel OMP loop
255 
256  // prefer occupied cells over free ones (and make sets disjunct)
257  for(KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ){
258  if (occupied_cells.find(*it) != occupied_cells.end()){
259  it = free_cells.erase(it);
260  } else {
261  ++it;
262  }
263  }
264  }
265 
266  template <class NODE>
267  NODE* OccupancyOcTreeBase<NODE>::setNodeValue(const OcTreeKey& key, float log_odds_value, bool lazy_eval) {
268  // clamp log odds within range:
269  log_odds_value = std::min(std::max(log_odds_value, this->clamping_thres_min), this->clamping_thres_max);
270 
271  bool createdRoot = false;
272  if (this->root == NULL){
273  this->root = new NODE();
274  this->tree_size++;
275  createdRoot = true;
276  }
277 
278  return setNodeValueRecurs(this->root, createdRoot, key, 0, log_odds_value, lazy_eval);
279  }
280 
281  template <class NODE>
282  NODE* OccupancyOcTreeBase<NODE>::setNodeValue(const point3d& value, float log_odds_value, bool lazy_eval) {
283  OcTreeKey key;
284  if (!this->coordToKeyChecked(value, key))
285  return NULL;
286 
287  return setNodeValue(key, log_odds_value, lazy_eval);
288  }
289 
290  template <class NODE>
291  NODE* OccupancyOcTreeBase<NODE>::setNodeValue(double x, double y, double z, float log_odds_value, bool lazy_eval) {
292  OcTreeKey key;
293  if (!this->coordToKeyChecked(x, y, z, key))
294  return NULL;
295 
296  return setNodeValue(key, log_odds_value, lazy_eval);
297  }
298 
299 
300  template <class NODE>
301  NODE* OccupancyOcTreeBase<NODE>::updateNode(const OcTreeKey& key, float log_odds_update, bool lazy_eval) {
302  // early abort (no change will happen).
303  // may cause an overhead in some configuration, but more often helps
304  NODE* leaf = this->search(key);
305  // no change: node already at threshold
306  if (leaf
307  && ((log_odds_update >= 0 && leaf->getLogOdds() >= this->clamping_thres_max)
308  || ( log_odds_update <= 0 && leaf->getLogOdds() <= this->clamping_thres_min)))
309  {
310  return leaf;
311  }
312 
313  bool createdRoot = false;
314  if (this->root == NULL){
315  this->root = new NODE();
316  this->tree_size++;
317  createdRoot = true;
318  }
319 
320  return updateNodeRecurs(this->root, createdRoot, key, 0, log_odds_update, lazy_eval);
321  }
322 
323  template <class NODE>
324  NODE* OccupancyOcTreeBase<NODE>::updateNode(const point3d& value, float log_odds_update, bool lazy_eval) {
325  OcTreeKey key;
326  if (!this->coordToKeyChecked(value, key))
327  return NULL;
328 
329  return updateNode(key, log_odds_update, lazy_eval);
330  }
331 
332  template <class NODE>
333  NODE* OccupancyOcTreeBase<NODE>::updateNode(double x, double y, double z, float log_odds_update, bool lazy_eval) {
334  OcTreeKey key;
335  if (!this->coordToKeyChecked(x, y, z, key))
336  return NULL;
337 
338  return updateNode(key, log_odds_update, lazy_eval);
339  }
340 
341  template <class NODE>
342  NODE* OccupancyOcTreeBase<NODE>::updateNode(const OcTreeKey& key, bool occupied, bool lazy_eval) {
343  float logOdds = this->prob_miss_log;
344  if (occupied)
345  logOdds = this->prob_hit_log;
346 
347  return updateNode(key, logOdds, lazy_eval);
348  }
349 
350  template <class NODE>
351  NODE* OccupancyOcTreeBase<NODE>::updateNode(const point3d& value, bool occupied, bool lazy_eval) {
352  OcTreeKey key;
353  if (!this->coordToKeyChecked(value, key))
354  return NULL;
355  return updateNode(key, occupied, lazy_eval);
356  }
357 
358  template <class NODE>
359  NODE* OccupancyOcTreeBase<NODE>::updateNode(double x, double y, double z, bool occupied, bool lazy_eval) {
360  OcTreeKey key;
361  if (!this->coordToKeyChecked(x, y, z, key))
362  return NULL;
363  return updateNode(key, occupied, lazy_eval);
364  }
365 
366  template <class NODE>
367  NODE* OccupancyOcTreeBase<NODE>::updateNodeRecurs(NODE* node, bool node_just_created, const OcTreeKey& key,
368  unsigned int depth, const float& log_odds_update, bool lazy_eval) {
369  bool created_node = false;
370 
371  assert(node);
372 
373  // follow down to last level
374  if (depth < this->tree_depth) {
375  unsigned int pos = computeChildIdx(key, this->tree_depth -1 - depth);
376  if (!this->nodeChildExists(node, pos)) {
377  // child does not exist, but maybe it's a pruned node?
378  if (!this->nodeHasChildren(node) && !node_just_created ) {
379  // current node does not have children AND it is not a new node
380  // -> expand pruned node
381  this->expandNode(node);
382  }
383  else {
384  // not a pruned node, create requested child
385  this->createNodeChild(node, pos);
386  created_node = true;
387  }
388  }
389 
390  if (lazy_eval)
391  return updateNodeRecurs(this->getNodeChild(node, pos), created_node, key, depth+1, log_odds_update, lazy_eval);
392  else {
393  NODE* retval = updateNodeRecurs(this->getNodeChild(node, pos), created_node, key, depth+1, log_odds_update, lazy_eval);
394  // prune node if possible, otherwise set own probability
395  // note: combining both did not lead to a speedup!
396  if (this->pruneNode(node)){
397  // return pointer to current parent (pruned), the just updated node no longer exists
398  retval = node;
399  } else{
400  node->updateOccupancyChildren();
401  }
402 
403  return retval;
404  }
405  }
406 
407  // at last level, update node, end of recursion
408  else {
409  if (use_change_detection) {
410  bool occBefore = this->isNodeOccupied(node);
411  updateNodeLogOdds(node, log_odds_update);
412 
413  if (node_just_created){ // new node
414  changed_keys.insert(std::pair<OcTreeKey,bool>(key, true));
415  } else if (occBefore != this->isNodeOccupied(node)) { // occupancy changed, track it
416  KeyBoolMap::iterator it = changed_keys.find(key);
417  if (it == changed_keys.end())
418  changed_keys.insert(std::pair<OcTreeKey,bool>(key, false));
419  else if (it->second == false)
420  changed_keys.erase(it);
421  }
422  } else {
423  updateNodeLogOdds(node, log_odds_update);
424  }
425  return node;
426  }
427  }
428 
429  // TODO: mostly copy of updateNodeRecurs => merge code or general tree modifier / traversal
430  template <class NODE>
431  NODE* OccupancyOcTreeBase<NODE>::setNodeValueRecurs(NODE* node, bool node_just_created, const OcTreeKey& key,
432  unsigned int depth, const float& log_odds_value, bool lazy_eval) {
433  bool created_node = false;
434 
435  assert(node);
436 
437  // follow down to last level
438  if (depth < this->tree_depth) {
439  unsigned int pos = computeChildIdx(key, this->tree_depth -1 - depth);
440  if (!this->nodeChildExists(node, pos)) {
441  // child does not exist, but maybe it's a pruned node?
442  if (!this->nodeHasChildren(node) && !node_just_created ) {
443  // current node does not have children AND it is not a new node
444  // -> expand pruned node
445  this->expandNode(node);
446  }
447  else {
448  // not a pruned node, create requested child
449  this->createNodeChild(node, pos);
450  created_node = true;
451  }
452  }
453 
454  if (lazy_eval)
455  return setNodeValueRecurs(this->getNodeChild(node, pos), created_node, key, depth+1, log_odds_value, lazy_eval);
456  else {
457  NODE* retval = setNodeValueRecurs(this->getNodeChild(node, pos), created_node, key, depth+1, log_odds_value, lazy_eval);
458  // prune node if possible, otherwise set own probability
459  // note: combining both did not lead to a speedup!
460  if (this->pruneNode(node)){
461  // return pointer to current parent (pruned), the just updated node no longer exists
462  retval = node;
463  } else{
464  node->updateOccupancyChildren();
465  }
466 
467  return retval;
468  }
469  }
470 
471  // at last level, update node, end of recursion
472  else {
473  if (use_change_detection) {
474  bool occBefore = this->isNodeOccupied(node);
475  node->setLogOdds(log_odds_value);
476 
477  if (node_just_created){ // new node
478  changed_keys.insert(std::pair<OcTreeKey,bool>(key, true));
479  } else if (occBefore != this->isNodeOccupied(node)) { // occupancy changed, track it
480  KeyBoolMap::iterator it = changed_keys.find(key);
481  if (it == changed_keys.end())
482  changed_keys.insert(std::pair<OcTreeKey,bool>(key, false));
483  else if (it->second == false)
484  changed_keys.erase(it);
485  }
486  } else {
487  node->setLogOdds(log_odds_value);
488  }
489  return node;
490  }
491  }
492 
493  template <class NODE>
495  if (this->root)
496  this->updateInnerOccupancyRecurs(this->root, 0);
497  }
498 
499  template <class NODE>
500  void OccupancyOcTreeBase<NODE>::updateInnerOccupancyRecurs(NODE* node, unsigned int depth){
501  assert(node);
502 
503  // only recurse and update for inner nodes:
504  if (this->nodeHasChildren(node)){
505  // return early for last level:
506  if (depth < this->tree_depth){
507  for (unsigned int i=0; i<8; i++) {
508  if (this->nodeChildExists(node, i)) {
509  updateInnerOccupancyRecurs(this->getNodeChild(node, i), depth+1);
510  }
511  }
512  }
513  node->updateOccupancyChildren();
514  }
515  }
516 
517  template <class NODE>
519  if (this->root == NULL)
520  return;
521 
522  // convert bottom up
523  for (unsigned int depth=this->tree_depth; depth>0; depth--) {
524  toMaxLikelihoodRecurs(this->root, 0, depth);
525  }
526 
527  // convert root
528  nodeToMaxLikelihood(this->root);
529  }
530 
531  template <class NODE>
532  void OccupancyOcTreeBase<NODE>::toMaxLikelihoodRecurs(NODE* node, unsigned int depth,
533  unsigned int max_depth) {
534 
535  assert(node);
536 
537  if (depth < max_depth) {
538  for (unsigned int i=0; i<8; i++) {
539  if (this->nodeChildExists(node, i)) {
540  toMaxLikelihoodRecurs(this->getNodeChild(node, i), depth+1, max_depth);
541  }
542  }
543  }
544 
545  else { // max level reached
546  nodeToMaxLikelihood(node);
547  }
548  }
549 
550  template <class NODE>
551  bool OccupancyOcTreeBase<NODE>::getNormals(const point3d& point, std::vector<point3d>& normals,
552  bool unknownStatus) const {
553  normals.clear();
554 
555  OcTreeKey init_key;
557  OCTOMAP_WARNING_STR("Voxel out of bounds");
558  return false;
559  }
560 
561  // OCTOMAP_WARNING("Normal for %f, %f, %f\n", point.x(), point.y(), point.z());
562 
563  int vertex_values[8];
564 
565  OcTreeKey current_key;
566  NODE* current_node;
567 
568  // There is 8 neighbouring sets
569  // The current cube can be at any of the 8 vertex
570  int x_index[4][4] = {{1, 1, 0, 0}, {1, 1, 0, 0}, {0, 0 -1, -1}, {0, 0 -1, -1}};
571  int y_index[4][4] = {{1, 0, 0, 1}, {0, -1, -1, 0}, {0, -1, -1, 0}, {1, 0, 0, 1}};
572  int z_index[2][2] = {{0, 1}, {-1, 0}};
573 
574  // Iterate over the 8 neighboring sets
575  for(int m = 0; m < 2; ++m){
576  for(int l = 0; l < 4; ++l){
577 
578  int k = 0;
579  // Iterate over the cubes
580  for(int j = 0; j < 2; ++j){
581  for(int i = 0; i < 4; ++i){
582  current_key[0] = init_key[0] + x_index[l][i];
583  current_key[1] = init_key[1] + y_index[l][i];
584  current_key[2] = init_key[2] + z_index[m][j];
585  current_node = this->search(current_key);
586 
587  if(current_node){
588  vertex_values[k] = this->isNodeOccupied(current_node);
589 
590  // point3d coord = this->keyToCoord(current_key);
591  // OCTOMAP_WARNING_STR("vertex " << k << " at " << coord << "; value " << vertex_values[k]);
592  }else{
593  // Occupancy of unknown cells
594  vertex_values[k] = unknownStatus;
595  }
596  ++k;
597  }
598  }
599 
600  int cube_index = 0;
601  if (vertex_values[0]) cube_index |= 1;
602  if (vertex_values[1]) cube_index |= 2;
603  if (vertex_values[2]) cube_index |= 4;
604  if (vertex_values[3]) cube_index |= 8;
605  if (vertex_values[4]) cube_index |= 16;
606  if (vertex_values[5]) cube_index |= 32;
607  if (vertex_values[6]) cube_index |= 64;
608  if (vertex_values[7]) cube_index |= 128;
609 
610  // OCTOMAP_WARNING_STR("cubde_index: " << cube_index);
611 
612  // All vertices are occupied or free resulting in no normal
613  if (edgeTable[cube_index] == 0)
614  return true;
615 
616  // No interpolation is done yet, we use vertexList in <MCTables.h>.
617  for(int i = 0; triTable[cube_index][i] != -1; i += 3){
618  point3d p1 = vertexList[triTable[cube_index][i ]];
619  point3d p2 = vertexList[triTable[cube_index][i+1]];
620  point3d p3 = vertexList[triTable[cube_index][i+2]];
621  point3d v1 = p2 - p1;
622  point3d v2 = p3 - p1;
623 
624  // OCTOMAP_WARNING("Vertex p1 %f, %f, %f\n", p1.x(), p1.y(), p1.z());
625  // OCTOMAP_WARNING("Vertex p2 %f, %f, %f\n", p2.x(), p2.y(), p2.z());
626  // OCTOMAP_WARNING("Vertex p3 %f, %f, %f\n", p3.x(), p3.y(), p3.z());
627 
628  // Right hand side cross product to retrieve the normal in the good
629  // direction (pointing to the free nodes).
630  normals.push_back(v1.cross(v2).normalize());
631  }
632  }
633  }
634 
635  return true;
636  }
637 
638  template <class NODE>
639  bool OccupancyOcTreeBase<NODE>::castRay(const point3d& origin, const point3d& directionP, point3d& end,
640  bool ignoreUnknown, double maxRange) const {
641 
643 
644  // Initialization phase -------------------------------------------------------
645  OcTreeKey current_key;
647  OCTOMAP_WARNING_STR("Coordinates out of bounds during ray casting");
648  return false;
649  }
650 
651  NODE* startingNode = this->search(current_key);
652  if (startingNode){
653  if (this->isNodeOccupied(startingNode)){
654  // Occupied node found at origin
655  // (need to convert from key, since origin does not need to be a voxel center)
656  end = this->keyToCoord(current_key);
657  return true;
658  }
659  } else if(!ignoreUnknown){
660  end = this->keyToCoord(current_key);
661  return false;
662  }
663 
664  point3d direction = directionP.normalized();
665  bool max_range_set = (maxRange > 0.0);
666 
667  int step[3];
668  double tMax[3];
669  double tDelta[3];
670 
671  for(unsigned int i=0; i < 3; ++i) {
672  // compute step direction
673  if (direction(i) > 0.0) step[i] = 1;
674  else if (direction(i) < 0.0) step[i] = -1;
675  else step[i] = 0;
676 
677  // compute tMax, tDelta
678  if (step[i] != 0) {
679  // corner point of voxel (in direction of ray)
680  double voxelBorder = this->keyToCoord(current_key[i]);
681  voxelBorder += double(step[i] * this->resolution * 0.5);
682 
683  tMax[i] = ( voxelBorder - origin(i) ) / direction(i);
684  tDelta[i] = this->resolution / fabs( direction(i) );
685  }
686  else {
687  tMax[i] = std::numeric_limits<double>::max();
688  tDelta[i] = std::numeric_limits<double>::max();
689  }
690  }
691 
692  if (step[0] == 0 && step[1] == 0 && step[2] == 0){
693  OCTOMAP_ERROR("Raycasting in direction (0,0,0) is not possible!");
694  return false;
695  }
696 
697  // for speedup:
698  double maxrange_sq = maxRange *maxRange;
699 
700  // Incremental phase ---------------------------------------------------------
701 
702  bool done = false;
703 
704  while (!done) {
705  unsigned int dim;
706 
707  // find minimum tMax:
708  if (tMax[0] < tMax[1]){
709  if (tMax[0] < tMax[2]) dim = 0;
710  else dim = 2;
711  }
712  else {
713  if (tMax[1] < tMax[2]) dim = 1;
714  else dim = 2;
715  }
716 
717  // check for overflow:
718  if ((step[dim] < 0 && current_key[dim] == 0)
719  || (step[dim] > 0 && current_key[dim] == 2* this->tree_max_val-1))
720  {
721  OCTOMAP_WARNING("Coordinate hit bounds in dim %d, aborting raycast\n", dim);
722  // return border point nevertheless:
723  end = this->keyToCoord(current_key);
724  return false;
725  }
726 
727  // advance in direction "dim"
728  current_key[dim] += step[dim];
729  tMax[dim] += tDelta[dim];
730 
731 
732  // generate world coords from key
733  end = this->keyToCoord(current_key);
734 
735  // check for maxrange:
736  if (max_range_set){
737  double dist_from_origin_sq(0.0);
738  for (unsigned int j = 0; j < 3; j++) {
739  dist_from_origin_sq += ((end(j) - origin(j)) * (end(j) - origin(j)));
740  }
741  if (dist_from_origin_sq > maxrange_sq)
742  return false;
743 
744  }
745 
746  NODE* currentNode = this->search(current_key);
747  if (currentNode){
748  if (this->isNodeOccupied(currentNode)) {
749  done = true;
750  break;
751  }
752  // otherwise: node is free and valid, raycasting continues
753  } else if (!ignoreUnknown){ // no node found, this usually means we are in "unknown" areas
754  return false;
755  }
756  } // end while
757 
758  return true;
759  }
760 
761  template <class NODE>
762  bool OccupancyOcTreeBase<NODE>::getRayIntersection (const point3d& origin, const point3d& direction, const point3d& center,
763  point3d& intersection, double delta/*=0.0*/) const {
764  // We only need three normals for the six planes
765  octomap::point3d normalX(1, 0, 0);
766  octomap::point3d normalY(0, 1, 0);
767  octomap::point3d normalZ(0, 0, 1);
768 
769  // One point on each plane, let them be the center for simplicity
770  octomap::point3d pointXNeg(center(0) - float(this->resolution / 2.0), center(1), center(2));
771  octomap::point3d pointXPos(center(0) + float(this->resolution / 2.0), center(1), center(2));
772  octomap::point3d pointYNeg(center(0), center(1) - float(this->resolution / 2.0), center(2));
773  octomap::point3d pointYPos(center(0), center(1) + float(this->resolution / 2.0), center(2));
774  octomap::point3d pointZNeg(center(0), center(1), center(2) - float(this->resolution / 2.0));
775  octomap::point3d pointZPos(center(0), center(1), center(2) + float(this->resolution / 2.0));
776 
777  double lineDotNormal = 0.0;
778  double d = 0.0;
779  double outD = std::numeric_limits<double>::max();
780  octomap::point3d intersect;
781  bool found = false;
782 
783  // Find the intersection (if any) with each place
784  // Line dot normal will be zero if they are parallel, in which case no intersection can be the entry one
785  // if there is an intersection does it occur in the bounded plane of the voxel
786  // if yes keep only the closest (smallest distance to sensor origin).
787  if((lineDotNormal = normalX.dot(direction)) != 0.0){ // Ensure lineDotNormal is non-zero (assign and test)
788  d = (pointXNeg - origin).dot(normalX) / lineDotNormal;
789  intersect = direction * float(d) + origin;
790  if(!(intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6) ||
791  intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
792  outD = std::min(outD, d);
793  found = true;
794  }
795 
796  d = (pointXPos - origin).dot(normalX) / lineDotNormal;
797  intersect = direction * float(d) + origin;
798  if(!(intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6) ||
799  intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
800  outD = std::min(outD, d);
801  found = true;
802  }
803  }
804 
805  if((lineDotNormal = normalY.dot(direction)) != 0.0){ // Ensure lineDotNormal is non-zero (assign and test)
806  d = (pointYNeg - origin).dot(normalY) / lineDotNormal;
807  intersect = direction * float(d) + origin;
808  if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
809  intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
810  outD = std::min(outD, d);
811  found = true;
812  }
813 
814  d = (pointYPos - origin).dot(normalY) / lineDotNormal;
815  intersect = direction * float(d) + origin;
816  if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
817  intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
818  outD = std::min(outD, d);
819  found = true;
820  }
821  }
822 
823  if((lineDotNormal = normalZ.dot(direction)) != 0.0){ // Ensure lineDotNormal is non-zero (assign and test)
824  d = (pointZNeg - origin).dot(normalZ) / lineDotNormal;
825  intersect = direction * float(d) + origin;
826  if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
827  intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6))){
828  outD = std::min(outD, d);
829  found = true;
830  }
831 
832  d = (pointZPos - origin).dot(normalZ) / lineDotNormal;
833  intersect = direction * float(d) + origin;
834  if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
835  intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6))){
836  outD = std::min(outD, d);
837  found = true;
838  }
839  }
840 
841  // Substract (add) a fraction to ensure no ambiguity on the starting voxel
842  // Don't start on a boundary.
843  if(found)
844  intersection = direction * float(outD + delta) + origin;
845 
846  return found;
847  }
848 
849 
850  template <class NODE> inline bool
851  OccupancyOcTreeBase<NODE>::integrateMissOnRay(const point3d& origin, const point3d& end, bool lazy_eval) {
852 
853  if (!this->computeRayKeys(origin, end, this->keyrays.at(0))) {
854  return false;
855  }
856 
857  for(KeyRay::iterator it=this->keyrays[0].begin(); it != this->keyrays[0].end(); it++) {
858  updateNode(*it, false, lazy_eval); // insert freespace measurement
859  }
860 
861  return true;
862  }
863 
864  template <class NODE> bool
865  OccupancyOcTreeBase<NODE>::insertRay(const point3d& origin, const point3d& end, double maxrange, bool lazy_eval)
866  {
867  // cut ray at maxrange
868  if ((maxrange > 0) && ((end - origin).norm () > maxrange))
869  {
870  point3d direction = (end - origin).normalized ();
871  point3d new_end = origin + direction * (float) maxrange;
872  return integrateMissOnRay(origin, new_end,lazy_eval);
873  }
874  // insert complete ray
875  else
876  {
877  if (!integrateMissOnRay(origin, end,lazy_eval))
878  return false;
879  updateNode(end, true, lazy_eval); // insert hit cell
880  return true;
881  }
882  }
883 
884  template <class NODE>
886  bbx_min = min;
887  if (!this->coordToKeyChecked(bbx_min, bbx_min_key)) {
888  OCTOMAP_ERROR("ERROR while generating bbx min key.\n");
889  }
890  }
891 
892  template <class NODE>
894  bbx_max = max;
895  if (!this->coordToKeyChecked(bbx_max, bbx_max_key)) {
896  OCTOMAP_ERROR("ERROR while generating bbx max key.\n");
897  }
898  }
899 
900 
901  template <class NODE>
903  return ((p.x() >= bbx_min.x()) && (p.y() >= bbx_min.y()) && (p.z() >= bbx_min.z()) &&
904  (p.x() <= bbx_max.x()) && (p.y() <= bbx_max.y()) && (p.z() <= bbx_max.z()) );
905  }
906 
907 
908  template <class NODE>
910  return ((key[0] >= bbx_min_key[0]) && (key[1] >= bbx_min_key[1]) && (key[2] >= bbx_min_key[2]) &&
911  (key[0] <= bbx_max_key[0]) && (key[1] <= bbx_max_key[1]) && (key[2] <= bbx_max_key[2]) );
912  }
913 
914  template <class NODE>
916  octomap::point3d obj_bounds = (bbx_max - bbx_min);
917  obj_bounds /= 2.;
918  return obj_bounds;
919  }
920 
921  template <class NODE>
923  octomap::point3d obj_bounds = (bbx_max - bbx_min);
924  obj_bounds /= 2.;
925  return bbx_min + obj_bounds;
926  }
927 
928  // -- I/O -----------------------------------------
929 
930  template <class NODE>
931  std::istream& OccupancyOcTreeBase<NODE>::readBinaryData(std::istream &s){
932  // tree needs to be newly created or cleared externally
933  if (this->root) {
934  OCTOMAP_ERROR_STR("Trying to read into an existing tree.");
935  return s;
936  }
937 
938  this->root = new NODE();
939  this->readBinaryNode(s, this->root);
940  this->size_changed = true;
941  this->tree_size = OcTreeBaseImpl<NODE,AbstractOccupancyOcTree>::calcNumNodes(); // compute number of nodes
942  return s;
943  }
944 
945  template <class NODE>
946  std::ostream& OccupancyOcTreeBase<NODE>::writeBinaryData(std::ostream &s) const{
947  OCTOMAP_DEBUG("Writing %zu nodes to output stream...", this->size());
948  if (this->root)
949  this->writeBinaryNode(s, this->root);
950  return s;
951  }
952 
953  template <class NODE>
954  std::istream& OccupancyOcTreeBase<NODE>::readBinaryNode(std::istream &s, NODE* node){
955 
956  assert(node);
957 
958  char child1to4_char;
959  char child5to8_char;
960  s.read((char*)&child1to4_char, sizeof(char));
961  s.read((char*)&child5to8_char, sizeof(char));
962 
963  std::bitset<8> child1to4 ((unsigned long long) child1to4_char);
964  std::bitset<8> child5to8 ((unsigned long long) child5to8_char);
965 
966  // std::cout << "read: "
967  // << child1to4.to_string<char,std::char_traits<char>,std::allocator<char> >() << " "
968  // << child5to8.to_string<char,std::char_traits<char>,std::allocator<char> >() << std::endl;
969 
970 
971  // inner nodes default to occupied
972  node->setLogOdds(this->clamping_thres_max);
973 
974  for (unsigned int i=0; i<4; i++) {
975  if ((child1to4[i*2] == 1) && (child1to4[i*2+1] == 0)) {
976  // child is free leaf
977  this->createNodeChild(node, i);
978  this->getNodeChild(node, i)->setLogOdds(this->clamping_thres_min);
979  }
980  else if ((child1to4[i*2] == 0) && (child1to4[i*2+1] == 1)) {
981  // child is occupied leaf
982  this->createNodeChild(node, i);
983  this->getNodeChild(node, i)->setLogOdds(this->clamping_thres_max);
984  }
985  else if ((child1to4[i*2] == 1) && (child1to4[i*2+1] == 1)) {
986  // child has children
987  this->createNodeChild(node, i);
988  this->getNodeChild(node, i)->setLogOdds(-200.); // child is unkown, we leave it uninitialized
989  }
990  }
991  for (unsigned int i=0; i<4; i++) {
992  if ((child5to8[i*2] == 1) && (child5to8[i*2+1] == 0)) {
993  // child is free leaf
994  this->createNodeChild(node, i+4);
995  this->getNodeChild(node, i+4)->setLogOdds(this->clamping_thres_min);
996  }
997  else if ((child5to8[i*2] == 0) && (child5to8[i*2+1] == 1)) {
998  // child is occupied leaf
999  this->createNodeChild(node, i+4);
1000  this->getNodeChild(node, i+4)->setLogOdds(this->clamping_thres_max);
1001  }
1002  else if ((child5to8[i*2] == 1) && (child5to8[i*2+1] == 1)) {
1003  // child has children
1004  this->createNodeChild(node, i+4);
1005  this->getNodeChild(node, i+4)->setLogOdds(-200.); // set occupancy when all children have been read
1006  }
1007  // child is unkown, we leave it uninitialized
1008  }
1009 
1010  // read children's children and set the label
1011  for (unsigned int i=0; i<8; i++) {
1012  if (this->nodeChildExists(node, i)) {
1013  NODE* child = this->getNodeChild(node, i);
1014  if (fabs(child->getLogOdds() + 200.)<1e-3) {
1015  readBinaryNode(s, child);
1016  child->setLogOdds(child->getMaxChildLogOdds());
1017  }
1018  } // end if child exists
1019  } // end for children
1020 
1021  return s;
1022  }
1023 
1024  template <class NODE>
1025  std::ostream& OccupancyOcTreeBase<NODE>::writeBinaryNode(std::ostream &s, const NODE* node) const{
1026 
1027  assert(node);
1028 
1029  // 2 bits for each children, 8 children per node -> 16 bits
1030  std::bitset<8> child1to4;
1031  std::bitset<8> child5to8;
1032 
1033  // 10 : child is free node
1034  // 01 : child is occupied node
1035  // 00 : child is unkown node
1036  // 11 : child has children
1037 
1038 
1039  // speedup: only set bits to 1, rest is init with 0 anyway,
1040  // can be one logic expression per bit
1041 
1042  for (unsigned int i=0; i<4; i++) {
1043  if (this->nodeChildExists(node, i)) {
1044  const NODE* child = this->getNodeChild(node, i);
1045  if (this->nodeHasChildren(child)) { child1to4[i*2] = 1; child1to4[i*2+1] = 1; }
1046  else if (this->isNodeOccupied(child)) { child1to4[i*2] = 0; child1to4[i*2+1] = 1; }
1047  else { child1to4[i*2] = 1; child1to4[i*2+1] = 0; }
1048  }
1049  else {
1050  child1to4[i*2] = 0; child1to4[i*2+1] = 0;
1051  }
1052  }
1053 
1054  for (unsigned int i=0; i<4; i++) {
1055  if (this->nodeChildExists(node, i+4)) {
1056  const NODE* child = this->getNodeChild(node, i+4);
1057  if (this->nodeHasChildren(child)) { child5to8[i*2] = 1; child5to8[i*2+1] = 1; }
1058  else if (this->isNodeOccupied(child)) { child5to8[i*2] = 0; child5to8[i*2+1] = 1; }
1059  else { child5to8[i*2] = 1; child5to8[i*2+1] = 0; }
1060  }
1061  else {
1062  child5to8[i*2] = 0; child5to8[i*2+1] = 0;
1063  }
1064  }
1065  // std::cout << "wrote: "
1066  // << child1to4.to_string<char,std::char_traits<char>,std::allocator<char> >() << " "
1067  // << child5to8.to_string<char,std::char_traits<char>,std::allocator<char> >() << std::endl;
1068 
1069  char child1to4_char = (char) child1to4.to_ulong();
1070  char child5to8_char = (char) child5to8.to_ulong();
1071 
1072  s.write((char*)&child1to4_char, sizeof(char));
1073  s.write((char*)&child5to8_char, sizeof(char));
1074 
1075  // write children's children
1076  for (unsigned int i=0; i<8; i++) {
1077  if (this->nodeChildExists(node, i)) {
1078  const NODE* child = this->getNodeChild(node, i);
1079  if (this->nodeHasChildren(child)) {
1080  writeBinaryNode(s, child);
1081  }
1082  }
1083  }
1084 
1085  return s;
1086  }
1087 
1088  //-- Occupancy queries on nodes:
1089 
1090  template <class NODE>
1091  void OccupancyOcTreeBase<NODE>::updateNodeLogOdds(NODE* occupancyNode, const float& update) const {
1092  occupancyNode->addValue(update);
1093  if (occupancyNode->getLogOdds() < this->clamping_thres_min) {
1094  occupancyNode->setLogOdds(this->clamping_thres_min);
1095  return;
1096  }
1097  if (occupancyNode->getLogOdds() > this->clamping_thres_max) {
1098  occupancyNode->setLogOdds(this->clamping_thres_max);
1099  }
1100  }
1101 
1102  template <class NODE>
1103  void OccupancyOcTreeBase<NODE>::integrateHit(NODE* occupancyNode) const {
1104  updateNodeLogOdds(occupancyNode, this->prob_hit_log);
1105  }
1106 
1107  template <class NODE>
1108  void OccupancyOcTreeBase<NODE>::integrateMiss(NODE* occupancyNode) const {
1109  updateNodeLogOdds(occupancyNode, this->prob_miss_log);
1110  }
1111 
1112  template <class NODE>
1113  void OccupancyOcTreeBase<NODE>::nodeToMaxLikelihood(NODE* occupancyNode) const{
1114  if (this->isNodeOccupied(occupancyNode))
1115  occupancyNode->setLogOdds(this->clamping_thres_max);
1116  else
1117  occupancyNode->setLogOdds(this->clamping_thres_min);
1118  }
1119 
1120  template <class NODE>
1121  void OccupancyOcTreeBase<NODE>::nodeToMaxLikelihood(NODE& occupancyNode) const{
1122  if (this->isNodeOccupied(occupancyNode))
1123  occupancyNode.setLogOdds(this->clamping_thres_max);
1124  else
1125  occupancyNode.setLogOdds(this->clamping_thres_min);
1126  }
1127 
1128 } // namespace
octomap::OcTreeBaseImpl
OcTree base class, to be used with with any kind of OcTreeDataNode.
Definition: OcTreeBaseImpl.h:75
octomap::triTable
static const int triTable[256][16]
Definition: MCTables.h:78
octomap::OccupancyOcTreeBase
Base implementation for Occupancy Octrees (e.g.
Definition: OccupancyOcTreeBase.h:69
OCTOMAP_WARNING_STR
#define OCTOMAP_WARNING_STR(args)
Definition: octomap_types.h:77
octomap::ScanNode::pose
pose6d pose
6D pose from which the scan was performed
Definition: ScanGraph.h:74
octomap::KeyRay::iterator
std::vector< OcTreeKey >::iterator iterator
Definition: OcTreeKey.h:166
octomap::Pointcloud
A collection of 3D coordinates (point3d), which are regarded as endpoints of a 3D laser scan.
Definition: Pointcloud.h:47
octomap::KeyRay::reverse_iterator
std::vector< OcTreeKey >::reverse_iterator reverse_iterator
Definition: OcTreeKey.h:168
octomap::Pointcloud::push_back
void push_back(float x, float y, float z)
Definition: Pointcloud.h:61
octomap::Pointcloud::reserve
void reserve(size_t size)
Definition: Pointcloud.h:59
octomap::AbstractOccupancyOcTree::clamping_thres_max
float clamping_thres_max
Definition: AbstractOccupancyOcTree.h:230
octomath::Vector3::normalize
Vector3 & normalize()
normalizes this vector, so that it has norm=1.0
Definition: Vector3.h:270
octomap::KeyRay
Definition: OcTreeKey.h:139
OCTOMAP_DEBUG
#define OCTOMAP_DEBUG(...)
Definition: octomap_types.h:72
octomap::OccupancyOcTreeBase::OccupancyOcTreeBase
OccupancyOcTreeBase(double resolution)
Default constructor, sets resolution of leafs.
Definition: OccupancyOcTreeBase.hxx:42
octomath::Vector3
This class represents a three-dimensional vector.
Definition: Vector3.h:50
octomath::Vector3::dot
double dot(const Vector3 &other) const
dot product
Definition: Vector3.h:117
octomap::vertexList
static const point3d vertexList[12]
Definition: MCTables.h:336
octomap::KeySet
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
octomap::edgeTable
static const int edgeTable[256]
Definition: MCTables.h:44
octomap::ScanNode
A 3D scan as Pointcloud, performed from a Pose6D.
Definition: ScanGraph.h:52
octomath::Pose6D::transform
Vector3 transform(const Vector3 &v) const
Transformation of a vector.
Definition: Pose6D.cpp:85
OCTOMAP_ERROR
#define OCTOMAP_ERROR(...)
Definition: octomap_types.h:78
octomap::AbstractOccupancyOcTree::occ_prob_thres_log
float occ_prob_thres_log
Definition: AbstractOccupancyOcTree.h:233
octomap::KeyRay::end
iterator end()
Definition: OcTreeKey.h:171
octomap::OcTreeKey
OcTreeKey is a container class for internal key addressing.
Definition: OcTreeKey.h:70
octomap::Pointcloud::transform
void transform(pose6d transform)
Apply transform to each point.
Definition: Pointcloud.cpp:102
octomap::KeyRay::rbegin
reverse_iterator rbegin()
Definition: OcTreeKey.h:175
octomap::computeChildIdx
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
octomap::KeyRay::rend
reverse_iterator rend()
Definition: OcTreeKey.h:176
OCTOMAP_ERROR_STR
#define OCTOMAP_ERROR_STR(args)
Definition: octomap_types.h:79
octomath::Vector3::normalized
Vector3 normalized() const
Definition: Vector3.h:278
octomath::Vector3::cross
Vector3 cross(const Vector3 &other) const
Three-dimensional vector (cross) product.
Definition: Vector3.h:107
OCTOMAP_WARNING
#define OCTOMAP_WARNING(...)
Definition: octomap_types.h:76
octomath::Vector3::y
float & y()
Definition: Vector3.h:136
octomap::AbstractOccupancyOcTree
Interface class for all octree types that store occupancy.
Definition: AbstractOccupancyOcTree.h:52
octomap::AbstractOccupancyOcTree::prob_hit_log
float prob_hit_log
Definition: AbstractOccupancyOcTree.h:231
octomap::AbstractOccupancyOcTree::clamping_thres_min
float clamping_thres_min
Definition: AbstractOccupancyOcTree.h:229
octomap
Tables used by the Marching Cubes Algorithm The tables are from Paul Bourke's web page http://paulbou...
octomap::KeyRay::begin
iterator begin()
Definition: OcTreeKey.h:170
octomath::Pose6D
This class represents a tree-dimensional pose of an object.
Definition: Pose6D.h:49
octomap::ScanNode::scan
Pointcloud * scan
Definition: ScanGraph.h:73
octomath::Vector3::z
float & z()
Definition: Vector3.h:141
octomath::Vector3::x
float & x()
Definition: Vector3.h:131
octomap::Pointcloud::size
size_t size() const
Definition: Pointcloud.h:57
octomath::Pose6D::inv
Pose6D inv() const
Inversion.
Definition: Pose6D.cpp:70
MCTables.h
octomap::AbstractOccupancyOcTree::prob_miss_log
float prob_miss_log
Definition: AbstractOccupancyOcTree.h:232
octomath::Pose6D::trans
Vector3 & trans()
Translational component.
Definition: Pose6D.h:82