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)
81 insertPointCloud(cloud, sensor_origin, frame_origin, maxrange, lazy_eval, discretize);
87 double maxrange,
bool lazy_eval,
bool discretize) {
89 KeySet free_cells, occupied_cells;
91 computeDiscreteUpdate(scan, sensor_origin, free_cells, occupied_cells, maxrange);
93 computeUpdate(scan, sensor_origin, free_cells, occupied_cells, maxrange);
96 for (KeySet::iterator it = free_cells.begin(); it != free_cells.end(); ++it) {
97 updateNode(*it,
false, lazy_eval);
99 for (KeySet::iterator it = occupied_cells.begin(); it != occupied_cells.end(); ++it) {
100 updateNode(*it,
true, lazy_eval);
104 template <
class NODE>
106 double maxrange,
bool lazy_eval,
bool discretize) {
109 transformed_scan.
transform(frame_origin);
111 insertPointCloud(transformed_scan, transformed_sensor_origin, maxrange, lazy_eval, discretize);
115 template <
class NODE>
121 omp_set_num_threads(this->keyrays.size());
122 #pragma omp parallel for
124 for (
int i = 0; i < (int)pc.
size(); ++i) {
126 unsigned threadIdx = 0;
128 threadIdx = omp_get_thread_num();
130 KeyRay* keyray = &(this->keyrays.at(threadIdx));
132 if (this->computeRayKeys(origin, p, *keyray)){
138 updateNode(*it,
false, lazy_eval);
140 updateNode(p,
true, lazy_eval);
147 template <
class NODE>
156 for (
int i = 0; i < (int)scan.
size(); ++i) {
158 std::pair<KeySet::iterator,bool> ret = endpoints.insert(k);
160 discretePC.
push_back(this->keyToCoord(k));
164 computeUpdate(discretePC, origin, free_cells, occupied_cells, maxrange);
168 template <
class NODE>
177 omp_set_num_threads(this->keyrays.size());
178 #pragma omp parallel for schedule(guided)
180 for (
int i = 0; i < (int)scan.
size(); ++i) {
182 unsigned threadIdx = 0;
184 threadIdx = omp_get_thread_num();
186 KeyRay* keyray = &(this->keyrays.at(threadIdx));
189 if (!use_bbx_limit) {
190 if ((maxrange < 0.0) || ((p - origin).norm() <= maxrange) ) {
192 if (this->computeRayKeys(origin, p, *keyray)){
194 #pragma omp critical (free_insert)
197 free_cells.insert(keyray->
begin(), keyray->
end());
202 if (this->coordToKeyChecked(p, key)){
204 #pragma omp critical (occupied_insert)
207 occupied_cells.insert(key);
211 point3d direction = (p - origin).normalized ();
212 point3d new_end = origin + direction * (float) maxrange;
213 if (this->computeRayKeys(origin, new_end, *keyray)){
215 #pragma omp critical (free_insert)
218 free_cells.insert(keyray->
begin(), keyray->
end());
224 if ( inBBX(p) && ((maxrange < 0.0) || ((p - origin).norm () <= maxrange) ) ) {
228 if (this->coordToKeyChecked(p, key)){
230 #pragma omp critical (occupied_insert)
233 occupied_cells.insert(key);
240 if ((maxrange >= 0.0) && ((p - origin).norm() > maxrange)) {
241 const point3d direction = (p - origin).normalized();
242 new_end = origin + direction * (float) maxrange;
246 if (this->computeRayKeys(origin, new_end, *keyray)){
250 #pragma omp critical (free_insert)
253 free_cells.insert(*it);
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);
272 template <
class NODE>
275 log_odds_value = std::min(std::max(log_odds_value, this->clamping_thres_min), this->clamping_thres_max);
277 bool createdRoot =
false;
278 if (this->root == NULL){
279 this->root =
new NODE();
284 return setNodeValueRecurs(this->root, createdRoot, key, 0, log_odds_value, lazy_eval);
287 template <
class NODE>
290 if (!this->coordToKeyChecked(value, key))
293 return setNodeValue(key, log_odds_value, lazy_eval);
296 template <
class NODE>
299 if (!this->coordToKeyChecked(x, y, z, key))
302 return setNodeValue(key, log_odds_value, lazy_eval);
306 template <
class NODE>
310 NODE* leaf = this->search(key);
313 && ((log_odds_update >= 0 && leaf->getLogOdds() >= this->clamping_thres_max)
314 || ( log_odds_update <= 0 && leaf->getLogOdds() <= this->clamping_thres_min)))
319 bool createdRoot =
false;
320 if (this->root == NULL){
321 this->root =
new NODE();
326 return updateNodeRecurs(this->root, createdRoot, key, 0, log_odds_update, lazy_eval);
329 template <
class NODE>
332 if (!this->coordToKeyChecked(value, key))
335 return updateNode(key, log_odds_update, lazy_eval);
338 template <
class NODE>
341 if (!this->coordToKeyChecked(x, y, z, key))
344 return updateNode(key, log_odds_update, lazy_eval);
347 template <
class NODE>
349 float logOdds = this->prob_miss_log;
351 logOdds = this->prob_hit_log;
353 return updateNode(key, logOdds, lazy_eval);
356 template <
class NODE>
359 if (!this->coordToKeyChecked(value, key))
361 return updateNode(key, occupied, lazy_eval);
364 template <
class NODE>
367 if (!this->coordToKeyChecked(x, y, z, key))
369 return updateNode(key, occupied, lazy_eval);
372 template <
class NODE>
374 unsigned int depth,
const float& log_odds_update,
bool lazy_eval) {
375 bool created_node =
false;
380 if (depth < this->tree_depth) {
382 if (!this->nodeChildExists(node, pos)) {
384 if (!this->nodeHasChildren(node) && !node_just_created ) {
387 this->expandNode(node);
391 this->createNodeChild(node, pos);
397 return updateNodeRecurs(this->getNodeChild(node, pos), created_node, key, depth+1, log_odds_update, lazy_eval);
399 NODE* retval = updateNodeRecurs(this->getNodeChild(node, pos), created_node, key, depth+1, log_odds_update, lazy_eval);
402 if (this->pruneNode(node)){
406 node->updateOccupancyChildren();
415 if (use_change_detection) {
416 bool occBefore = this->isNodeOccupied(node);
417 updateNodeLogOdds(node, log_odds_update);
419 if (node_just_created){
420 changed_keys.insert(std::pair<OcTreeKey,bool>(key,
true));
421 }
else if (occBefore != this->isNodeOccupied(node)) {
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);
429 updateNodeLogOdds(node, log_odds_update);
436 template <
class NODE>
438 unsigned int depth,
const float& log_odds_value,
bool lazy_eval) {
439 bool created_node =
false;
444 if (depth < this->tree_depth) {
446 if (!this->nodeChildExists(node, pos)) {
448 if (!this->nodeHasChildren(node) && !node_just_created ) {
451 this->expandNode(node);
455 this->createNodeChild(node, pos);
461 return setNodeValueRecurs(this->getNodeChild(node, pos), created_node, key, depth+1, log_odds_value, lazy_eval);
463 NODE* retval = setNodeValueRecurs(this->getNodeChild(node, pos), created_node, key, depth+1, log_odds_value, lazy_eval);
466 if (this->pruneNode(node)){
470 node->updateOccupancyChildren();
479 if (use_change_detection) {
480 bool occBefore = this->isNodeOccupied(node);
481 node->setLogOdds(log_odds_value);
483 if (node_just_created){
484 changed_keys.insert(std::pair<OcTreeKey,bool>(key,
true));
485 }
else if (occBefore != this->isNodeOccupied(node)) {
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);
493 node->setLogOdds(log_odds_value);
499 template <
class NODE>
502 this->updateInnerOccupancyRecurs(this->root, 0);
505 template <
class NODE>
510 if (this->nodeHasChildren(node)){
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);
519 node->updateOccupancyChildren();
523 template <
class NODE>
525 if (this->root == NULL)
529 for (
unsigned int depth=this->tree_depth; depth>0; depth--) {
530 toMaxLikelihoodRecurs(this->root, 0, depth);
534 nodeToMaxLikelihood(this->root);
537 template <
class NODE>
539 unsigned int max_depth) {
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);
552 nodeToMaxLikelihood(node);
556 template <
class NODE>
558 bool unknownStatus)
const {
569 int vertex_values[8];
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}};
581 for(
int m = 0; m < 2; ++m){
582 for(
int l = 0; l < 4; ++l){
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);
594 vertex_values[k] = this->isNodeOccupied(current_node);
600 vertex_values[k] = unknownStatus;
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;
623 for(
int i = 0;
triTable[cube_index][i] != -1; i += 3){
644 template <
class NODE>
646 bool ignoreUnknown,
double maxRange)
const {
657 NODE* startingNode = this->search(current_key);
659 if (this->isNodeOccupied(startingNode)){
662 end = this->keyToCoord(current_key);
665 }
else if(!ignoreUnknown){
666 end = this->keyToCoord(current_key);
671 bool max_range_set = (maxRange > 0.0);
677 for(
unsigned int i=0; i < 3; ++i) {
679 if (direction(i) > 0.0) step[i] = 1;
680 else if (direction(i) < 0.0) step[i] = -1;
686 double voxelBorder = this->keyToCoord(current_key[i]);
687 voxelBorder += double(step[i] * this->resolution * 0.5);
689 tMax[i] = ( voxelBorder - origin(i) ) / direction(i);
690 tDelta[i] = this->resolution / fabs( direction(i) );
693 tMax[i] = std::numeric_limits<double>::max();
694 tDelta[i] = std::numeric_limits<double>::max();
698 if (step[0] == 0 && step[1] == 0 && step[2] == 0){
699 OCTOMAP_ERROR(
"Raycasting in direction (0,0,0) is not possible!");
704 double maxrange_sq = maxRange *maxRange;
714 if (tMax[0] < tMax[1]){
715 if (tMax[0] < tMax[2]) dim = 0;
719 if (tMax[1] < tMax[2]) dim = 1;
724 if ((step[dim] < 0 && current_key[dim] == 0)
725 || (step[dim] > 0 && current_key[dim] == 2* this->tree_max_val-1))
727 OCTOMAP_WARNING(
"Coordinate hit bounds in dim %d, aborting raycast\n", dim);
729 end = this->keyToCoord(current_key);
734 current_key[dim] += step[dim];
735 tMax[dim] += tDelta[dim];
739 end = this->keyToCoord(current_key);
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)));
747 if (dist_from_origin_sq > maxrange_sq)
752 NODE* currentNode = this->search(current_key);
754 if (this->isNodeOccupied(currentNode)) {
759 }
else if (!ignoreUnknown){
767 template <
class NODE>
769 point3d& intersection,
double delta)
const {
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));
783 double lineDotNormal = 0.0;
785 double outD = std::numeric_limits<double>::max();
793 if((lineDotNormal = normalX.
dot(direction)) != 0.0){
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);
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);
811 if((lineDotNormal = normalY.
dot(direction)) != 0.0){
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);
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);
829 if((lineDotNormal = normalZ.
dot(direction)) != 0.0){
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);
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);
850 intersection = direction * float(outD + delta) + origin;
856 template <
class NODE>
inline bool
859 if (!this->computeRayKeys(origin, end, this->keyrays.at(0))) {
863 for(
KeyRay::iterator it=this->keyrays[0].begin(); it != this->keyrays[0].end(); it++) {
864 updateNode(*it,
false, lazy_eval);
870 template <
class NODE>
bool
874 if ((maxrange > 0) && ((end - origin).norm () > maxrange))
876 point3d direction = (end - origin).normalized ();
877 point3d new_end = origin + direction * (float) maxrange;
878 return integrateMissOnRay(origin, new_end,lazy_eval);
883 if (!integrateMissOnRay(origin, end,lazy_eval))
885 updateNode(end,
true, lazy_eval);
890 template <
class NODE>
893 if (!this->coordToKeyChecked(bbx_min, bbx_min_key)) {
898 template <
class NODE>
901 if (!this->coordToKeyChecked(bbx_max, bbx_max_key)) {
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()) );
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]) );
920 template <
class NODE>
927 template <
class NODE>
931 return bbx_min + obj_bounds;
936 template <
class NODE>
944 this->root =
new NODE();
945 this->readBinaryNode(s, this->root);
946 this->size_changed =
true;
951 template <
class NODE>
953 OCTOMAP_DEBUG(
"Writing %zu nodes to output stream...", this->size());
955 this->writeBinaryNode(s, this->root);
959 template <
class NODE>
966 s.read((
char*)&child1to4_char,
sizeof(
char));
967 s.read((
char*)&child5to8_char,
sizeof(
char));
969 std::bitset<8> child1to4 ((
unsigned long long) child1to4_char);
970 std::bitset<8> child5to8 ((
unsigned long long) child5to8_char);
978 node->setLogOdds(this->clamping_thres_max);
980 for (
unsigned int i=0; i<4; i++) {
981 if ((child1to4[i*2] == 1) && (child1to4[i*2+1] == 0)) {
983 this->createNodeChild(node, i);
984 this->getNodeChild(node, i)->setLogOdds(this->clamping_thres_min);
986 else if ((child1to4[i*2] == 0) && (child1to4[i*2+1] == 1)) {
988 this->createNodeChild(node, i);
989 this->getNodeChild(node, i)->setLogOdds(this->clamping_thres_max);
991 else if ((child1to4[i*2] == 1) && (child1to4[i*2+1] == 1)) {
993 this->createNodeChild(node, i);
994 this->getNodeChild(node, i)->setLogOdds(-200.);
997 for (
unsigned int i=0; i<4; i++) {
998 if ((child5to8[i*2] == 1) && (child5to8[i*2+1] == 0)) {
1000 this->createNodeChild(node, i+4);
1001 this->getNodeChild(node, i+4)->setLogOdds(this->clamping_thres_min);
1003 else if ((child5to8[i*2] == 0) && (child5to8[i*2+1] == 1)) {
1005 this->createNodeChild(node, i+4);
1006 this->getNodeChild(node, i+4)->setLogOdds(this->clamping_thres_max);
1008 else if ((child5to8[i*2] == 1) && (child5to8[i*2+1] == 1)) {
1010 this->createNodeChild(node, i+4);
1011 this->getNodeChild(node, i+4)->setLogOdds(-200.);
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());
1030 template <
class NODE>
1036 std::bitset<8> child1to4;
1037 std::bitset<8> child5to8;
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; }
1056 child1to4[i*2] = 0; child1to4[i*2+1] = 0;
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; }
1068 child5to8[i*2] = 0; child5to8[i*2+1] = 0;
1075 char child1to4_char = (char) child1to4.to_ulong();
1076 char child5to8_char = (char) child5to8.to_ulong();
1078 s.write((
char*)&child1to4_char,
sizeof(char));
1079 s.write((
char*)&child5to8_char,
sizeof(char));
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);
1096 template <
class NODE>
1098 occupancyNode->addValue(update);
1099 if (occupancyNode->getLogOdds() < this->clamping_thres_min) {
1100 occupancyNode->setLogOdds(this->clamping_thres_min);
1103 if (occupancyNode->getLogOdds() > this->clamping_thres_max) {
1104 occupancyNode->setLogOdds(this->clamping_thres_max);
1108 template <
class NODE>
1110 updateNodeLogOdds(occupancyNode, this->prob_hit_log);
1113 template <
class NODE>
1115 updateNodeLogOdds(occupancyNode, this->prob_miss_log);
1118 template <
class NODE>
1120 if (this->isNodeOccupied(occupancyNode))
1121 occupancyNode->setLogOdds(this->clamping_thres_max);
1123 occupancyNode->setLogOdds(this->clamping_thres_min);
1126 template <
class NODE>
1128 if (this->isNodeOccupied(occupancyNode))
1129 occupancyNode.setLogOdds(this->clamping_thres_max);
1131 occupancyNode.setLogOdds(this->clamping_thres_min);
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 ¢er, 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