42 #ifdef _MSC_VER // fix missing isnan for VC++ 43 #define isnan(x) _isnan(x) 52 std::cerr <<
"\nUSAGE: " <<
self <<
" tree1.ot tree2.ot\n\n";
54 std::cerr <<
"Compare two octrees for accuracy / compression.\n\n";
59 int main(
int argc,
char** argv) {
61 if (argc != 3 || (argc > 1 && strcmp(argv[1],
"-h") == 0)){
65 std::string filename1 = std::string(argv[1]);
66 std::string filename2 = std::string(argv[2]);
68 cout <<
"\nReading octree files...\n";
70 OcTree* tree1 =
dynamic_cast<OcTree*
>(OcTree::read(filename1));
71 OcTree* tree2 =
dynamic_cast<OcTree*
>(OcTree::read(filename2));
79 cout <<
"Expanding octrees... \n";
92 double x1, x2, y1, y2, z1, z2;
96 if ((fabs(x1-x2) > 1e-6)
97 || (fabs(y1-y2) > 1e-6)
98 || (fabs(z1-z2) > 1e-6))
100 OCTOMAP_WARNING(
"Trees span over different volumes, results may be wrong\n");
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)
111 OCTOMAP_ERROR(
"Could not find coordinate of 1st octree in 2nd octree\n");
114 double p1 = it->getOccupancy();
116 if (p1 < 0.0 || p1 > 1.0)
118 if (p2 < 0.0 || p2 > 1.0)
122 if (p1 > 0.001 && p2 < 0.001)
124 if (p1 < 0.999 && p2 > 0.999)
129 kld =log((1-p1)/(1-p2))*(1-p1);
130 else if (p1 > 0.9999)
133 kld +=log(p1/p2)*p1 + log((1-p1)/(1-p2))*(1-p1);
136 OCTOMAP_ERROR(
"KLD is nan! KLD(%f,%f)=%f; sum = %f", p1, p2, kld, kld_sum);
150 cout <<
"KLD: " << kld_sum << endl;
size_t getNumLeafNodes() const
Traverses the tree to calculate the total number of leaf nodes.
virtual void getMetricSize(double &x, double &y, double &z)
Size of OcTree (all known space) in meters for x, y and z dimension.
const leaf_iterator end_leafs() const
NODE * search(double x, double y, double z, unsigned int depth=0) const
double getOccupancy() const
#define OCTOMAP_WARNING(...)
int main(int argc, char **argv)
#define OCTOMAP_ERROR_STR(args)
double getResolution() const
void printUsage(char *self)
leaf_iterator begin_leafs(unsigned char maxDepth=0) const
#define OCTOMAP_ERROR(...)