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

octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Wed Apr 3 2024 02:40:59