compare_octrees.cpp
Go to the documentation of this file.
1 /*
2  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3  * http://octomap.github.com/
4  *
5  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
6  * All rights reserved.
7  * License: New BSD
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above copyright
15  * notice, this list of conditions and the following disclaimer in the
16  * documentation and/or other materials provided with the distribution.
17  * * Neither the name of the University of Freiburg nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  */
33 
34 #include <octomap/octomap.h>
35 #include <fstream>
36 #include <iostream>
37 #include <string.h>
38 #include <stdlib.h>
39 #include <list>
40 #include <cmath>
41 
42 #ifdef _MSC_VER // fix missing isnan for VC++
43 #define isnan(x) _isnan(x)
44 #endif
45 
46 // on MacOS, isnan is in std (also C++11)
47 using namespace std;
48 
49 using namespace octomap;
50 
51 void printUsage(char* self){
52  std::cerr << "\nUSAGE: " << self << " tree1.ot tree2.ot\n\n";
53 
54  std::cerr << "Compare two octrees for accuracy / compression.\n\n";
55 
56  exit(0);
57 }
58 
59 int main(int argc, char** argv) {
60 
61  if (argc != 3 || (argc > 1 && strcmp(argv[1], "-h") == 0)){
62  printUsage(argv[0]);
63  }
64 
65  std::string filename1 = std::string(argv[1]);
66  std::string filename2 = std::string(argv[2]);
67 
68  cout << "\nReading octree files...\n";
69 
70  OcTree* tree1 = dynamic_cast<OcTree*>(OcTree::read(filename1));
71  OcTree* tree2 = dynamic_cast<OcTree*>(OcTree::read(filename2));
72 
73  if (fabs(tree1->getResolution()-tree2->getResolution()) > 1e-6){
74  OCTOMAP_ERROR("Error: Tree resolutions don't match!");
75  exit(-1);
76  }
77 
78 
79  cout << "Expanding octrees... \n";
80  // expand both to full resolution:
81  tree1->expand();
82  tree2->expand();
83 
84  if (tree1->getNumLeafNodes() != tree2->getNumLeafNodes()){
85  OCTOMAP_ERROR_STR("Octrees have different size: " << tree1->getNumLeafNodes() << "!=" <<tree2->getNumLeafNodes() << endl);
86  exit(-1);
87  }
88 
89  cout << "Expanded num. leafs: " << tree1->getNumLeafNodes() << endl;
90 
91  // check bbx:
92  double x1, x2, y1, y2, z1, z2;
93  tree1->getMetricSize(x1, y1, z1);
94  tree2->getMetricSize(x2, y2, z2);
95 
96  if ((fabs(x1-x2) > 1e-6)
97  || (fabs(y1-y2) > 1e-6)
98  || (fabs(z1-z2) > 1e-6))
99  {
100  OCTOMAP_WARNING("Trees span over different volumes, results may be wrong\n");
101  exit(1);
102  }
103 
104  double kld_sum = 0.0;
105  cout << "Comparing trees... \n";
106  for (OcTree::leaf_iterator it = tree1->begin_leafs(),
107  end = tree1->end_leafs(); it != end; ++it)
108  {
109  OcTreeNode* n = tree2->search(it.getKey());
110  if (!n){
111  OCTOMAP_ERROR("Could not find coordinate of 1st octree in 2nd octree\n");
112  } else{
113  // check occupancy prob:
114  double p1 = it->getOccupancy();
115  double p2 = n->getOccupancy();
116  if (p1 < 0.0 || p1 > 1.0)
117  OCTOMAP_ERROR("p1 wrong: %f", p1);
118  if (p2 < 0.0 || p2 > 1.0)
119  OCTOMAP_ERROR("p2 wrong: %f", p2);
120 
121 // if (p1 > 0.1 || p2 > 0.1)
122  if (p1 > 0.001 && p2 < 0.001)
123  OCTOMAP_WARNING("p2 near 0, p1 > 0 => inf?");
124  if (p1 < 0.999 && p2 > 0.999)
125  OCTOMAP_WARNING("p2 near 1, p1 < 1 => inf?");
126 
127  double kld = 0;
128  if (p1 < 0.0001)
129  kld =log((1-p1)/(1-p2))*(1-p1);
130  else if (p1 > 0.9999)
131  kld =log(p1/p2)*p1;
132  else
133  kld +=log(p1/p2)*p1 + log((1-p1)/(1-p2))*(1-p1);
134 
135 #if __cplusplus >= 201103L
136  if (std::isnan(kld)){
137 #else
138  if (isnan(kld)){
139 #endif
140  OCTOMAP_ERROR("KLD is nan! KLD(%f,%f)=%f; sum = %f", p1, p2, kld, kld_sum);
141  exit(-1);
142  }
143 
144  kld_sum+=kld;
145 
146  //if (p1 <)
147 // if (fabs(p1-p2) > 1e-6)
148 // cout << "diff: " << p1-p2 << endl;
149  }
150 
151 
152  }
153 
154  cout << "KLD: " << kld_sum << endl;
155 
156 
157 
158  delete tree1;
159  delete tree2;
160 
161  return 0;
162 }
virtual void expand()
virtual void getMetricSize(double &x, double &y, double &z)
Size of OcTree (all known space) in meters for x, y and z dimension.
double getOccupancy() const
Definition: OcTreeNode.h:65
leaf_iterator begin_leafs(unsigned char maxDepth=0) const
NODE * search(double x, double y, double z, unsigned int depth=0) const
double getResolution() const
size_t getNumLeafNodes() const
Traverses the tree to calculate the total number of leaf nodes.
#define OCTOMAP_WARNING(...)
Definition: octomap_types.h:76
int main(int argc, char **argv)
const leaf_iterator end_leafs() const
#define OCTOMAP_ERROR_STR(args)
Definition: octomap_types.h:79
void printUsage(char *self)
#define OCTOMAP_ERROR(...)
Definition: octomap_types.h:78


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Mon Feb 28 2022 22:58:06