test_color_tree.cpp
Go to the documentation of this file.
00001 #include <octomap/octomap.h>
00002 #include <octomap/ColorOcTree.h>
00003 #include "testing.h"
00004 
00005 using namespace std;
00006 using namespace octomap;
00007 
00008 
00009 void print_query_info(point3d query, ColorOcTreeNode* node) {
00010   if (node != NULL) {
00011     cout << "occupancy probability at " << query << ":\t " << node->getOccupancy() << endl;
00012     cout << "color of node is: " << node->getColor()
00013          << endl;    
00014   }
00015   else 
00016     cout << "occupancy probability at " << query << ":\t is unknown" << endl;  
00017 }
00018 
00019 
00020 int main(int argc, char** argv) {
00021 
00022   double res = 0.05;  // create empty tree with resolution 0.05 (different from default 0.1 for test)
00023   ColorOcTree tree (res);
00024   // insert some measurements of occupied cells
00025   for (int x=-20; x<20; x++) {
00026     for (int y=-20; y<20; y++) {
00027       for (int z=-20; z<20; z++) {
00028         point3d endpoint ((float) x*0.05f+0.01f, (float) y*0.05f+0.01f, (float) z*0.05f+0.01f);
00029         ColorOcTreeNode* n = tree.updateNode(endpoint, true); 
00030         n->setColor(z*5+100,x*5+100,y*5+100); // set color to red
00031       }
00032     }
00033   }
00034 
00035   // insert some measurements of free cells
00036   for (int x=-30; x<30; x++) {
00037     for (int y=-30; y<30; y++) {
00038       for (int z=-30; z<30; z++) {
00039         point3d endpoint ((float) x*0.02f+2.0f, (float) y*0.02f+2.0f, (float) z*0.02f+2.0f);
00040         ColorOcTreeNode* n = tree.updateNode(endpoint, false); 
00041         n->setColor(255,255,0); // set color to yellow
00042       }
00043     }
00044   }
00045 
00046   // set inner node colors
00047   tree.updateInnerOccupancy();
00048 
00049   cout << endl;
00050 
00051 
00052   std::string filename ("simple_color_tree.ot");
00053   std::cout << "Writing color tree to " << filename << std::endl;
00054   // write color tree
00055   EXPECT_TRUE(tree.write(filename));
00056 
00057 
00058   // read tree file
00059   cout << "Reading color tree from "<< filename <<"\n";
00060   AbstractOcTree* read_tree = AbstractOcTree::read(filename);
00061   EXPECT_TRUE(read_tree);
00062   EXPECT_EQ(read_tree->getTreeType().compare(tree.getTreeType()), 0);
00063   EXPECT_FLOAT_EQ(read_tree->getResolution(), tree.getResolution());
00064   EXPECT_EQ(read_tree->size(), tree.size());
00065   ColorOcTree* read_color_tree = dynamic_cast<ColorOcTree*>(read_tree);
00066   EXPECT_TRUE(read_color_tree);
00067 
00068 
00069   cout << "Performing some queries:" << endl;
00070   
00071   {
00072     point3d query (0., 0., 0.);
00073     ColorOcTreeNode* result = tree.search (query);
00074     ColorOcTreeNode* result2 = read_color_tree->search (query);
00075     std::cout << "READ: ";
00076     print_query_info(query, result);
00077     std::cout << "WRITE: ";
00078     print_query_info(query, result2);
00079     EXPECT_TRUE(result);
00080     EXPECT_TRUE(result2);
00081     EXPECT_EQ(result->getColor(), result2->getColor());
00082     EXPECT_EQ(result->getLogOdds(), result2->getLogOdds());
00083     
00084     query = point3d(-1.,-1.,-1.);
00085     result = tree.search (query);
00086     result2 = read_color_tree->search (query);
00087     print_query_info(query, result);
00088     std::cout << "READ: ";
00089     print_query_info(query, result);
00090     std::cout << "WRITE: ";
00091     print_query_info(query, result2);
00092     EXPECT_TRUE(result);
00093     EXPECT_TRUE(result2);
00094     EXPECT_EQ(result->getColor(), result2->getColor());
00095     EXPECT_EQ(result->getLogOdds(), result2->getLogOdds());
00096     
00097     query = point3d(1.,1.,1.);
00098     result = tree.search (query);
00099     result2 = read_color_tree->search (query);
00100     print_query_info(query, result);
00101     std::cout << "READ: ";
00102     print_query_info(query, result);
00103     std::cout << "WRITE: ";
00104     print_query_info(query, result2);
00105     EXPECT_FALSE(result);
00106     EXPECT_FALSE(result2);
00107 
00108   }
00109 
00110   delete read_tree;
00111 
00112   return 0;
00113 }


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