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); // set color to red
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  cout << endl;
50
51
52  std::string filename ("simple_color_tree.ot");
53  std::cout << "Writing color tree to " << filename << std::endl;
54  // write color tree
55  EXPECT_TRUE(tree.write(filename));
56
57
59  cout << "Reading color tree from "<< filename <<"\n";
67
68
69  cout << "Performing some queries:" << endl;
70
71  {
72  point3d query (0., 0., 0.);
73  ColorOcTreeNode* result = tree.search (query);
74  ColorOcTreeNode* result2 = read_color_tree->search (query);
76  print_query_info(query, result);
77  std::cout << "WRITE: ";
78  print_query_info(query, result2);
79  EXPECT_TRUE(result);
80  EXPECT_TRUE(result2);
81  EXPECT_EQ(result->getColor(), result2->getColor());
82  EXPECT_EQ(result->getLogOdds(), result2->getLogOdds());
83
84  query = point3d(-1.,-1.,-1.);
85  result = tree.search (query);
87  print_query_info(query, result);
89  print_query_info(query, result);
90  std::cout << "WRITE: ";
91  print_query_info(query, result2);
92  EXPECT_TRUE(result);
93  EXPECT_TRUE(result2);
94  EXPECT_EQ(result->getColor(), result2->getColor());
95  EXPECT_EQ(result->getLogOdds(), result2->getLogOdds());
96
97  query = point3d(1.,1.,1.);
98  result = tree.search (query);
100  print_query_info(query, result);
102  print_query_info(query, result);
103  std::cout << "WRITE: ";
104  print_query_info(query, result2);
105  EXPECT_FALSE(result);
106  EXPECT_FALSE(result2);
107
108  }
109
111
112  return 0;
113 }
int main(int argc, char **argv)
virtual std::string getTreeType() const =0
returns actual class name as string for identification
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
std::string getTreeType() const
returns actual class name as string for identification
Definition: ColorOcTree.h:126
NODE * search(double x, double y, double z, unsigned int depth=0) const
virtual size_t size() const =0
void print_query_info(point3d query, ColorOcTreeNode *node)
void setColor(Color c)
Definition: ColorOcTree.h:89
#define EXPECT_FALSE(args)
Definition: testing.h:11
#define EXPECT_FLOAT_EQ(a, b)
Definition: testing.h:22
#define EXPECT_EQ(a, b)
Definition: testing.h:16
virtual double getResolution() const =0
float getLogOdds() const
Definition: OcTreeNode.h:77
bool write(const std::string &filename) const
Write file header and complete tree to file (serialization)
double getOccupancy() const
Definition: OcTreeNode.h:74
This class represents a three-dimensional vector.
Definition: Vector3.h:50
double getResolution() const
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:48
#define EXPECT_TRUE(args)
Definition: testing.h:6
virtual size_t size() const
Color getColor() const
Definition: ColorOcTree.h:88

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