Go to the documentation of this file.00001
00002 #include <stdio.h>
00003 #include <octomap/octomap.h>
00004 #include <octomap/math/Utils.h>
00005
00006 using namespace std;
00007 using namespace octomap;
00008
00009 void printChanges(OcTree& tree){
00010 unsigned int changedOccupied = 0;
00011 unsigned int changedFree = 0;
00012 unsigned int actualOccupied = 0;
00013 unsigned int actualFree = 0;
00014 unsigned int missingChanged = 0;
00015
00016 tree.expand();
00017
00018
00019 KeyBoolMap::const_iterator it;
00020 for (it=tree.changedKeysBegin(); it!=tree.changedKeysEnd(); it++) {
00021 OcTreeNode* node = tree.search(it->first);
00022 if (node != NULL) {
00023 if (tree.isNodeOccupied(node)) {
00024 changedOccupied += 1;
00025 }
00026 else {
00027 changedFree += 1;
00028 }
00029 } else {
00030 missingChanged +=1;
00031 }
00032 }
00033
00034
00035
00036 for(OcTree::tree_iterator it=tree.begin_tree(),
00037 end=tree.end_tree(); it!= end; ++it) {
00038 if (it.isLeaf()) {
00039 if (tree.isNodeOccupied(*it)) {
00040 actualOccupied += 1;
00041 }
00042 else {
00043 actualFree += 1;
00044 }
00045 }
00046 }
00047
00048 cout << "change detection: " << changedOccupied << " occ; " << changedFree << " free; "<< missingChanged << " missing" << endl;
00049 cout << "actual: " << actualOccupied << " occ; " << actualFree << " free; " << endl;
00050
00051 tree.prune();
00052 }
00053
00054
00055
00056 int main(int argc, char** argv) {
00057
00058
00059
00060
00061 OcTree tree (0.05);
00062 tree.enableChangeDetection(true);
00063
00064 point3d origin (0.01, 0.01, 0.02);
00065 point3d point_on_surface (4.01,0.01,0.01);
00066 tree.insertRay(origin, point_on_surface);
00067 printChanges(tree);
00068 tree.updateNode(point3d(2.01, 0.01, 0.01), 2.0f);
00069 printChanges(tree);
00070 tree.updateNode(point3d(2.01, 0.01, 0.01), -2.0f);
00071 printChanges(tree);
00072
00073 cout << "generating spherical scan at " << origin << " ..." << endl;
00074
00075 for (int i=-100; i<101; i++) {
00076 Pointcloud cloud;
00077 for (int j=-100; j<101; j++) {
00078 point3d rotated = point_on_surface;
00079 rotated.rotate_IP(0, DEG2RAD(i*0.5), DEG2RAD(j*0.5));
00080 cloud.push_back(rotated);
00081 }
00082
00083
00084 tree.insertPointCloud(cloud, origin, -1);
00085 }
00086
00087 printChanges(tree);
00088
00089
00090 cout << "done." << endl;
00091
00092 return 0;
00093 }
00094