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
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
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
00053 EXPECT_EQ(tree.calcNumNodes(), tree.size());
00054 EXPECT_EQ(tree.size(), 17);
00055
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);
00083
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
00107
00108
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
00116 OcTreeNode* prunedNode = tree.updateNode(OcTreeKey(singleKey[0]+0, singleKey[1]+1, singleKey[2]+1), true);
00117 EXPECT_EQ(tree.size(), 16);
00118
00119 OcTreeNode* parentNode = tree.search(singleKey);
00120 OcTreeNode* parentNode2 = tree.search(singleKey2);
00121 EXPECT_EQ(parentNode, parentNode2);
00122
00123 EXPECT_EQ(prunedNode, parentNode);
00124
00125
00126
00127
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
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
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
00176 std::cerr << "Test successful.\n";
00177 return 0;
00178
00179 }