test_color_tree.cpp
Go to the documentation of this file.
1 #include <octomap/octomap.h>
2 #include <octomap/ColorOcTree.h>
3 #include "testing.h"
4 
5 using namespace std;
6 using namespace octomap;
7 
8 
10  if (node != NULL) {
11  cout << "occupancy probability at " << query << ":\t " << node->getOccupancy() << endl;
12  cout << "color of node is: " << node->getColor()
13  << endl;
14  }
15  else
16  cout << "occupancy probability at " << query << ":\t is unknown" << endl;
17 }
18 
19 
20 int main(int /*argc*/, char** /*argv*/) {
21 
22  double res = 0.05; // create empty tree with resolution 0.05 (different from default 0.1 for test)
23  ColorOcTree tree (res);
24  // insert some measurements of occupied cells
25  for (int x=-20; x<20; x++) {
26  for (int y=-20; y<20; y++) {
27  for (int z=-20; z<20; z++) {
28  point3d endpoint ((float) x*0.05f+0.01f, (float) y*0.05f+0.01f, (float) z*0.05f+0.01f);
29  ColorOcTreeNode* n = tree.updateNode(endpoint, true);
30  n->setColor(z*5+100,x*5+100,y*5+100);
31  }
32  }
33  }
34 
35  // insert some measurements of free cells
36  for (int x=-30; x<30; x++) {
37  for (int y=-30; y<30; y++) {
38  for (int z=-30; z<30; z++) {
39  point3d endpoint ((float) x*0.02f+2.0f, (float) y*0.02f+2.0f, (float) z*0.02f+2.0f);
40  ColorOcTreeNode* n = tree.updateNode(endpoint, false);
41  n->setColor(255,255,0); // set color to yellow
42  }
43  }
44  }
45 
46  // set inner node colors
47  tree.updateInnerOccupancy();
48 
49  // should already be pruned
50  EXPECT_EQ(tree.size(), tree.calcNumNodes());
51  const size_t initialSize = tree.size();
52  EXPECT_EQ(initialSize, 1034);
53  tree.prune();
54  EXPECT_EQ(tree.size(), tree.calcNumNodes());
55  EXPECT_EQ(initialSize, tree.size());
56 
57  cout << endl;
58 
59  std::cout << "\nWriting to / from file\n===============================\n";
60  std::string filename ("simple_color_tree.ot");
61  std::cout << "Writing color tree to " << filename << std::endl;
62  // write color tree
63  EXPECT_TRUE(tree.write(filename));
64 
65 
66  // read tree file
67  cout << "Reading color tree from "<< filename <<"\n";
68  AbstractOcTree* read_tree = AbstractOcTree::read(filename);
69  EXPECT_TRUE(read_tree);
70  EXPECT_EQ(read_tree->getTreeType().compare(tree.getTreeType()), 0);
71  EXPECT_FLOAT_EQ(read_tree->getResolution(), tree.getResolution());
72  EXPECT_EQ(read_tree->size(), tree.size());
73  ColorOcTree* read_color_tree = dynamic_cast<ColorOcTree*>(read_tree);
74  EXPECT_TRUE(read_color_tree);
75 
76  EXPECT_TRUE(tree == *read_color_tree);
77 
78 
79  {
80  cout << "Performing some queries:" << endl;
81  // TODO: some more meaningful tests
82  point3d query (0., 0., 0.);
83  ColorOcTreeNode* result = tree.search (query);
84  ColorOcTreeNode* result2 = read_color_tree->search (query);
85  std::cout << "READ: ";
86  print_query_info(query, result);
87  std::cout << "WRITE: ";
88  print_query_info(query, result2);
89  EXPECT_TRUE(result);
90  EXPECT_TRUE(result2);
91  EXPECT_EQ(result->getColor(), result2->getColor());
92  EXPECT_EQ(result->getLogOdds(), result2->getLogOdds());
93 
94  query = point3d(-1.,-1.,-1.);
95  result = tree.search (query);
96  result2 = read_color_tree->search (query);
97  print_query_info(query, result);
98  std::cout << "READ: ";
99  print_query_info(query, result);
100  std::cout << "WRITE: ";
101  print_query_info(query, result2);
102  EXPECT_TRUE(result);
103  EXPECT_TRUE(result2);
104  EXPECT_EQ(result->getColor(), result2->getColor());
105  EXPECT_EQ(result->getLogOdds(), result2->getLogOdds());
106 
107  query = point3d(1.,1.,1.);
108  result = tree.search (query);
109  result2 = read_color_tree->search (query);
110  print_query_info(query, result);
111  std::cout << "READ: ";
112  print_query_info(query, result);
113  std::cout << "WRITE: ";
114  print_query_info(query, result2);
115  EXPECT_FALSE(result);
116  EXPECT_FALSE(result2);
117  }
118 
119  delete read_tree;
120  read_tree = NULL;
121 
122  {
123  std::cout << "\nPruning / expansion\n===============================\n";
124  EXPECT_EQ(initialSize, tree.size());
125  EXPECT_EQ(initialSize, tree.calcNumNodes());
126  std::cout << "Initial size: " << tree.size() << std::endl;
127 
128  // tree should already be pruned during insertion:
129  tree.prune();
130  EXPECT_EQ(initialSize, tree.size());
131  EXPECT_EQ(initialSize, tree.calcNumNodes());
132 
133  tree.expand();
134  std::cout << "Size after expansion: " << tree.size() << std::endl;
135  EXPECT_EQ(tree.size(), tree.calcNumNodes());
136 
137  // prune again, should be same as initial size
138 
139  tree.prune();
140  EXPECT_EQ(initialSize, tree.size());
141  EXPECT_EQ(initialSize, tree.calcNumNodes());
142 
143  }
144 
145  // delete / create some nodes
146  {
147  std::cout << "\nCreating / deleting nodes\n===============================\n";
148  size_t initialSize = tree.size();
149  EXPECT_EQ(initialSize, tree.calcNumNodes());
150  std::cout << "Initial size: " << initialSize << std::endl;
151 
152  point3d newCoord(-2.0, -2.0, -2.0);
153  ColorOcTreeNode* newNode = tree.updateNode(newCoord, true);
154  newNode->setColor(255,0,0);
155  EXPECT_TRUE(newNode != NULL);
156 
157  const size_t insertedSize = tree.size();
158  std::cout << "Size after one insertion: " << insertedSize << std::endl;
159  EXPECT_EQ(insertedSize, initialSize+6);
160  EXPECT_EQ(insertedSize, tree.calcNumNodes());
161 
162  // find parent of newly inserted node:
163  ColorOcTreeNode* parentNode = tree.search(newCoord, tree.getTreeDepth() -1);
164  EXPECT_TRUE(parentNode);
165  EXPECT_TRUE(tree.nodeHasChildren(parentNode));
166 
167  // only one child exists:
168  EXPECT_TRUE(tree.nodeChildExists(parentNode, 0));
169  for (size_t i = 1; i < 8; ++i){
170  EXPECT_FALSE(tree.nodeChildExists(parentNode, i));
171  }
172 
173  tree.deleteNodeChild(parentNode, 0);
174  EXPECT_EQ(tree.size(), tree.calcNumNodes());
175  EXPECT_EQ(tree.size(), insertedSize - 1);
176 
177  tree.prune();
178  EXPECT_EQ(tree.size(), tree.calcNumNodes());
179  EXPECT_EQ(tree.size(), insertedSize - 1);
180 
181  tree.expandNode(parentNode);
182  EXPECT_EQ(tree.size(), tree.calcNumNodes());
183  EXPECT_EQ(tree.size(), insertedSize + 7);
184 
185  EXPECT_TRUE(tree.pruneNode(parentNode));
186  EXPECT_EQ(tree.size(), tree.calcNumNodes());
187  EXPECT_EQ(tree.size(), insertedSize - 1);
188 
189  EXPECT_TRUE(tree.write("simple_color_tree_ed.ot"));
190 
191  }
192 
193  return 0;
194 }
octomap::ColorOcTree::getTreeType
std::string getTreeType() const
returns actual class name as string for identification
Definition: ColorOcTree.h:118
ColorOcTree.h
octomap::OccupancyOcTreeBase::updateNode
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
octomap::ColorOcTree::pruneNode
virtual bool pruneNode(ColorOcTreeNode *node)
Definition: ColorOcTree.cpp:107
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)
octomap::ColorOcTreeNode::setColor
void setColor(Color c)
Definition: ColorOcTree.h:81
octomap::OcTreeNode::getLogOdds
float getLogOdds() const
Definition: OcTreeNode.h:68
octomap::ColorOcTree
Definition: ColorOcTree.h:108
octomap::ColorOcTree::updateInnerOccupancy
void updateInnerOccupancy()
Definition: ColorOcTree.cpp:188
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::OcTreeBaseImpl::getResolution
double getResolution() const
Definition: OcTreeBaseImpl.h:109
octomap::AbstractOcTree::getResolution
virtual double getResolution() const =0
EXPECT_FLOAT_EQ
#define EXPECT_FLOAT_EQ(a, b)
Definition: testing.h:22
octomap::AbstractOcTree
Definition: AbstractOcTree.h:50
octomap::ColorOcTreeNode
Definition: ColorOcTree.h:48
octomap::OcTreeBaseImpl::nodeChildExists
bool nodeChildExists(const NODE *node, unsigned int childIdx) const
octomap::OcTreeBaseImpl::nodeHasChildren
bool nodeHasChildren(const NODE *node) const
octomap::OcTreeBaseImpl::prune
virtual void prune()
octomap::AbstractOcTree::getTreeType
virtual std::string getTreeType() const =0
returns actual class name as string for identification
octomap.h
octomap::AbstractOcTree::size
virtual size_t size() const =0
octomap::ColorOcTreeNode::getColor
Color getColor() const
Definition: ColorOcTree.h:80
main
int main(int, char **)
Definition: test_color_tree.cpp:20
print_query_info
void print_query_info(point3d query, ColorOcTreeNode *node)
Definition: test_color_tree.cpp:9
octomap::OcTreeNode::getOccupancy
double getOccupancy() const
Definition: OcTreeNode.h:65
octomap
octomap::point3d
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:49
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 Tue Dec 12 2023 03:39:40