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


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