test_io.cpp
Go to the documentation of this file.
00001 #include <stdio.h>
00002 #include <string>
00003 
00004 #include <octomap/OcTree.h>
00005 #include <octomap/ColorOcTree.h>
00006 #include <octomap/math/Utils.h>
00007 #include "testing.h"
00008  
00009 using namespace std;
00010 using namespace octomap;
00011 using namespace octomath;
00012 
00013 int main(int argc, char** argv) {
00014 
00015   if (argc != 2){
00016     std::cerr << "Error: you need to specify a testfile (.bt) as argument to read" << std::endl;
00017     return 1; // exit 1 means failure
00018   }
00019 
00020   //empty tree
00021   OcTree emptyTree(0.999);
00022   EXPECT_EQ(emptyTree.size(), 0);
00023   EXPECT_TRUE(emptyTree.writeBinary("empty.bt"));
00024   EXPECT_TRUE(emptyTree.write("empty.ot"));
00025 
00026   OcTree emptyReadTree(0.2);
00027   EXPECT_TRUE(emptyReadTree.readBinary("empty.bt"));
00028   EXPECT_EQ(emptyReadTree.size(), 0);
00029   EXPECT_TRUE(emptyTree == emptyReadTree);
00030 
00031   EXPECT_TRUE(emptyReadTree.read("empty.ot"));
00032   EXPECT_EQ(emptyReadTree.size(), 0);
00033   EXPECT_TRUE(emptyTree == emptyReadTree);
00034 
00035 
00036 
00037   string filename = string(argv[1]);
00038 
00039   string filenameOt = "test_io_file.ot";
00040   string filenameBtOut = "test_io_file.bt";
00041   string filenameBtCopyOut = "test_io_file_copy.bt";
00042 
00043   // read reference tree from input file
00044   OcTree tree (0.1);
00045   EXPECT_TRUE (tree.readBinary(filename));
00046 
00047   // test copy constructor / assignment:
00048   OcTree* treeCopy = new OcTree(tree);
00049   EXPECT_TRUE(tree == *treeCopy);
00050   EXPECT_TRUE(treeCopy->writeBinary(filenameBtCopyOut));
00051 
00052   // change a tree property, trees must be different afterwards
00053   treeCopy->setResolution(tree.getResolution()*2.0);
00054   EXPECT_FALSE(tree == *treeCopy);
00055   treeCopy->setResolution(tree.getResolution());
00056   EXPECT_TRUE(tree == *treeCopy);
00057 
00058   // flip one value, trees must be different afterwards:
00059   point3d pt(0.5, 0.5, 0.5);
00060   OcTreeNode* node = treeCopy->search(pt);
00061   if (node && treeCopy->isNodeOccupied(node))
00062     treeCopy->updateNode(pt, false);
00063   else
00064     treeCopy->updateNode(pt, true);
00065 
00066   EXPECT_FALSE(tree == *treeCopy);
00067 
00068 
00069   delete treeCopy;
00070 
00071   // test swap:
00072   OcTree emptyT(tree.getResolution());
00073   OcTree emptySw(emptyT);
00074   OcTree otherSw(tree);
00075   emptySw.swapContent(otherSw);
00076   EXPECT_FALSE(emptyT == emptySw);
00077   EXPECT_TRUE(emptySw == tree);
00078   EXPECT_TRUE(otherSw == emptyT);
00079 
00080 
00081   // write again to bt, read & compare
00082   EXPECT_TRUE(tree.writeBinary(filenameBtOut));
00083   OcTree readTreeBt(0.1);
00084   EXPECT_TRUE(readTreeBt.readBinary(filenameBtOut));
00085   EXPECT_TRUE(tree == readTreeBt);
00086 
00087   // now write to .ot, read & compare
00088   EXPECT_TRUE(tree.write(filenameOt));
00089 
00090   AbstractOcTree* readTreeAbstract = AbstractOcTree::read(filenameOt);
00091   EXPECT_TRUE(readTreeAbstract);
00092 
00093   OcTree* readTreeOt = dynamic_cast<OcTree*>(readTreeAbstract);
00094   EXPECT_TRUE(readTreeOt);
00095   EXPECT_TRUE(tree == *readTreeOt);
00096 
00097   // sanity test for "==": flip one node, compare again
00098   point3d coord(0.1, 0.1, 0.1);
00099   node = readTreeOt->search(coord);
00100   if (node && readTreeOt->isNodeOccupied(node))
00101     readTreeOt->updateNode(coord, false);
00102   else
00103     readTreeOt->updateNode(coord, true);
00104 
00105   EXPECT_FALSE(tree == *readTreeOt);
00106 
00107   // simple test for tree headers (color)
00108   double res = 0.02;
00109   std::string filenameColor = "test_io_color_file.ot";
00110   ColorOcTree colorTree(res);
00111   EXPECT_EQ(colorTree.getTreeType(), "ColorOcTree");
00112   ColorOcTreeNode* colorNode = colorTree.updateNode(point3d(0.0, 0.0, 0.0), true);
00113   ColorOcTreeNode::Color color_red(255, 0, 0);
00114   colorNode->setColor(color_red);
00115   colorTree.setNodeColor(0.0, 0.0, 0.0, 255, 0, 0);
00116   colorTree.updateNode(point3d(0.1, 0.1, 0.1), true);
00117   colorTree.setNodeColor(0.1, 0.1, 0.1, 0, 0, 255);
00118 
00119   EXPECT_TRUE(colorTree.write(filenameColor));
00120   readTreeAbstract = AbstractOcTree::read(filenameColor);
00121   EXPECT_TRUE(readTreeAbstract);
00122   EXPECT_EQ(colorTree.getTreeType(),  readTreeAbstract->getTreeType());
00123   ColorOcTree* readColorTree = dynamic_cast<ColorOcTree*>(readTreeAbstract);
00124   EXPECT_TRUE(readColorTree);
00125   EXPECT_TRUE(colorTree == *readColorTree);
00126   colorNode = colorTree.search(0.0, 0.0, 0.0);
00127   EXPECT_TRUE(colorNode);
00128   EXPECT_EQ(colorNode->getColor(), color_red);
00129 
00130 
00131 
00132   std::cerr << "Test successful.\n";
00133   return 0;
00134 }


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Jun 6 2019 17:31:45