Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
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
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
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
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
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
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
00143
00144
00145 }
00146
00147
00148 }
00149
00150 cout << "KLD: " << kld_sum << endl;
00151
00152
00153
00154 delete tree1;
00155 delete tree2;
00156 }