test_changedkeys.cpp
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   // iterate through the changed nodes
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   // iterate through the entire tree
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     // insert in global coordinates:
00084     tree.insertPointCloud(cloud, origin, -1);
00085   }
00086 
00087   printChanges(tree);
00088 
00089 
00090   cout << "done." << endl;
00091 
00092   return 0;
00093 }
00094 


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Feb 11 2016 23:50:59