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 
octomap::OccupancyOcTreeBase::insertRay
virtual bool insertRay(const point3d &origin, const point3d &end, double maxrange=-1.0, bool lazy_eval=false)
octomap::OccupancyOcTreeBase::updateNode
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
octomap::Pointcloud
Definition: Pointcloud.h:47
octomap::OccupancyOcTreeBase::changedKeysBegin
KeyBoolMap::const_iterator changedKeysBegin() const
Definition: OccupancyOcTreeBase.h:363
octomap::Pointcloud::push_back
void push_back(float x, float y, float z)
Definition: Pointcloud.h:61
main
int main(int, char **)
Definition: test_changedkeys.cpp:56
octomath::Vector3::rotate_IP
Vector3 & rotate_IP(double roll, double pitch, double yaw)
Definition: Vector3.cpp:41
octomap::OcTreeBaseImpl::expand
virtual void expand()
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
DEG2RAD
#define DEG2RAD(x)
Definition: Utils.h:47
octomap::OcTree
Definition: OcTree.h:49
octomap::OcTreeBaseImpl::end_tree
const tree_iterator end_tree() const
Definition: OcTreeBaseImpl.h:350
Utils.h
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
printChanges
void printChanges(OcTree &tree)
Definition: test_changedkeys.cpp:9
octomap::OcTreeNode
Definition: OcTreeNode.h:55
octomap::OcTreeBaseImpl::prune
virtual void prune()
octomap.h
octomap::OcTreeBaseImpl::begin_tree
tree_iterator begin_tree(unsigned char maxDepth=0) const
Definition: OcTreeBaseImpl.h:348
octomap::OccupancyOcTreeBase::changedKeysEnd
KeyBoolMap::const_iterator changedKeysEnd() const
Iterator to traverse all keys of changed nodes.
Definition: OccupancyOcTreeBase.h:366
octomap::OccupancyOcTreeBase::enableChangeDetection
void enableChangeDetection(bool enable)
track or ignore changes while inserting scans (default: ignore)
Definition: OccupancyOcTreeBase.h:353
octomap
octomap::point3d
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:49
octomap::OccupancyOcTreeBase::insertPointCloud
virtual void insertPointCloud(const Pointcloud &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false, bool discretize=false)


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Tue Dec 12 2023 03:39:40