test_io.cpp
Go to the documentation of this file.
1 #include <stdio.h>
2 #include <string>
3 
4 #include <octomap/OcTree.h>
5 #include <octomap/ColorOcTree.h>
7 #include <octomap/math/Utils.h>
8 #include "testing.h"
9 
10 using namespace std;
11 using namespace octomap;
12 using namespace octomath;
13 
14 int main(int argc, char** argv) {
15 
16  if (argc != 2){
17  std::cerr << "Error: you need to specify a testfile (.bt) as argument to read" << std::endl;
18  return 1; // exit 1 means failure
19  }
20 
21  std::cout << "Testing empty OcTree...\n";
22  //empty tree
23  {
24  OcTree emptyTree(0.999);
25  EXPECT_EQ(emptyTree.size(), 0);
26  EXPECT_TRUE(emptyTree.writeBinary("empty.bt"));
27  EXPECT_TRUE(emptyTree.write("empty.ot"));
28 
29  OcTree emptyReadTree(0.2);
30  EXPECT_TRUE(emptyReadTree.readBinary("empty.bt"));
31  EXPECT_EQ(emptyReadTree.size(), 0);
32  EXPECT_TRUE(emptyTree == emptyReadTree);
33 
34 
35  AbstractOcTree* readTreeAbstract = AbstractOcTree::read("empty.ot");
36  EXPECT_TRUE(readTreeAbstract);
37  OcTree* readTreeOt = dynamic_cast<OcTree*>(readTreeAbstract);
38  EXPECT_TRUE(readTreeOt);
39  EXPECT_EQ(readTreeOt->size(), 0);
40  EXPECT_TRUE(emptyTree == *readTreeOt);
41  delete readTreeOt;
42  }
43 
44  std::cout << "Testing reference OcTree from file ...\n";
45  string filename = string(argv[1]);
46 
47  {
48  string filenameOt = "test_io_file.ot";
49  string filenameBtOut = "test_io_file.bt";
50  string filenameBtCopyOut = "test_io_file_copy.bt";
51 
52  // read reference tree from input file
53  OcTree tree (0.1);
54  EXPECT_TRUE (tree.readBinary(filename));
55 
56  std::cout << " Copy Constructor / assignment / ==\n";
57  // test copy constructor / assignment:
58  OcTree* treeCopy = new OcTree(tree);
59  EXPECT_TRUE(tree == *treeCopy);
60  EXPECT_TRUE(treeCopy->writeBinary(filenameBtCopyOut));
61 
62  // change a tree property, trees must be different afterwards
63  treeCopy->setResolution(tree.getResolution()*2.0);
64  EXPECT_FALSE(tree == *treeCopy);
65  treeCopy->setResolution(tree.getResolution());
66  EXPECT_TRUE(tree == *treeCopy);
67 
68  // flip one value, trees must be different afterwards:
69  point3d pt(0.5, 0.5, 0.5);
70  OcTreeNode* node = treeCopy->search(pt);
71  if (node && treeCopy->isNodeOccupied(node))
72  treeCopy->updateNode(pt, false);
73  else
74  treeCopy->updateNode(pt, true);
75 
76  EXPECT_FALSE(tree == *treeCopy);
77 
78 
79  delete treeCopy;
80 
81  std::cout << " Swap\n";
82  // test swap:
83  OcTree emptyT(tree.getResolution());
84  OcTree emptySw(emptyT);
85  OcTree otherSw(tree);
86  emptySw.swapContent(otherSw);
87  EXPECT_FALSE(emptyT == emptySw);
88  EXPECT_TRUE(emptySw == tree);
89  EXPECT_TRUE(otherSw == emptyT);
90 
91 
92  // write again to bt, read & compare
93  EXPECT_TRUE(tree.writeBinary(filenameBtOut));
94  OcTree readTreeBt(0.1);
95  EXPECT_TRUE(readTreeBt.readBinary(filenameBtOut));
96  EXPECT_TRUE(tree == readTreeBt);
97 
98  std::cout <<" Write to .ot / read through AbstractOcTree\n";
99  // now write to .ot, read & compare
100  EXPECT_TRUE(tree.write(filenameOt));
101 
102  AbstractOcTree* readTreeAbstract = AbstractOcTree::read(filenameOt);
103  EXPECT_TRUE(readTreeAbstract);
104 
105  OcTree* readTreeOt = dynamic_cast<OcTree*>(readTreeAbstract);
106  EXPECT_TRUE(readTreeOt);
107  EXPECT_TRUE(tree == *readTreeOt);
108 
109  // sanity test for "==": flip one node, compare again
110  point3d coord(0.1f, 0.1f, 0.1f);
111  node = readTreeOt->search(coord);
112  if (node && readTreeOt->isNodeOccupied(node))
113  readTreeOt->updateNode(coord, false);
114  else
115  readTreeOt->updateNode(coord, true);
116 
117  EXPECT_FALSE(tree == *readTreeOt);
118 
119  delete readTreeOt;
120  }
121 
122  // Test for tree headers and IO factory registry (color)
123  {
124  std::cout << "Testing ColorOcTree...\n";
125 
126  double res = 0.02;
127  std::string filenameColor = "test_io_color_file.ot";
128  ColorOcTree colorTree(res);
129  EXPECT_EQ(colorTree.getTreeType(), "ColorOcTree");
130  ColorOcTreeNode* colorNode = colorTree.updateNode(point3d(0.0, 0.0, 0.0), true);
131  ColorOcTreeNode::Color color_red(255, 0, 0);
132  colorNode->setColor(color_red);
133  colorTree.setNodeColor(0.0, 0.0, 0.0, 255, 0, 0);
134  colorTree.updateNode(point3d(0.1f, 0.1f, 0.1f), true);
135  colorTree.setNodeColor(0.1f, 0.1f, 0.1f, 0, 0, 255);
136 
137  EXPECT_TRUE(colorTree.write(filenameColor));
138  AbstractOcTree* readTreeAbstract = AbstractOcTree::read(filenameColor);
139  EXPECT_TRUE(readTreeAbstract);
140  EXPECT_EQ(colorTree.getTreeType(), readTreeAbstract->getTreeType());
141  ColorOcTree* readColorTree = dynamic_cast<ColorOcTree*>(readTreeAbstract);
142  EXPECT_TRUE(readColorTree);
143  EXPECT_TRUE(colorTree == *readColorTree);
144  colorNode = colorTree.search(0.0, 0.0, 0.0);
145  EXPECT_TRUE(colorNode);
146  EXPECT_EQ(colorNode->getColor(), color_red);
147  delete readColorTree;
148  }
149 
150  // Test for tree headers and IO factory registry (stamped)
151  {
152  std::cout << "Testing OcTreeStamped...\n";
153  double res = 0.05;
154  std::string filenameStamped = "test_io_stamped_file.ot";
155  OcTreeStamped stampedTree(res);
156  EXPECT_EQ(stampedTree.getTreeType(), "OcTreeStamped");
157  // TODO: add / modify some stamped nodes
158  //ColorOcTreeNode* colorNode = colorTree.updateNode(point3d(0.0, 0.0, 0.0), true);
159  //ColorOcTreeNode::Color color_red(255, 0, 0);
160  //colorNode->setColor(color_red);
161  //colorTree.setNodeColor(0.0, 0.0, 0.0, 255, 0, 0);
162  //colorTree.updateNode(point3d(0.1f, 0.1f, 0.1f), true);
163  //colorTree.setNodeColor(0.1f, 0.1f, 0.1f, 0, 0, 255);
164 
165  EXPECT_TRUE(stampedTree.write(filenameStamped));
166  AbstractOcTree* readTreeAbstract = AbstractOcTree::read(filenameStamped);
167  EXPECT_TRUE(readTreeAbstract);
168  EXPECT_EQ(stampedTree.getTreeType(), readTreeAbstract->getTreeType());
169  OcTreeStamped* readStampedTree = dynamic_cast<OcTreeStamped*>(readTreeAbstract);
170  EXPECT_TRUE(readStampedTree);
171  EXPECT_TRUE(stampedTree == *readStampedTree);
172  //colorNode = colorTree.search(0.0, 0.0, 0.0);
173  //EXPECT_TRUE(colorNode);
174  //EXPECT_EQ(colorNode->getColor(), color_red);
175 
176  delete readStampedTree;
177  }
178 
179 
180  std::cerr << "Test successful.\n";
181  return 0;
182 }
void setResolution(double r)
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
ColorOcTreeNode * setNodeColor(const OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
Definition: ColorOcTree.cpp:96
#define EXPECT_FALSE(args)
Definition: testing.h:11
#define EXPECT_EQ(a, b)
Definition: testing.h:16
NODE * search(double x, double y, double z, unsigned int depth=0) const
int main(int argc, char **argv)
Definition: test_io.cpp:14
double getResolution() const
bool writeBinary(const std::string &filename)
std::string getTreeType() const
returns actual class name as string for identification
Definition: ColorOcTree.h:118
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
virtual size_t size() const
bool write(const std::string &filename) const
Write file header and complete tree to file (serialization)
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
queries whether a node is occupied according to the tree&#39;s parameter for "occupancy" ...
#define EXPECT_TRUE(args)
Definition: testing.h:6
std::string getTreeType() const
returns actual class name as string for identification
Definition: OcTreeStamped.h:88


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Fri Jun 7 2019 21:17:32