test_iterators.cpp
Go to the documentation of this file.
1 
2 #include <stdio.h>
3 #include <stdlib.h>
4 #include <string.h>
5 
7 #include <octomap/octomap.h>
8 #include <octomap/math/Utils.h>
9 #include "testing.h"
10 
11 using namespace std;
12 using namespace octomap;
13 
14 void printUsage(char* self){
15  std::cerr << "\nUSAGE: " << self << " inputfile.bt [max_depth] (optional)\n\n";
16 
17 
18  exit(1);
19 }
20 
21 void computeChildCenter (const unsigned int& pos,
22  const float& center_offset,
23  const point3d& parent_center,
24  point3d& child_center) {
25  // x-axis
26  if (pos & 1) child_center(0) = parent_center(0) + center_offset;
27  else child_center(0) = parent_center(0) - center_offset;
28 
29  // y-axis
30  if (pos & 2) child_center(1) = parent_center(1) + center_offset;
31  else child_center(1) = parent_center(1) - center_offset;
32  // z-axis
33  if (pos & 4) child_center(2) = parent_center(2) + center_offset;
34  else child_center(2) = parent_center(2) - center_offset;
35 }
36 
38 void getLeafNodesRecurs(std::list<OcTreeVolume>& voxels,
39  unsigned int max_depth,
40  OcTreeNode* node, unsigned int depth,
41  const point3d& parent_center, const point3d& tree_center,
42  OcTree* tree, bool occupied)
43 {
44  if ((depth <= max_depth) && (node != NULL) ) {
45  if (tree->nodeHasChildren(node) && (depth != max_depth)) {
46 
47  float center_offset = float(tree_center(0) / pow( 2., (double) depth+1));
48  point3d search_center;
49 
50  for (unsigned int i=0; i<8; i++) {
51  if (tree->nodeChildExists(node, i)) {
52 
53  computeChildCenter(i, center_offset, parent_center, search_center);
54  getLeafNodesRecurs(voxels, max_depth, tree->getNodeChild(node, i), depth+1, search_center, tree_center, tree, occupied);
55 
56  }
57  }
58  }
59  else {
60  if (tree->isNodeOccupied(node) == occupied){
61  double voxelSize = tree->getResolution() * pow(2., double(16 - depth));
62  voxels.push_back(std::make_pair(parent_center - tree_center, voxelSize));
63 
64  }
65  }
66  }
67 }
68 
69 
71 void getVoxelsRecurs(std::list<OcTreeVolume>& voxels,
72  unsigned int max_depth,
73  OcTreeNode* node, unsigned int depth,
74  const point3d& parent_center, const point3d& tree_center,
75  OcTree* tree){
76 
77  if ((depth <= max_depth) && (node != NULL) ) {
78  if (tree->nodeHasChildren(node) && (depth != max_depth)) {
79 
80  double center_offset = tree_center(0) / pow(2., (double) depth + 1);
81  point3d search_center;
82 
83  for (unsigned int i = 0; i < 8; i++) {
84  if (tree->nodeChildExists(node, i)) {
85  computeChildCenter(i, (float) center_offset, parent_center, search_center);
86  getVoxelsRecurs(voxels, max_depth, tree->getNodeChild(node, i), depth + 1, search_center, tree_center, tree);
87 
88  }
89  } // depth
90  }
91  double voxelSize = tree->getResolution() * pow(2., double(16 - depth));
92  voxels.push_back(std::make_pair(parent_center - tree_center, voxelSize));
93  }
94 }
95 
96 
98 void compareResults(const std::list<OcTreeVolume>& list_iterator, const std::list<OcTreeVolume>& list_depr){
99  EXPECT_EQ(list_iterator.size(), list_depr.size());
100  list<OcTreeVolume>::const_iterator list_it = list_iterator.begin();
101  list<OcTreeVolume>::const_iterator list_depr_it = list_depr.begin();
102 
103  for (; list_it != list_iterator.end(); ++list_it, ++list_depr_it){
104  EXPECT_NEAR(list_it->first.x(), list_depr_it->first.x(), 0.005);
105  EXPECT_NEAR(list_it->first.y(), list_depr_it->first.y(), 0.005);
106  EXPECT_NEAR(list_it->first.z(), list_depr_it->first.z(), 0.005);
107  }
108  std::cout << "Resulting lists (size "<< list_iterator.size() << ") identical\n";
109 }
110 
111 // for unique comparing, need to sort the lists:
113 {
114  return ( lhs.second < rhs.second
115  || (lhs.second == rhs.second &&
116  lhs.first.x() < rhs.first.x()
117  && lhs.first.y() < rhs.first.y()
118  && lhs.first.z() < rhs.first.z()));
119 }
120 
121 
122 double timediff(const timeval& start, const timeval& stop){
123  return (stop.tv_sec - start.tv_sec) + 1.0e-6 *(stop.tv_usec - start.tv_usec);
124 }
125 
127  //tree->expand();
128  // test complete tree (should be equal to no bbx)
129  OcTreeKey bbxMinKey, bbxMaxKey;
130  double temp_x,temp_y,temp_z;
131  tree->getMetricMin(temp_x,temp_y,temp_z);
132  octomap::point3d bbxMin = octomap::point3d(float(temp_x), float(temp_y), float(temp_z));
133 
134  tree->getMetricMax(temp_x,temp_y,temp_z);
135  octomap::point3d bbxMax = octomap::point3d(float(temp_x), float(temp_y), float(temp_z));
136 
137  EXPECT_TRUE(tree->coordToKeyChecked(bbxMin, bbxMinKey));
138  EXPECT_TRUE(tree->coordToKeyChecked(bbxMax, bbxMaxKey));
139 
140  OcTree::leaf_bbx_iterator it_bbx = tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey);
141  EXPECT_TRUE(it_bbx == tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey));
142  OcTree::leaf_bbx_iterator end_bbx = tree->end_leafs_bbx();
143  EXPECT_TRUE(end_bbx == tree->end_leafs_bbx());
144 
145  OcTree::leaf_iterator it = tree->begin_leafs();
146  EXPECT_TRUE(it == tree->begin_leafs());
147  OcTree::leaf_iterator end = tree->end_leafs();
148  EXPECT_TRUE(end == tree->end_leafs());
149 
150 
151  for( ; it!= end && it_bbx != end_bbx; ++it, ++it_bbx){
152  EXPECT_TRUE(it == it_bbx);
153  }
154  EXPECT_TRUE(it == end && it_bbx == end_bbx);
155 
156 
157  // now test an actual bounding box:
158  tree->expand(); // (currently only works properly for expanded tree (no multires)
159  bbxMin = point3d(-1, -1, - 1);
160  bbxMax = point3d(3, 2, 1);
161  EXPECT_TRUE(tree->coordToKeyChecked(bbxMin, bbxMinKey));
162  EXPECT_TRUE(tree->coordToKeyChecked(bbxMax, bbxMaxKey));
163 
164  typedef unordered_ns::unordered_map<OcTreeKey, double, OcTreeKey::KeyHash> KeyVolumeMap;
165 
166  KeyVolumeMap bbxVoxels;
167 
168  size_t count = 0;
169  for(OcTree::leaf_bbx_iterator it = tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey), end=tree->end_leafs_bbx();
170  it!= end; ++it)
171  {
172  count++;
173  OcTreeKey currentKey = it.getKey();
174  // leaf is actually a leaf:
175  EXPECT_FALSE(tree->nodeHasChildren(&(*it)));
176 
177  // leaf exists in tree:
178  OcTreeNode* node = tree->search(currentKey);
179  EXPECT_TRUE(node);
180  EXPECT_EQ(node, &(*it));
181  // all leafs are actually in the bbx:
182  for (unsigned i = 0; i < 3; ++i){
183 // if (!(currentKey[i] >= bbxMinKey[i] && currentKey[i] <= bbxMaxKey[i])){
184 // std::cout << "Key failed: " << i << " " << currentKey[i] << " "<< bbxMinKey[i] << " "<< bbxMaxKey[i]
185 // << "size: "<< it.getSize()<< std::endl;
186 // }
187  EXPECT_TRUE(currentKey[i] >= bbxMinKey[i] && currentKey[i] <= bbxMaxKey[i]);
188  }
189 
190  bbxVoxels.insert(std::pair<OcTreeKey,double>(currentKey, it.getSize()));
191  }
192  EXPECT_EQ(bbxVoxels.size(), count);
193  std::cout << "Bounding box traversed ("<< count << " leaf nodes)\n\n";
194 
195 
196  // compare with manual BBX check on all leafs:
197  for(OcTree::leaf_iterator it = tree->begin(), end=tree->end(); it!= end; ++it) {
198  OcTreeKey key = it.getKey();
199  if ( key[0] >= bbxMinKey[0] && key[0] <= bbxMaxKey[0]
200  && key[1] >= bbxMinKey[1] && key[1] <= bbxMaxKey[1]
201  && key[2] >= bbxMinKey[2] && key[2] <= bbxMaxKey[2])
202  {
203  KeyVolumeMap::iterator bbxIt = bbxVoxels.find(key);
204  EXPECT_FALSE(bbxIt == bbxVoxels.end());
205  EXPECT_TRUE(key == bbxIt->first);
206  EXPECT_EQ(it.getSize(), bbxIt->second);
207  }
208 
209  }
210 }
211 
212 int main(int argc, char** argv) {
213 
214 
215  //##############################################################
216 
217  string btFilename = "";
218  unsigned char maxDepth = 16;
219 
220 
221  // test timing:
222  timeval start;
223  timeval stop;
224  const unsigned char tree_depth(16);
225  const unsigned int tree_max_val(32768);
226  double time_it, time_depr;
227 
228  if (argc <= 1|| argc >3 || strcmp(argv[1], "-h") == 0){
229  printUsage(argv[0]);
230  }
231 
232  btFilename = std::string(argv[1]);
233  if (argc > 2){
234  maxDepth = (unsigned char)atoi(argv[2]);
235  }
236  maxDepth = std::min((unsigned char)16,maxDepth);
237 
238  if (maxDepth== 0)
239  maxDepth = tree_depth;
240 
241  // iterate over empty tree:
242  OcTree emptyTree(0.2);
243  EXPECT_EQ(emptyTree.size(), 0);
244  EXPECT_EQ(emptyTree.calcNumNodes(), 0);
245 
246  size_t iteratedNodes = 0;
247  OcTree::tree_iterator t_it = emptyTree.begin_tree(maxDepth);
248  OcTree::tree_iterator t_end = emptyTree.end_tree();
249  EXPECT_TRUE (t_it == t_end);
250  for( ; t_it != t_end; ++t_it){
251  iteratedNodes++;
252  }
253  EXPECT_EQ(iteratedNodes, 0);
254 
255  for(OcTree::leaf_iterator l_it = emptyTree.begin_leafs(maxDepth), l_end=emptyTree.end_leafs(); l_it!= l_end; ++l_it){
256  iteratedNodes++;
257  }
258  EXPECT_EQ(iteratedNodes, 0);
259 
260  octomap::point3d ptMinBBX(-0.1, -0.1, -0.1);
261  octomap::point3d ptMaxBBX(0.1, 0.1, 0.1);
262  for(OcTree::leaf_bbx_iterator l_it = emptyTree.begin_leafs_bbx(ptMinBBX, ptMaxBBX, maxDepth), l_end=emptyTree.end_leafs_bbx(); l_it!= l_end; ++l_it){
263  iteratedNodes++;
264  }
265  EXPECT_EQ(iteratedNodes, 0);
266 
267  octomap::OcTreeKey minBBX(10, 10, 16);
268  octomap::OcTreeKey maxBBX(10, 10, 18);
269  for(OcTree::leaf_bbx_iterator l_it = emptyTree.begin_leafs_bbx(minBBX, maxBBX, maxDepth), l_end=emptyTree.end_leafs_bbx(); l_it!= l_end; ++l_it){
270  iteratedNodes++;
271  }
272  EXPECT_EQ(iteratedNodes, 0);
273 
274 
275 
276 
277 
278 
279  cout << "\nReading OcTree file\n===========================\n";
280  OcTree* tree = new OcTree(btFilename);
281  if (tree->size()<= 1){
282  std::cout << "Error reading file, exiting!\n";
283  return 1;
284  }
285 
286  size_t count;
287  std::list<OcTreeVolume> list_depr;
288  std::list<OcTreeVolume> list_iterator;
289 
293  gettimeofday(&start, NULL); // start timer
294  size_t num_leafs_recurs = tree->getNumLeafNodes();
295  gettimeofday(&stop, NULL); // stop timer
296  time_depr = timediff(start, stop);
297 
298  gettimeofday(&start, NULL); // start timer
299  size_t num_leafs_it = 0;
300  for(OcTree::leaf_iterator it = tree->begin(), end=tree->end(); it!= end; ++it) {
301  num_leafs_it++;
302  }
303  gettimeofday(&stop, NULL); // stop timer
304  time_it = timediff(start, stop);
305  std::cout << "Number of leafs: " << num_leafs_it << " / " << num_leafs_recurs << ", times: "
306  <<time_it << " / " << time_depr << "\n========================\n\n";
307 
308 
312  point3d tree_center;
313  tree_center(0) = tree_center(1) = tree_center(2)
314  = (float) (((double) tree_max_val) * tree->getResolution());
315 
316  gettimeofday(&start, NULL); // start timer
317  getLeafNodesRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree, true);
318  gettimeofday(&stop, NULL); // stop timer
319  time_depr = timediff(start, stop);
320 
321  gettimeofday(&start, NULL); // start timer
322  for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it){
323  if(tree->isNodeOccupied(*it))
324  {
325  //count ++;
326  list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
327  }
328 
329  }
330  gettimeofday(&stop, NULL); // stop timer
331  time_it = timediff(start, stop);
332 
333  std::cout << "Occupied lists traversed, times: "
334  <<time_it << " / " << time_depr << "\n";
335  compareResults(list_iterator, list_depr);
336  std::cout << "========================\n\n";
337 
338 
342  list_iterator.clear();
343  list_depr.clear();
344  gettimeofday(&start, NULL); // start timer
345  for(OcTree::leaf_iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it) {
346  if(!tree->isNodeOccupied(*it))
347  list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
348  }
349  gettimeofday(&stop, NULL); // stop timer
350  time_it = timediff(start, stop);
351 
352  gettimeofday(&start, NULL); // start timer
353  getLeafNodesRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree, false);
354  gettimeofday(&stop, NULL); // stop timer
355  time_depr = timediff(start, stop);
356 
357  std::cout << "Free lists traversed, times: "
358  <<time_it << " / " << time_depr << "\n";
359  compareResults(list_iterator, list_depr);
360  std::cout << "========================\n\n";
361 
362 
363 
367  list_iterator.clear();
368  list_depr.clear();
369 
370  gettimeofday(&start, NULL); // start timer
371  getVoxelsRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree);
372  gettimeofday(&stop, NULL); // stop timer
373  time_depr = timediff(start, stop);
374 
375  gettimeofday(&start, NULL); // start timers
376  for(OcTree::tree_iterator it = tree->begin_tree(maxDepth), end=tree->end_tree();
377  it!= end; ++it){
378  //count ++;
379  //std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl;
380  list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
381  }
382  gettimeofday(&stop, NULL); // stop timer
383  time_it = timediff(start, stop);
384 
385  list_iterator.sort(OcTreeVolumeSortPredicate);
386  list_depr.sort(OcTreeVolumeSortPredicate);
387 
388  std::cout << "All inner lists traversed, times: "
389  <<time_it << " / " << time_depr << "\n";
390  compareResults(list_iterator, list_depr);
391  std::cout << "========================\n\n";
392 
393 
394 
395  // traverse all leaf nodes, timing:
396  gettimeofday(&start, NULL); // start timers
397  count = 0;
398  for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end();
399  it!= end; ++it){
400  // do something:
401  // std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl;
402  count++;
403  }
404 
405  gettimeofday(&stop, NULL); // stop timer
406  time_it = timediff(start, stop);
407 
408  std::cout << "Time to traverse all leafs at max depth " <<(unsigned int)maxDepth <<" ("<<count<<" nodes): "<< time_it << " s\n\n";
409 
410 
411 
412 
416  boundingBoxTest(tree);
417  boundingBoxTest(&emptyTree);
418 
419 
420 
421  // test tree with one node:
422  OcTree simpleTree(0.01);
423  simpleTree.updateNode(point3d(10, 10, 10), 5.0f);
424  for(OcTree::leaf_iterator it = simpleTree.begin_leafs(maxDepth), end=simpleTree.end_leafs(); it!= end; ++it) {
425  std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl;
426  }
427 
428 
429  std::cout << "Tests successful\n";
430 
431 
432  return 0;
433 }
434 
tree_iterator begin_tree(unsigned char maxDepth=0) const
const tree_iterator end_tree() const
bool OcTreeVolumeSortPredicate(const OcTreeVolume &lhs, const OcTreeVolume &rhs)
virtual void expand()
double timediff(const timeval &start, const timeval &stop)
size_t calcNumNodes() const
Traverses the tree to calculate the total number of nodes.
const iterator end() const
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
NODE * getNodeChild(NODE *node, unsigned int childIdx) const
std::pair< point3d, double > OcTreeVolume
A voxel defined by its center point3d and its side length.
Definition: octomap_types.h:57
const leaf_bbx_iterator end_leafs_bbx() const
#define EXPECT_NEAR(a, b, prec)
Definition: testing.h:27
void compareResults(const std::list< OcTreeVolume > &list_iterator, const std::list< OcTreeVolume > &list_depr)
compare two lists of octree nodes on equality
void getVoxelsRecurs(std::list< OcTreeVolume > &voxels, unsigned int max_depth, OcTreeNode *node, unsigned int depth, const point3d &parent_center, const point3d &tree_center, OcTree *tree)
mimics old deprecated behavior to compare against
int main(int argc, char **argv)
leaf_iterator begin_leafs(unsigned char maxDepth=0) const
#define EXPECT_FALSE(args)
Definition: testing.h:11
#define EXPECT_EQ(a, b)
Definition: testing.h:16
NODE * search(double x, double y, double z, unsigned int depth=0) const
iterator begin(unsigned char maxDepth=0) const
bool coordToKeyChecked(const point3d &coord, OcTreeKey &key) const
virtual void getMetricMin(double &x, double &y, double &z)
minimum value of the bounding box of all known space in x, y, z
void printUsage(char *self)
double getResolution() const
leaf_bbx_iterator begin_leafs_bbx(const OcTreeKey &min, const OcTreeKey &max, unsigned char maxDepth=0) const
size_t getNumLeafNodes() const
Traverses the tree to calculate the total number of leaf nodes.
This class represents a three-dimensional vector.
Definition: Vector3.h:50
virtual void getMetricMax(double &x, double &y, double &z)
maximum value of the bounding box of all known space in x, y, z
const leaf_iterator end_leafs() const
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:49
virtual size_t size() const
void boundingBoxTest(OcTree *tree)
void computeChildCenter(const unsigned int &pos, const float &center_offset, const point3d &parent_center, point3d &child_center)
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
queries whether a node is occupied according to the tree&#39;s parameter for "occupancy" ...
void getLeafNodesRecurs(std::list< OcTreeVolume > &voxels, unsigned int max_depth, OcTreeNode *node, unsigned int depth, const point3d &parent_center, const point3d &tree_center, OcTree *tree, bool occupied)
mimics old deprecated behavior to compare against
#define EXPECT_TRUE(args)
Definition: testing.h:6
bool nodeChildExists(const NODE *node, unsigned int childIdx) const
bool nodeHasChildren(const NODE *node) const


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Mon Feb 28 2022 22:58:06