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 (node->hasChildren() && (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 (node->childExists(i)) {
52 
53  computeChildCenter(i, center_offset, parent_center, search_center);
54  getLeafNodesRecurs(voxels, max_depth, node->getChild(i), depth+1, search_center, tree_center, tree, occupied);
55 
56  } // GetChild
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  double resolution){
76 
77  if ((depth <= max_depth) && (node != NULL) ) {
78  if (node->hasChildren() && (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 (node->childExists(i)) {
85  computeChildCenter(i, (float) center_offset, parent_center, search_center);
86  getVoxelsRecurs(voxels, max_depth, node->getChild(i), depth + 1, search_center, tree_center, resolution);
87 
88  }
89  } // depth
90  }
91  double voxelSize = resolution * 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(it->hasChildren());
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 
261 
262 
263 
264  cout << "\nReading OcTree file\n===========================\n";
265  OcTree* tree = new OcTree(btFilename);
266  if (tree->size()<= 1){
267  std::cout << "Error reading file, exiting!\n";
268  return 1;
269  }
270 
271  size_t count;
272  std::list<OcTreeVolume> list_depr;
273  std::list<OcTreeVolume> list_iterator;
274 
278  gettimeofday(&start, NULL); // start timer
279  size_t num_leafs_recurs = tree->getNumLeafNodes();
280  gettimeofday(&stop, NULL); // stop timer
281  time_depr = timediff(start, stop);
282 
283  gettimeofday(&start, NULL); // start timer
284  size_t num_leafs_it = 0;
285  for(OcTree::leaf_iterator it = tree->begin(), end=tree->end(); it!= end; ++it) {
286  num_leafs_it++;
287  }
288  gettimeofday(&stop, NULL); // stop timer
289  time_it = timediff(start, stop);
290  std::cout << "Number of leafs: " << num_leafs_it << " / " << num_leafs_recurs << ", times: "
291  <<time_it << " / " << time_depr << "\n========================\n\n";
292 
293 
297  point3d tree_center;
298  tree_center(0) = tree_center(1) = tree_center(2)
299  = (float) (((double) tree_max_val) * tree->getResolution());
300 
301  gettimeofday(&start, NULL); // start timer
302  getLeafNodesRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree, true);
303  gettimeofday(&stop, NULL); // stop timer
304  time_depr = timediff(start, stop);
305 
306  gettimeofday(&start, NULL); // start timer
307  for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it){
308  if(tree->isNodeOccupied(*it))
309  {
310  //count ++;
311  list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
312  }
313 
314  }
315  gettimeofday(&stop, NULL); // stop timer
316  time_it = timediff(start, stop);
317 
318  std::cout << "Occupied lists traversed, times: "
319  <<time_it << " / " << time_depr << "\n";
320  compareResults(list_iterator, list_depr);
321  std::cout << "========================\n\n";
322 
323 
327  list_iterator.clear();
328  list_depr.clear();
329  gettimeofday(&start, NULL); // start timer
330  for(OcTree::leaf_iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it) {
331  if(!tree->isNodeOccupied(*it))
332  list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
333  }
334  gettimeofday(&stop, NULL); // stop timer
335  time_it = timediff(start, stop);
336 
337  gettimeofday(&start, NULL); // start timer
338  getLeafNodesRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree, false);
339  gettimeofday(&stop, NULL); // stop timer
340  time_depr = timediff(start, stop);
341 
342  std::cout << "Free lists traversed, times: "
343  <<time_it << " / " << time_depr << "\n";
344  compareResults(list_iterator, list_depr);
345  std::cout << "========================\n\n";
346 
347 
348 
352  list_iterator.clear();
353  list_depr.clear();
354 
355  gettimeofday(&start, NULL); // start timer
356  getVoxelsRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree->getResolution());
357  gettimeofday(&stop, NULL); // stop timer
358  time_depr = timediff(start, stop);
359 
360  gettimeofday(&start, NULL); // start timers
361  for(OcTree::tree_iterator it = tree->begin_tree(maxDepth), end=tree->end_tree();
362  it!= end; ++it){
363  //count ++;
364  //std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl;
365  list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
366  }
367  gettimeofday(&stop, NULL); // stop timer
368  time_it = timediff(start, stop);
369 
370  list_iterator.sort(OcTreeVolumeSortPredicate);
371  list_depr.sort(OcTreeVolumeSortPredicate);
372 
373  std::cout << "All inner lists traversed, times: "
374  <<time_it << " / " << time_depr << "\n";
375  compareResults(list_iterator, list_depr);
376  std::cout << "========================\n\n";
377 
378 
379 
380  // traverse all leaf nodes, timing:
381  gettimeofday(&start, NULL); // start timers
382  count = 0;
383  for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end();
384  it!= end; ++it){
385  // do something:
386  // std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl;
387  count++;
388  }
389 
390  gettimeofday(&stop, NULL); // stop timer
391  time_it = timediff(start, stop);
392 
393  std::cout << "Time to traverse all leafs at max depth " <<(unsigned int)maxDepth <<" ("<<count<<" nodes): "<< time_it << " s\n\n";
394 
395 
396 
397 
401  boundingBoxTest(tree);
402  boundingBoxTest(&emptyTree);
403 
404 
405 
406  // test tree with one node:
407  OcTree simpleTree(0.01);
408  simpleTree.updateNode(point3d(10, 10, 10), 5.0f);
409  for(OcTree::leaf_iterator it = simpleTree.begin_leafs(maxDepth), end=simpleTree.end_leafs(); it!= end; ++it) {
410  std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl;
411  }
412 
413 
414  std::cout << "Tests successful\n";
415 
416 
417  return 0;
418 }
419 
size_t getNumLeafNodes() const
Traverses the tree to calculate the total number of leaf nodes.
bool OcTreeVolumeSortPredicate(const OcTreeVolume &lhs, const OcTreeVolume &rhs)
virtual void expand()
double timediff(const timeval &start, const timeval &stop)
iterator begin(unsigned char maxDepth=0) const
const tree_iterator end_tree() const
NODE * getRoot() const
OcTreeNode * getChild(unsigned int i)
Definition: OcTreeNode.h:64
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
void getVoxelsRecurs(std::list< OcTreeVolume > &voxels, unsigned int max_depth, OcTreeNode *node, unsigned int depth, const point3d &parent_center, const point3d &tree_center, double resolution)
mimics old deprecated behavior to compare against
const iterator end() const
const leaf_iterator end_leafs() const
leaf_bbx_iterator begin_leafs_bbx(const OcTreeKey &min, const OcTreeKey &max, unsigned char maxDepth=0) const
NODE * search(double x, double y, double z, unsigned int depth=0) const
std::pair< point3d, double > OcTreeVolume
A voxel defined by its center point3d and its side length.
Definition: octomap_types.h:56
bool childExists(unsigned int i) const
bool coordToKeyChecked(const point3d &coord, OcTreeKey &key) 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
int main(int argc, char **argv)
#define EXPECT_FALSE(args)
Definition: testing.h:11
#define EXPECT_EQ(a, b)
Definition: testing.h:16
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)
This class represents a three-dimensional vector.
Definition: Vector3.h:50
tree_iterator begin_tree(unsigned char maxDepth=0) const
virtual void getMetricMax(double &x, double &y, double &z)
maximum value of the bounding box of all known space in x, y, z
double getResolution() const
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:48
leaf_iterator begin_leafs(unsigned char maxDepth=0) const
void boundingBoxTest(OcTree *tree)
void computeChildCenter(const unsigned int &pos, const float &center_offset, const point3d &parent_center, point3d &child_center)
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
const leaf_bbx_iterator end_leafs_bbx() const
#define EXPECT_TRUE(args)
Definition: testing.h:6
size_t calcNumNodes() const
Traverses the tree to calculate the total number of nodes.
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
queries whether a node is occupied according to the tree&#39;s parameter for "occupancy" ...
virtual size_t size() const
bool hasChildren() const


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Mon Jun 10 2019 14:00:13