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;
00023 ColorOcTree tree (res);
00024
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);
00031 }
00032 }
00033 }
00034
00035
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);
00042 }
00043 }
00044 }
00045
00046
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
00055 EXPECT_TRUE(tree.write(filename));
00056
00057
00058
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 }