compare_octrees.cpp
Go to the documentation of this file.
00001 /*
00002  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
00003  * http://octomap.github.com/
00004  *
00005  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
00006  * All rights reserved.
00007  * License: New BSD
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  *     * Redistributions of source code must retain the above copyright
00013  *       notice, this list of conditions and the following disclaimer.
00014  *     * Redistributions in binary form must reproduce the above copyright
00015  *       notice, this list of conditions and the following disclaimer in the
00016  *       documentation and/or other materials provided with the distribution.
00017  *     * Neither the name of the University of Freiburg nor the names of its
00018  *       contributors may be used to endorse or promote products derived from
00019  *       this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00024  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00025  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00026  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00027  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00028  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00029  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00030  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
00032  */
00033 
00034 #include <octomap/octomap.h>
00035 #include <fstream>
00036 #include <iostream>
00037 #include <string.h>
00038 #include <stdlib.h>
00039 #include <list>
00040 #include <cmath>
00041 
00042 #ifdef _MSC_VER // fix missing isnan for VC++
00043 #define isnan(x) _isnan(x)  
00044 #endif
00045 
00046 // on MacOS, isnan is in std (also C++11)
00047 using namespace std;
00048 
00049 using namespace octomap;
00050 
00051 void printUsage(char* self){
00052   std::cerr << "\nUSAGE: " << self << " tree1.ot tree2.ot\n\n";
00053 
00054   std::cerr << "Compare two octrees for accuracy / compression.\n\n";
00055 
00056   exit(0);
00057 }
00058 
00059 int main(int argc, char** argv) {
00060 
00061   if (argc != 3 || (argc > 1 && strcmp(argv[1], "-h") == 0)){
00062     printUsage(argv[0]);
00063   }
00064 
00065   std::string filename1 = std::string(argv[1]);
00066   std::string filename2 = std::string(argv[2]);
00067 
00068   cout << "\nReading octree files...\n";
00069 
00070   OcTree* tree1 = dynamic_cast<OcTree*>(OcTree::read(filename1));
00071   OcTree* tree2 = dynamic_cast<OcTree*>(OcTree::read(filename2));
00072 
00073   if (fabs(tree1->getResolution()-tree2->getResolution()) > 1e-6){
00074     OCTOMAP_ERROR("Error: Tree resolutions don't match!");
00075     exit(-1);
00076   }
00077 
00078 
00079   cout << "Expanding octrees... \n";
00080   // expand both to full resolution:
00081   tree1->expand();
00082   tree2->expand();
00083 
00084   if (tree1->getNumLeafNodes() != tree2->getNumLeafNodes()){
00085       OCTOMAP_ERROR_STR("Octrees have different size: " << tree1->getNumLeafNodes() << "!=" <<tree2->getNumLeafNodes() << endl);
00086       exit(-1);
00087   }
00088 
00089   cout << "Expanded num. leafs: " << tree1->getNumLeafNodes() << endl;
00090 
00091   // check bbx:
00092   double x1, x2, y1, y2, z1, z2;
00093   tree1->getMetricSize(x1, y1, z1);
00094   tree2->getMetricSize(x2, y2, z2);
00095 
00096   if ((fabs(x1-x2) > 1e-6)
00097       || (fabs(y1-y2) > 1e-6)
00098       || (fabs(z1-z2) > 1e-6))
00099   {
00100     OCTOMAP_WARNING("Trees span over different volumes, results may be wrong\n");
00101     exit(1);
00102   }
00103 
00104   double kld_sum = 0.0;
00105   cout << "Comparing trees... \n";
00106   for (OcTree::leaf_iterator it = tree1->begin_leafs(),
00107       end = tree1->end_leafs();  it != end; ++it)
00108   {
00109     OcTreeNode* n = tree2->search(it.getKey());
00110     if (!n){
00111       OCTOMAP_ERROR("Could not find coordinate of 1st octree in 2nd octree\n");
00112     } else{
00113       // check occupancy prob:
00114       double p1 = it->getOccupancy();
00115       double p2 = n->getOccupancy();
00116       if (p1 < 0.0 || p1 > 1.0)
00117         OCTOMAP_ERROR("p1 wrong: %f", p1);
00118       if (p2 < 0.0 || p2 > 1.0)
00119         OCTOMAP_ERROR("p2 wrong: %f", p2);
00120 
00121 //      if (p1 > 0.1 || p2 > 0.1)
00122       if (p1 > 0.001 && p2 < 0.001)
00123         OCTOMAP_WARNING("p2 near 0, p1 > 0 => inf?");
00124       if (p1 < 0.999 && p2 > 0.999)
00125          OCTOMAP_WARNING("p2 near 1, p1 < 1 => inf?");
00126 
00127       double kld = 0;
00128       if (p1 < 0.0001)
00129         kld =log((1-p1)/(1-p2))*(1-p1);
00130       else if (p1 > 0.9999)
00131         kld =log(p1/p2)*p1;
00132       else
00133         kld +=log(p1/p2)*p1 + log((1-p1)/(1-p2))*(1-p1);
00134 
00135       if (isnan(kld)){
00136         OCTOMAP_ERROR("KLD is nan! KLD(%f,%f)=%f; sum = %f", p1, p2, kld, kld_sum);
00137         exit(-1);
00138       }
00139 
00140       kld_sum+=kld;
00141 
00142       //if (p1 <)
00143 //      if (fabs(p1-p2) > 1e-6)
00144 //        cout << "diff: " << p1-p2 << endl;
00145     }
00146 
00147 
00148   }
00149 
00150   cout << "KLD: " << kld_sum << endl;
00151 
00152 
00153 
00154   delete tree1;
00155   delete tree2;
00156 }


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Aug 27 2015 14:13:14