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;
00018 }
00019
00020
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
00044 OcTree tree (0.1);
00045 EXPECT_TRUE (tree.readBinary(filename));
00046
00047
00048 OcTree* treeCopy = new OcTree(tree);
00049 EXPECT_TRUE(tree == *treeCopy);
00050 EXPECT_TRUE(treeCopy->writeBinary(filenameBtCopyOut));
00051
00052
00053 treeCopy->setResolution(tree.getResolution()*2.0);
00054 EXPECT_FALSE(tree == *treeCopy);
00055 treeCopy->setResolution(tree.getResolution());
00056 EXPECT_TRUE(tree == *treeCopy);
00057
00058
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
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
00082 EXPECT_TRUE(tree.writeBinary(filenameBtOut));
00083 OcTree readTreeBt(0.1);
00084 EXPECT_TRUE(readTreeBt.readBinary(filenameBtOut));
00085 EXPECT_TRUE(tree == readTreeBt);
00086
00087
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
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
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 }