test_iterators.cpp
Go to the documentation of this file.
00001 
00002 #include <stdio.h>
00003 #include <stdlib.h>
00004 #include <string.h>
00005 
00006 #include <octomap/octomap_timing.h>
00007 #include <octomap/octomap.h>
00008 #include <octomap/math/Utils.h>
00009 #include "testing.h"
00010 
00011 using namespace std;
00012 using namespace octomap;
00013 
00014 void printUsage(char* self){
00015   std::cerr << "\nUSAGE: " << self << " inputfile.bt [max_depth]  (optional)\n\n";
00016 
00017 
00018   exit(1);
00019 }
00020 
00021 void computeChildCenter (const unsigned int& pos,
00022     const float& center_offset,
00023     const point3d& parent_center,
00024     point3d& child_center) {
00025   // x-axis
00026   if (pos & 1) child_center(0) = parent_center(0) + center_offset;
00027   else     child_center(0) = parent_center(0) - center_offset;
00028 
00029   // y-axis
00030   if (pos & 2) child_center(1) = parent_center(1) + center_offset;
00031   else   child_center(1) = parent_center(1) - center_offset;
00032   // z-axis
00033   if (pos & 4) child_center(2) = parent_center(2) + center_offset;
00034   else   child_center(2) = parent_center(2) - center_offset;
00035 }
00036 
00038 void getLeafNodesRecurs(std::list<OcTreeVolume>& voxels,
00039     unsigned int max_depth,
00040     OcTreeNode* node, unsigned int depth,
00041     const point3d& parent_center, const point3d& tree_center,
00042     OcTree* tree, bool occupied)
00043 {
00044   if ((depth <= max_depth) && (node != NULL) ) {
00045     if (node->hasChildren() && (depth != max_depth)) {
00046 
00047       double center_offset = tree_center(0) / pow( 2., (double) depth+1);
00048       point3d search_center;
00049 
00050       for (unsigned int i=0; i<8; i++) {
00051         if (node->childExists(i)) {
00052 
00053           computeChildCenter(i, center_offset, parent_center, search_center);
00054           getLeafNodesRecurs(voxels, max_depth, node->getChild(i), depth+1, search_center, tree_center, tree, occupied);
00055 
00056         } // GetChild
00057       }
00058     }
00059     else {
00060       if (tree->isNodeOccupied(node) == occupied){
00061         double voxelSize = tree->getResolution() * pow(2., double(16 - depth));
00062         voxels.push_back(std::make_pair(parent_center - tree_center, voxelSize));
00063 
00064       }
00065     }
00066   }
00067 }
00068 
00069 
00071 void getVoxelsRecurs(std::list<OcTreeVolume>& voxels,
00072                                        unsigned int max_depth,
00073                                        OcTreeNode* node, unsigned int depth,
00074                                        const point3d& parent_center, const point3d& tree_center,
00075                                        double resolution){
00076 
00077   if ((depth <= max_depth) && (node != NULL) ) {
00078     if (node->hasChildren() && (depth != max_depth)) {
00079 
00080       double center_offset = tree_center(0) / pow(2., (double) depth + 1);
00081       point3d search_center;
00082 
00083       for (unsigned int i = 0; i < 8; i++) {
00084         if (node->childExists(i)) {
00085           computeChildCenter(i, (float) center_offset, parent_center, search_center);
00086           getVoxelsRecurs(voxels, max_depth, node->getChild(i), depth + 1, search_center, tree_center, resolution);
00087 
00088         }
00089       } // depth
00090     }
00091     double voxelSize = resolution * pow(2., double(16 - depth));
00092     voxels.push_back(std::make_pair(parent_center - tree_center, voxelSize));
00093   }
00094 }
00095 
00096 
00098 void compareResults(const std::list<OcTreeVolume>& list_iterator, const std::list<OcTreeVolume>& list_depr){
00099   EXPECT_EQ(list_iterator.size(), list_depr.size());
00100   list<OcTreeVolume>::const_iterator list_it = list_iterator.begin();
00101   list<OcTreeVolume>::const_iterator list_depr_it = list_depr.begin();
00102 
00103   for (; list_it != list_iterator.end(); ++list_it, ++list_depr_it){
00104     EXPECT_NEAR(list_it->first.x(), list_depr_it->first.x(), 0.005);
00105     EXPECT_NEAR(list_it->first.y(), list_depr_it->first.y(), 0.005);
00106     EXPECT_NEAR(list_it->first.z(), list_depr_it->first.z(), 0.005);
00107   }
00108   std::cout << "Resulting lists (size "<< list_iterator.size() << ") identical\n";
00109 }
00110 
00111 // for unique comparing, need to sort the lists:
00112 bool OcTreeVolumeSortPredicate(const OcTreeVolume& lhs, const OcTreeVolume& rhs)
00113 {
00114   return ( lhs.second < rhs.second
00115       ||  (lhs.second == rhs.second &&
00116           lhs.first.x() < rhs.first.x()
00117           &&  lhs.first.y() < rhs.first.y()
00118           &&  lhs.first.z() < rhs.first.z()));
00119 }
00120 
00121 
00122 double timediff(const timeval& start, const timeval& stop){
00123   return (stop.tv_sec - start.tv_sec) + 1.0e-6 *(stop.tv_usec - start.tv_usec);
00124 }
00125 
00126 int main(int argc, char** argv) {
00127 
00128 
00129   //##############################################################     
00130 
00131   string btFilename = "";
00132   unsigned char maxDepth = 16;
00133 
00134 
00135   // test timing:
00136   timeval start;
00137   timeval stop;
00138   const unsigned char tree_depth(16);
00139   const unsigned int tree_max_val(32768);
00140   double time_it, time_depr;
00141 
00142   if (argc <= 1|| argc >3 || strcmp(argv[1], "-h") == 0){
00143     printUsage(argv[0]);
00144   }
00145 
00146   btFilename = std::string(argv[1]);
00147   if (argc > 2){
00148     maxDepth = (unsigned char)atoi(argv[2]);
00149   }
00150   maxDepth = std::min((unsigned char)16,maxDepth);
00151 
00152   if (maxDepth== 0)
00153     maxDepth = tree_depth;
00154 
00155   // iterate over empty tree:
00156   OcTree emptyTree(0.2);
00157   EXPECT_EQ(emptyTree.size(), 0);
00158   EXPECT_EQ(emptyTree.calcNumNodes(), 0);
00159 
00160   size_t iteratedNodes = 0;
00161   OcTree::tree_iterator t_it = emptyTree.begin_tree(maxDepth);
00162   OcTree::tree_iterator t_end = emptyTree.end_tree();
00163   EXPECT_TRUE (t_it == t_end);
00164   for( ; t_it != t_end; ++t_it){
00165     iteratedNodes++;
00166   }
00167   EXPECT_EQ(iteratedNodes, 0);
00168 
00169 
00170   for(OcTree::leaf_iterator l_it = emptyTree.begin_leafs(maxDepth), l_end=emptyTree.end_leafs(); l_it!= l_end; ++l_it){
00171     iteratedNodes++;
00172   }
00173   EXPECT_EQ(iteratedNodes, 0);
00174 
00175 
00176 
00177 
00178 
00179   cout << "\nReading OcTree file\n===========================\n";
00180   OcTree* tree = new OcTree(btFilename);
00181   if (tree->size()<= 1){
00182     std::cout << "Error reading file, exiting!\n";
00183     return 1;
00184   }
00185 
00186   size_t count;
00187   std::list<OcTreeVolume> list_depr;
00188   std::list<OcTreeVolume> list_iterator;
00189 
00193   gettimeofday(&start, NULL);  // start timer
00194   size_t num_leafs_recurs = tree->getNumLeafNodes();
00195   gettimeofday(&stop, NULL);  // stop timer
00196   time_depr = timediff(start, stop);
00197 
00198   gettimeofday(&start, NULL);  // start timer
00199   size_t num_leafs_it = 0;
00200   for(OcTree::leaf_iterator it = tree->begin(), end=tree->end(); it!= end; ++it) {
00201     num_leafs_it++;
00202   }
00203   gettimeofday(&stop, NULL);  // stop timer
00204   time_it = timediff(start, stop);
00205   std::cout << "Number of leafs: " << num_leafs_it << " / " << num_leafs_recurs << ", times: "
00206         <<time_it << " / " << time_depr << "\n========================\n\n";
00207 
00208 
00212   point3d tree_center;
00213   tree_center(0) = tree_center(1) = tree_center(2)
00214               = (float) (((double) tree_max_val) * tree->getResolution());
00215 
00216   gettimeofday(&start, NULL);  // start timer
00217   getLeafNodesRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree, true);
00218   gettimeofday(&stop, NULL);  // stop timer
00219   time_depr = timediff(start, stop);
00220 
00221   gettimeofday(&start, NULL);  // start timer
00222   for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it){
00223     if(tree->isNodeOccupied(*it))
00224     {
00225       //count ++;
00226      list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
00227     }
00228 
00229   }
00230   gettimeofday(&stop, NULL);  // stop timer
00231   time_it = timediff(start, stop);
00232 
00233   std::cout << "Occupied lists traversed, times: "
00234       <<time_it << " / " << time_depr << "\n";
00235   compareResults(list_iterator, list_depr);
00236   std::cout << "========================\n\n";
00237 
00238 
00242   list_iterator.clear();
00243   list_depr.clear();
00244   gettimeofday(&start, NULL);  // start timer
00245   for(OcTree::leaf_iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it) {
00246     if(!tree->isNodeOccupied(*it))
00247       list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
00248   }
00249   gettimeofday(&stop, NULL);  // stop timer
00250   time_it = timediff(start, stop);
00251 
00252   gettimeofday(&start, NULL);  // start timer
00253   getLeafNodesRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree, false);
00254   gettimeofday(&stop, NULL);  // stop timer
00255   time_depr = timediff(start, stop);
00256 
00257   std::cout << "Free lists traversed, times: "
00258       <<time_it << " / " << time_depr << "\n";
00259   compareResults(list_iterator, list_depr);
00260     std::cout << "========================\n\n";
00261 
00262 
00263 
00267   list_iterator.clear();
00268   list_depr.clear();
00269 
00270   gettimeofday(&start, NULL);  // start timer
00271   getVoxelsRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree->getResolution());
00272   gettimeofday(&stop, NULL);  // stop timer
00273   time_depr = timediff(start, stop);
00274 
00275   gettimeofday(&start, NULL);  // start timers
00276   for(OcTree::tree_iterator it = tree->begin_tree(maxDepth), end=tree->end_tree();
00277       it!= end; ++it){
00278       //count ++;
00279       //std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl;
00280      list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
00281   }
00282   gettimeofday(&stop, NULL);  // stop timer
00283   time_it = timediff(start, stop);
00284 
00285   list_iterator.sort(OcTreeVolumeSortPredicate);
00286   list_depr.sort(OcTreeVolumeSortPredicate);
00287 
00288   std::cout << "All inner lists traversed, times: "
00289       <<time_it << " / " << time_depr << "\n";
00290   compareResults(list_iterator, list_depr);
00291     std::cout << "========================\n\n";
00292 
00293 
00294 
00295     // traverse all leaf nodes, timing:
00296     gettimeofday(&start, NULL);  // start timers
00297     count = 0;
00298     for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end();
00299         it!= end; ++it){
00300       // do something:
00301       // std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl;
00302       count++;
00303     }
00304 
00305     gettimeofday(&stop, NULL);  // stop timer
00306     time_it = timediff(start, stop);
00307 
00308     std::cout << "Time to traverse all leafs at max depth " <<(unsigned int)maxDepth <<" ("<<count<<" nodes): "<< time_it << " s\n\n";
00309 
00310 
00311 
00312 
00316   //tree->expand();
00317   // test complete tree (should be equal to no bbx)
00318   OcTreeKey bbxMinKey, bbxMaxKey;
00319   double temp_x,temp_y,temp_z;
00320   tree->getMetricMin(temp_x,temp_y,temp_z);
00321   octomap::point3d bbxMin(temp_x,temp_y,temp_z);
00322 
00323   tree->getMetricMax(temp_x,temp_y,temp_z);
00324   octomap::point3d bbxMax(temp_x,temp_y,temp_z);
00325 
00326   EXPECT_TRUE(tree->coordToKeyChecked(bbxMin, bbxMinKey));
00327   EXPECT_TRUE(tree->coordToKeyChecked(bbxMax, bbxMaxKey));
00328 
00329   OcTree::leaf_bbx_iterator it_bbx = tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey);
00330   EXPECT_TRUE(it_bbx == tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey));
00331   OcTree::leaf_bbx_iterator end_bbx = tree->end_leafs_bbx();
00332   EXPECT_TRUE(end_bbx == tree->end_leafs_bbx());
00333 
00334   OcTree::leaf_iterator it = tree->begin_leafs();
00335   EXPECT_TRUE(it == tree->begin_leafs());
00336   OcTree::leaf_iterator end = tree->end_leafs();
00337   EXPECT_TRUE(end == tree->end_leafs());
00338 
00339 
00340   for( ; it!= end && it_bbx != end_bbx; ++it, ++it_bbx){
00341     EXPECT_TRUE(it == it_bbx);
00342   }
00343   EXPECT_TRUE(it == end && it_bbx == end_bbx);
00344 
00345 
00346   // now test an actual bounding box:
00347   tree->expand(); // (currently only works properly for expanded tree (no multires)
00348   bbxMin = point3d(-1, -1, - 1);
00349   bbxMax = point3d(3, 2, 1);
00350   EXPECT_TRUE(tree->coordToKeyChecked(bbxMin, bbxMinKey));
00351   EXPECT_TRUE(tree->coordToKeyChecked(bbxMax, bbxMaxKey));
00352 
00353   typedef unordered_ns::unordered_map<OcTreeKey, double, OcTreeKey::KeyHash> KeyVolumeMap;
00354 
00355   KeyVolumeMap bbxVoxels;
00356 
00357   count = 0;
00358   for(OcTree::leaf_bbx_iterator it = tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey), end=tree->end_leafs_bbx();
00359       it!= end; ++it)
00360   {
00361     count++;
00362     OcTreeKey currentKey = it.getKey();
00363     // leaf is actually a leaf:
00364     EXPECT_FALSE(it->hasChildren());
00365 
00366     // leaf exists in tree:
00367     OcTreeNode* node = tree->search(currentKey);
00368     EXPECT_TRUE(node);
00369     EXPECT_EQ(node, &(*it));
00370     // all leafs are actually in the bbx:
00371     for (unsigned i = 0; i < 3; ++i){
00372 //      if (!(currentKey[i] >= bbxMinKey[i] && currentKey[i] <= bbxMaxKey[i])){
00373 //        std::cout << "Key failed: " << i << " " << currentKey[i] << " "<< bbxMinKey[i] << " "<< bbxMaxKey[i]
00374 //             << "size: "<< it.getSize()<< std::endl;
00375 //      }
00376       EXPECT_TRUE(currentKey[i] >= bbxMinKey[i] && currentKey[i] <= bbxMaxKey[i]);
00377     }
00378 
00379     bbxVoxels.insert(std::pair<OcTreeKey,double>(currentKey, it.getSize()));
00380   }
00381   EXPECT_EQ(bbxVoxels.size(), count);
00382   std::cout << "Bounding box traversed ("<< count << " leaf nodes)\n\n";
00383 
00384 
00385   // compare with manual BBX check on all leafs:
00386   for(OcTree::leaf_iterator it = tree->begin(), end=tree->end(); it!= end; ++it) {
00387     OcTreeKey key = it.getKey();
00388     if (    key[0] >= bbxMinKey[0] && key[0] <= bbxMaxKey[0]
00389          && key[1] >= bbxMinKey[1] && key[1] <= bbxMaxKey[1]
00390          && key[2] >= bbxMinKey[2] && key[2] <= bbxMaxKey[2])
00391     {
00392       KeyVolumeMap::iterator bbxIt = bbxVoxels.find(key);
00393       EXPECT_FALSE(bbxIt == bbxVoxels.end());
00394       EXPECT_TRUE(key == bbxIt->first);
00395       EXPECT_EQ(it.getSize(), bbxIt->second);
00396     }
00397 
00398   }
00399 
00400   // test tree with one node:
00401   OcTree simpleTree(0.01);
00402   simpleTree.updateNode(point3d(10, 10, 10), 5.0f);
00403   for(OcTree::leaf_iterator it = simpleTree.begin_leafs(maxDepth), end=simpleTree.end_leafs(); it!= end; ++it) {
00404     std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl;
00405   }
00406 
00407 
00408   std::cout << "Tests successful\n";
00409 
00410 
00411   return 0;
00412 }
00413 


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Jun 6 2019 17:31:45