test_pruning.cpp
Go to the documentation of this file.
1 
2 #include <octomap/octomap.h>
3 #include "testing.h"
4 
5 using namespace std;
6 using namespace octomap;
7 using namespace octomath;
8 
9 int main(int argc, char** argv) {
10  float res = 0.01f;
11  OcTree tree(res);
12 
13  point3d singlePt(-0.05f, -0.02f, 1.0f);
14  OcTreeKey singleKey;
15  tree.coordToKeyChecked(singlePt, singleKey);
16  OcTreeNode* singleNode = tree.updateNode(singleKey, true);
17  EXPECT_TRUE(singleNode);
18  EXPECT_EQ(singleNode, tree.search(singlePt));
19 
20  OcTreeKey key;
21  // check all neighbors, none should exist:
22  for (key[2] = singleKey[2] - 1; key[2] <= singleKey[2] + 1; ++key[2]){
23  for (key[1] = singleKey[1] - 1; key[1] <= singleKey[1] + 1; ++key[1]){
24  for (key[0] = singleKey[0] - 1; key[0] <= singleKey[0] + 1; ++key[0]){
25  if (key != singleKey){
26  OcTreeNode* node = tree.search(key);
27  EXPECT_FALSE(node);
28  } else {
29  OcTreeNode* node = tree.search(key);
30  EXPECT_TRUE(node);
31  EXPECT_EQ(singleNode, node);
32  }
33  }
34  }
35  }
36  // pruning should do nothing:
37  tree.prune();
38  for (key[2] = singleKey[2] - 1; key[2] <= singleKey[2] + 1; ++key[2]){
39  for (key[1] = singleKey[1] - 1; key[1] <= singleKey[1] + 1; ++key[1]){
40  for (key[0] = singleKey[0] - 1; key[0] <= singleKey[0] + 1; ++key[0]){
41  if (key != singleKey){
42  OcTreeNode* node = tree.search(key);
43  EXPECT_FALSE(node);
44  } else {
45  OcTreeNode* node = tree.search(key);
46  EXPECT_TRUE(node);
47  EXPECT_EQ(singleNode, node);
48  }
49  }
50  }
51  }
52  // node + 1 branch of depth 16
53  EXPECT_EQ(tree.calcNumNodes(), tree.size());
54  EXPECT_EQ(tree.size(), 17);
55  // create diagonal neighbor in same parent node
56  OcTreeKey singleKey2 = singleKey;
57  singleKey2[0] +=1;
58  singleKey2[1] +=1;
59  singleKey2[2] +=1;
60  OcTreeNode* singleNode2 = tree.updateNode(singleKey2, true);
61  EXPECT_TRUE(singleNode2);
62 
63  for (key[2] = singleKey[2] - 1; key[2] <= singleKey[2] + 1; ++key[2]){
64  for (key[1] = singleKey[1] - 1; key[1] <= singleKey[1] + 1; ++key[1]){
65  for (key[0] = singleKey[0] - 1; key[0] <= singleKey[0] + 1; ++key[0]){
66  if (key == singleKey){
67  OcTreeNode* node = tree.search(key);
68  EXPECT_TRUE(node);
69  EXPECT_EQ(singleNode, node);
70  } else if (key == singleKey2){
71  OcTreeNode* node = tree.search(key);
72  EXPECT_TRUE(node);
73  EXPECT_EQ(singleNode2, node);
74  } else{
75  OcTreeNode* node = tree.search(key);
76  EXPECT_FALSE(node);
77  }
78  }
79  }
80  }
81  EXPECT_EQ(tree.calcNumNodes(), tree.size());
82  EXPECT_EQ(tree.size(), 18); // one more leaf at lowest level
83  // pruning should do nothing:
84  tree.prune();
85  for (key[2] = singleKey[2] - 1; key[2] <= singleKey[2] + 1; ++key[2]){
86  for (key[1] = singleKey[1] - 1; key[1] <= singleKey[1] + 1; ++key[1]){
87  for (key[0] = singleKey[0] - 1; key[0] <= singleKey[0] + 1; ++key[0]){
88  if (key == singleKey){
89  OcTreeNode* node = tree.search(key);
90  EXPECT_TRUE(node);
91  EXPECT_EQ(singleNode, node);
92  } else if (key == singleKey2){
93  OcTreeNode* node = tree.search(key);
94  EXPECT_TRUE(node);
95  EXPECT_EQ(singleNode2, node);
96  } else{
97  OcTreeNode* node = tree.search(key);
98  EXPECT_FALSE(node);
99  }
100  }
101  }
102  }
103  EXPECT_EQ(tree.calcNumNodes(), tree.size());
104  EXPECT_EQ(tree.size(), 18);
105 
106  //tree.write("pruning_test_out0.ot");
107 
108  // fill the complete octant, should auto-prune
109  tree.updateNode(OcTreeKey(singleKey[0]+1, singleKey[1]+0, singleKey[2]+0), true);
110  tree.updateNode(OcTreeKey(singleKey[0]+1, singleKey[1]+1, singleKey[2]+0), true);
111  tree.updateNode(OcTreeKey(singleKey[0]+0, singleKey[1]+1, singleKey[2]+0), true);
112  tree.updateNode(OcTreeKey(singleKey[0]+0, singleKey[1]+0, singleKey[2]+1), true);
113  tree.updateNode(OcTreeKey(singleKey[0]+1, singleKey[1]+0, singleKey[2]+1), true);
114  EXPECT_EQ(tree.size(), 23);
115  // last node should trigger auto-pruning:
116  OcTreeNode* prunedNode = tree.updateNode(OcTreeKey(singleKey[0]+0, singleKey[1]+1, singleKey[2]+1), true);
117  EXPECT_EQ(tree.size(), 16);
118  // all queries should now end up at same parent node:
119  OcTreeNode* parentNode = tree.search(singleKey);
120  OcTreeNode* parentNode2 = tree.search(singleKey2);
121  EXPECT_EQ(parentNode, parentNode2);
122  // test pointer returned by updateNode (pruned)
123  EXPECT_EQ(prunedNode, parentNode);
124 
125  //tree.write("pruning_test_out1.ot");
126 
127  // now test larger volume pruning:
128  for (float x=0.005f; x <= 0.32f; x+=res){
129  for (float y=0.005f; y <= 0.32f; y+=res){
130  for (float z=0.005f; z <= 0.32f; z+=res){
131  OcTreeNode* node = tree.updateNode(point3d(x,y,z), true);
132  EXPECT_TRUE(node);
133  EXPECT_TRUE(tree.isNodeOccupied(node));
134  }
135  }
136  }
137  EXPECT_EQ(tree.calcNumNodes(), tree.size());
138  EXPECT_EQ(27, tree.size());
139  // TODO: replace with test for lazy eval?
140  tree.prune();
141  EXPECT_EQ(tree.calcNumNodes(), tree.size());
142  EXPECT_EQ(27, tree.size());
143  tree.expand();
144  EXPECT_EQ(tree.calcNumNodes(), tree.size());
145  EXPECT_EQ(37483, tree.size());
146  tree.prune();
147  EXPECT_EQ(27, tree.size());
148  // test expansion:
149  for (float x=0.005f; x <= 0.32f; x+=res){
150  for (float y=0.005f; y <= 0.32f; y+=res){
151  for (float z=0.005f; z <= 0.32f; z+=res){
152  OcTreeNode* node = tree.search(point3d(x,y,z));
153  EXPECT_TRUE(node);
154  EXPECT_TRUE(tree.isNodeOccupied(node));
155  }
156  }
157  }
158 
159  tree.coordToKeyChecked(point3d(0.1f, 0.1f, 0.1f), singleKey);
160 
161  EXPECT_TRUE(tree.updateNode(singleKey, true));
162 
163  for (float x=0.005f; x <= 0.32f; x+=res){
164  for (float y=0.005f; y <= 0.32f; y+=res){
165  for (float z=0.005f; z <= 0.32f; z+=res){
166  OcTreeNode* node = tree.search(point3d(x,y,z));
167  EXPECT_TRUE(node);
168  EXPECT_TRUE(tree.isNodeOccupied(node));
169  }
170  }
171  }
172  EXPECT_EQ(tree.calcNumNodes(), tree.size());
173  EXPECT_EQ(67, tree.size());
174 
175  // test deletion / pruning of single nodes
176  {
177  std::cout << "\nCreating / deleting nodes\n===============================\n";
178  size_t initialSize = tree.size();
179  EXPECT_EQ(initialSize, tree.calcNumNodes());
180  EXPECT_EQ(initialSize, 67);
181 
182  point3d newCoord(-2.0, -2.0, -2.0);
183  OcTreeNode* newNode = tree.updateNode(newCoord, true);
184  EXPECT_TRUE(newNode != NULL);
185 
186  size_t insertedSize = tree.size();
187  std::cout << "Size after one insertion: " << insertedSize << std::endl;
188  EXPECT_EQ(insertedSize, tree.calcNumNodes());
189  EXPECT_EQ(insertedSize, 83);
190 
191  // find parent of newly inserted node:
192  OcTreeNode* parentNode = tree.search(newCoord, tree.getTreeDepth() -1);
193  EXPECT_TRUE(parentNode);
194  EXPECT_TRUE(tree.nodeHasChildren(parentNode));
195 
196  // only one child exists:
197  for (size_t i = 0; i < 7; ++i){
198  EXPECT_FALSE(tree.nodeChildExists(parentNode, i));
199  }
200  EXPECT_TRUE(tree.nodeChildExists(parentNode, 7));
201 
202  // create another new node manually:
203  OcTreeNode* newNodeCreated = tree.createNodeChild(parentNode, 0);
204  EXPECT_TRUE(newNodeCreated != NULL);
205  EXPECT_TRUE(tree.nodeChildExists(parentNode, 0));
206  const float value = 0.123f;
207  newNodeCreated->setValue(value);
208  tree.write("pruning_test_edited.ot");
209 
210  EXPECT_EQ(tree.size(), tree.calcNumNodes());
211  EXPECT_EQ(tree.size(), insertedSize+1);
212  tree.prune();
213  EXPECT_EQ(tree.calcNumNodes(), insertedSize+1);
214 
215  tree.deleteNodeChild(parentNode, 0);
216  tree.deleteNodeChild(parentNode, 7);
217 
218  EXPECT_EQ(tree.size(), tree.calcNumNodes());
219  EXPECT_EQ(tree.size(), insertedSize-1);
220 
221  tree.prune();
222  EXPECT_EQ(tree.size(), tree.calcNumNodes());
223  EXPECT_EQ(tree.size(), insertedSize-1);
224 
225  tree.expandNode(parentNode);
226  EXPECT_EQ(tree.size(), tree.calcNumNodes());
227  EXPECT_EQ(tree.size(), insertedSize+7);
228 
229 
230  EXPECT_TRUE(tree.pruneNode(parentNode));
231  EXPECT_EQ(tree.size(), tree.calcNumNodes());
232  EXPECT_EQ(tree.size(), insertedSize-1);
233 
234 
235  }
236 
237  tree.write("pruning_test_out.ot");
238  std::cerr << "Test successful.\n";
239  return 0;
240 
241 }
octomap::OcTreeBaseImpl::expand
virtual void expand()
octomap::OccupancyOcTreeBase::updateNode
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
octomap::OcTreeBaseImpl::deleteNodeChild
void deleteNodeChild(NODE *node, unsigned int childIdx)
Deletes the i-th child of the node.
octomap::OcTreeBaseImpl::search
NODE * search(double x, double y, double z, unsigned int depth=0) const
octomap::OcTreeBaseImpl::getTreeDepth
unsigned int getTreeDepth() const
Definition: OcTreeBaseImpl.h:111
octomap::OcTreeBaseImpl::coordToKeyChecked
bool coordToKeyChecked(const point3d &coord, OcTreeKey &key) const
octomap::OcTreeBaseImpl::createNodeChild
NODE * createNodeChild(NODE *node, unsigned int childIdx)
Creates (allocates) the i-th child of the node.
EXPECT_FALSE
#define EXPECT_FALSE(args)
Definition: testing.h:11
EXPECT_EQ
#define EXPECT_EQ(a, b)
Definition: testing.h:16
octomap::OcTreeBaseImpl::nodeHasChildren
bool nodeHasChildren(const NODE *node) const
octomap::OcTreeBaseImpl::expandNode
virtual void expandNode(NODE *node)
octomap::AbstractOcTree::write
bool write(const std::string &filename) const
Write file header and complete tree to file (serialization)
Definition: AbstractOcTree.cpp:45
octomath::Vector3
This class represents a three-dimensional vector.
Definition: Vector3.h:50
main
int main(int argc, char **argv)
Definition: test_pruning.cpp:9
octomap::point3d
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:48
octomap::OcTreeBaseImpl::nodeChildExists
bool nodeChildExists(const NODE *node, unsigned int childIdx) const
octomap::OcTreeBaseImpl::pruneNode
virtual bool pruneNode(NODE *node)
octomap::OcTreeBaseImpl::prune
virtual void prune()
EXPECT_TRUE
#define EXPECT_TRUE(args)
Definition: testing.h:6
octomap::OcTreeBaseImpl::calcNumNodes
size_t calcNumNodes() const
Traverses the tree to calculate the total number of nodes.
octomap::AbstractOccupancyOcTree::isNodeOccupied
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
queries whether a node is occupied according to the tree&#39;s parameter for "occupancy" ...
Definition: AbstractOccupancyOcTree.h:114
octomap::OcTreeBaseImpl::size
virtual size_t size() const
Definition: OcTreeBaseImpl.h:241

octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Wed Jun 5 2019 19:26:27