test_pruning.cpp
Go to the documentation of this file.
00001 
00002 #include <octomap/octomap.h>
00003 #include "testing.h"
00004 
00005 using namespace std;
00006 using namespace octomap;
00007 using namespace octomath;
00008 
00009 int main(int argc, char** argv) {
00010     double res = 0.01;
00011     OcTree tree(res);
00012 
00013     point3d singlePt(-0.05, -0.02, 1.0);
00014     OcTreeKey singleKey;
00015     tree.coordToKeyChecked(singlePt, singleKey);
00016     OcTreeNode* singleNode = tree.updateNode(singleKey, true);
00017     EXPECT_TRUE(singleNode);
00018     EXPECT_EQ(singleNode, tree.search(singlePt));
00019 
00020     OcTreeKey key;
00021     // check all neighbors, none should exist:
00022     for (key[2] = singleKey[2] - 1; key[2] <= singleKey[2] + 1; ++key[2]){
00023       for (key[1] = singleKey[1] - 1; key[1] <= singleKey[1] + 1; ++key[1]){
00024         for (key[0] = singleKey[0] - 1; key[0] <= singleKey[0] + 1; ++key[0]){
00025           if (key != singleKey){
00026             OcTreeNode* node = tree.search(key);
00027             EXPECT_FALSE(node);
00028           } else {
00029             OcTreeNode* node = tree.search(key);
00030             EXPECT_TRUE(node);
00031             EXPECT_EQ(singleNode, node);
00032           }
00033         }
00034       }
00035     }
00036     // pruning should do nothing:
00037     tree.prune();
00038     for (key[2] = singleKey[2] - 1; key[2] <= singleKey[2] + 1; ++key[2]){
00039       for (key[1] = singleKey[1] - 1; key[1] <= singleKey[1] + 1; ++key[1]){
00040         for (key[0] = singleKey[0] - 1; key[0] <= singleKey[0] + 1; ++key[0]){
00041           if (key != singleKey){
00042             OcTreeNode* node = tree.search(key);
00043             EXPECT_FALSE(node);
00044           } else {
00045             OcTreeNode* node = tree.search(key);
00046             EXPECT_TRUE(node);
00047             EXPECT_EQ(singleNode, node);
00048           }
00049         }
00050       }
00051     }
00052     // node + 1 branch of depth 16
00053     EXPECT_EQ(tree.calcNumNodes(), tree.size());
00054     EXPECT_EQ(tree.size(), 17);
00055     // create diagonal neighbor in same parent node
00056     OcTreeKey singleKey2 = singleKey;
00057     singleKey2[0] +=1;
00058     singleKey2[1] +=1;
00059     singleKey2[2] +=1;
00060     OcTreeNode* singleNode2 = tree.updateNode(singleKey2, true);
00061     EXPECT_TRUE(singleNode2);
00062 
00063     for (key[2] = singleKey[2] - 1; key[2] <= singleKey[2] + 1; ++key[2]){
00064       for (key[1] = singleKey[1] - 1; key[1] <= singleKey[1] + 1; ++key[1]){
00065         for (key[0] = singleKey[0] - 1; key[0] <= singleKey[0] + 1; ++key[0]){
00066           if (key == singleKey){
00067             OcTreeNode* node = tree.search(key);
00068             EXPECT_TRUE(node);
00069             EXPECT_EQ(singleNode, node);
00070           } else if (key == singleKey2){
00071             OcTreeNode* node = tree.search(key);
00072             EXPECT_TRUE(node);
00073             EXPECT_EQ(singleNode2, node);
00074           } else{
00075             OcTreeNode* node = tree.search(key);
00076             EXPECT_FALSE(node);
00077           }
00078         }
00079       }
00080     }
00081     EXPECT_EQ(tree.calcNumNodes(), tree.size());
00082     EXPECT_EQ(tree.size(), 18); // one more leaf at lowest level
00083     // pruning should do nothing:
00084     tree.prune();
00085     for (key[2] = singleKey[2] - 1; key[2] <= singleKey[2] + 1; ++key[2]){
00086       for (key[1] = singleKey[1] - 1; key[1] <= singleKey[1] + 1; ++key[1]){
00087         for (key[0] = singleKey[0] - 1; key[0] <= singleKey[0] + 1; ++key[0]){
00088           if (key == singleKey){
00089             OcTreeNode* node = tree.search(key);
00090             EXPECT_TRUE(node);
00091             EXPECT_EQ(singleNode, node);
00092           } else if (key == singleKey2){
00093             OcTreeNode* node = tree.search(key);
00094             EXPECT_TRUE(node);
00095             EXPECT_EQ(singleNode2, node);
00096           } else{
00097             OcTreeNode* node = tree.search(key);
00098             EXPECT_FALSE(node);
00099           }
00100         }
00101       }
00102     }
00103     EXPECT_EQ(tree.calcNumNodes(), tree.size());
00104     EXPECT_EQ(tree.size(), 18);
00105 
00106     //tree.write("pruning_test_out0.ot");
00107 
00108     // fill the complete octant, should auto-prune
00109     tree.updateNode(OcTreeKey(singleKey[0]+1, singleKey[1]+0, singleKey[2]+0), true);
00110     tree.updateNode(OcTreeKey(singleKey[0]+1, singleKey[1]+1, singleKey[2]+0), true);
00111     tree.updateNode(OcTreeKey(singleKey[0]+0, singleKey[1]+1, singleKey[2]+0), true);
00112     tree.updateNode(OcTreeKey(singleKey[0]+0, singleKey[1]+0, singleKey[2]+1), true);
00113     tree.updateNode(OcTreeKey(singleKey[0]+1, singleKey[1]+0, singleKey[2]+1), true);
00114     EXPECT_EQ(tree.size(), 23);
00115     // last node should trigger auto-pruning:
00116     OcTreeNode* prunedNode = tree.updateNode(OcTreeKey(singleKey[0]+0, singleKey[1]+1, singleKey[2]+1), true);
00117     EXPECT_EQ(tree.size(), 16);
00118     // all queries should now end up at same parent node:
00119     OcTreeNode* parentNode = tree.search(singleKey);
00120     OcTreeNode* parentNode2 = tree.search(singleKey2);
00121     EXPECT_EQ(parentNode, parentNode2);
00122     // test pointer returned by updateNode (pruned)
00123     EXPECT_EQ(prunedNode, parentNode);
00124 
00125     //tree.write("pruning_test_out1.ot");
00126 
00127     // now test larger volume pruning:
00128     for (float x=0.005; x <= 0.32; x+=res){
00129       for (float y=0.005; y <= 0.32; y+=res){
00130         for (float z=0.005; z <= 0.32; z+=res){
00131           OcTreeNode* node = tree.updateNode(point3d(x,y,z), true);
00132           EXPECT_TRUE(node);
00133           EXPECT_TRUE(tree.isNodeOccupied(node));
00134         }
00135       }
00136     }
00137     EXPECT_EQ(tree.calcNumNodes(), tree.size());
00138     EXPECT_EQ(27, tree.size());
00139     // TODO: replace with test for lazy eval?
00140     tree.prune();
00141     EXPECT_EQ(tree.calcNumNodes(), tree.size());
00142     EXPECT_EQ(27, tree.size());
00143     tree.expand();
00144     EXPECT_EQ(tree.calcNumNodes(), tree.size());
00145     EXPECT_EQ(37483, tree.size());
00146     tree.prune();
00147     EXPECT_EQ(27, tree.size());
00148     // test expansion:
00149     for (float x=0.005; x <= 0.32; x+=res){
00150       for (float y=0.005; y <= 0.32; y+=res){
00151         for (float z=0.005; z <= 0.32; z+=res){
00152           OcTreeNode* node = tree.search(point3d(x,y,z));
00153           EXPECT_TRUE(node);
00154           EXPECT_TRUE(tree.isNodeOccupied(node));
00155         }
00156       }
00157     }
00158 
00159     tree.coordToKeyChecked(point3d(0.1, 0.1, 0.1), singleKey);
00160 
00161     EXPECT_TRUE(tree.updateNode(singleKey, true));
00162 
00163     for (float x=0.005; x <= 0.32; x+=res){
00164       for (float y=0.005; y <= 0.32; y+=res){
00165         for (float z=0.005; z <= 0.32; z+=res){
00166           OcTreeNode* node = tree.search(point3d(x,y,z));
00167           EXPECT_TRUE(node);
00168           EXPECT_TRUE(tree.isNodeOccupied(node));
00169         }
00170       }
00171     }
00172     EXPECT_EQ(tree.calcNumNodes(), tree.size());
00173     EXPECT_EQ(67, tree.size());
00174 
00175     //tree.write("pruning_test_out.ot");
00176     std::cerr << "Test successful.\n";
00177     return 0;
00178 
00179 }


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Aug 27 2015 14:13:14