test_changedkeys.cpp
Go to the documentation of this file.
1 
2 #include <stdio.h>
3 #include <octomap/octomap.h>
4 #include <octomap/math/Utils.h>
5 
6 using namespace std;
7 using namespace octomap;
8 
9 void printChanges(OcTree& tree){
10  unsigned int changedOccupied = 0;
11  unsigned int changedFree = 0;
12  unsigned int actualOccupied = 0;
13  unsigned int actualFree = 0;
14  unsigned int missingChanged = 0;
15 
16  tree.expand();
17 
18  // iterate through the changed nodes
19  KeyBoolMap::const_iterator it;
20  for (it=tree.changedKeysBegin(); it!=tree.changedKeysEnd(); it++) {
21  OcTreeNode* node = tree.search(it->first);
22  if (node != NULL) {
23  if (tree.isNodeOccupied(node)) {
24  changedOccupied += 1;
25  }
26  else {
27  changedFree += 1;
28  }
29  } else {
30  missingChanged +=1;
31  }
32  }
33 
34 
35  // iterate through the entire tree
36  for(OcTree::tree_iterator it=tree.begin_tree(),
37  end=tree.end_tree(); it!= end; ++it) {
38  if (it.isLeaf()) {
39  if (tree.isNodeOccupied(*it)) {
40  actualOccupied += 1;
41  }
42  else {
43  actualFree += 1;
44  }
45  }
46  }
47 
48  cout << "change detection: " << changedOccupied << " occ; " << changedFree << " free; "<< missingChanged << " missing" << endl;
49  cout << "actual: " << actualOccupied << " occ; " << actualFree << " free; " << endl;
50 
51  tree.prune();
52 }
53 
54 
55 
56 int main(int argc, char** argv) {
57 
58 
59  //##############################################################
60 
61  OcTree tree (0.05);
62  tree.enableChangeDetection(true);
63 
64  point3d origin (0.01f, 0.01f, 0.02f);
65  point3d point_on_surface (4.01f,0.01f,0.01f);
66  tree.insertRay(origin, point_on_surface);
67  printChanges(tree);
68  tree.updateNode(point3d(2.01f, 0.01f, 0.01f), 2.0f);
69  printChanges(tree);
70  tree.updateNode(point3d(2.01f, 0.01f, 0.01f), -2.0f);
71  printChanges(tree);
72 
73  cout << "generating spherical scan at " << origin << " ..." << endl;
74 
75  for (int i=-100; i<101; i++) {
76  Pointcloud cloud;
77  for (int j=-100; j<101; j++) {
78  point3d rotated = point_on_surface;
79  rotated.rotate_IP(0, DEG2RAD(i*0.5), DEG2RAD(j*0.5));
80  cloud.push_back(rotated);
81  }
82 
83  // insert in global coordinates:
84  tree.insertPointCloud(cloud, origin, -1);
85  }
86 
87  printChanges(tree);
88 
89 
90  cout << "done." << endl;
91 
92  return 0;
93 }
94 
tree_iterator begin_tree(unsigned char maxDepth=0) const
const tree_iterator end_tree() const
virtual void expand()
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
KeyBoolMap::const_iterator changedKeysEnd() const
Iterator to traverse all keys of changed nodes.
void push_back(float x, float y, float z)
Definition: Pointcloud.h:61
virtual void insertPointCloud(const Pointcloud &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false, bool discretize=false)
Vector3 & rotate_IP(double roll, double pitch, double yaw)
Definition: Vector3.cpp:41
void printChanges(OcTree &tree)
NODE * search(double x, double y, double z, unsigned int depth=0) const
virtual bool insertRay(const point3d &origin, const point3d &end, double maxrange=-1.0, bool lazy_eval=false)
int main(int argc, char **argv)
KeyBoolMap::const_iterator changedKeysBegin() const
This class represents a three-dimensional vector.
Definition: Vector3.h:50
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:49
#define DEG2RAD(x)
Definition: Utils.h:47
void enableChangeDetection(bool enable)
track or ignore changes while inserting scans (default: ignore)
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
queries whether a node is occupied according to the tree&#39;s parameter for "occupancy" ...
virtual void prune()


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