eval_octree_accuracy.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 <string.h>
00035 #include <stdlib.h>
00036 #include <iostream>
00037 #include <fstream>
00038 
00039 #include <octomap/octomap.h>
00040 #include <octomap/octomap_timing.h>
00041 
00042 using namespace std;
00043 using namespace octomap;
00044 
00045 void printUsage(char* self){
00046   std::cerr << "USAGE: " << self << " <InputFile.graph>\n";
00047   std::cerr << "This tool is part of OctoMap and evaluates the statistical accuracy\n"
00048                "of an octree map from scan graph data (point clouds with poses).\n";
00049 
00050   std::cerr << "OPTIONS:\n"
00051             "  -res <resolution> (default: 0.1 m)\n"
00052             "  -m <maxrange> (optional) \n"
00053             "  -n <max scan no.> (optional) \n"
00054   "\n";
00055 
00056   exit(0);
00057 }
00058 
00059 
00060 int main(int argc, char** argv) {
00061   // default values:
00062   double res = 0.1;
00063 
00064   if (argc < 2)
00065     printUsage(argv[0]);
00066 
00067   string graphFilename = std::string(argv[1]);
00068 
00069   double maxrange = -1;
00070   int max_scan_no = -1;
00071   int skip_scan_eval = 5;
00072 
00073   int arg = 1;
00074   while (++arg < argc) {
00075     if (! strcmp(argv[arg], "-i"))
00076       graphFilename = std::string(argv[++arg]);
00077     else if (! strcmp(argv[arg], "-res"))
00078       res = atof(argv[++arg]);
00079     else if (! strcmp(argv[arg], "-m"))
00080       maxrange = atof(argv[++arg]);
00081     else if (! strcmp(argv[arg], "-n"))
00082       max_scan_no = atoi(argv[++arg]);
00083     else {
00084       printUsage(argv[0]);
00085     }
00086   }
00087 
00088   cout << "\nReading Graph file\n===========================\n";
00089   ScanGraph* graph = new ScanGraph();
00090   graph->readBinary(graphFilename);
00091   unsigned int num_points_in_graph = 0;
00092   if (max_scan_no > 0) {
00093     num_points_in_graph = graph->getNumPoints(max_scan_no-1);
00094     cout << "\n Data points in graph up to scan " << max_scan_no << ": " << num_points_in_graph << endl;
00095   }
00096   else {
00097     num_points_in_graph = graph->getNumPoints();
00098     cout << "\n Data points in graph: " << num_points_in_graph << endl;
00099   }
00100 
00101   cout << "\nCreating tree\n===========================\n";
00102   OcTree* tree = new OcTree(res);
00103 
00104   unsigned int numScans = graph->size();
00105   unsigned int currentScan = 1;
00106   for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
00107 
00108     if (currentScan % skip_scan_eval != 0){
00109       if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush;
00110       else cout << "("<<currentScan << "/" << numScans << ") " << flush;
00111       tree->insertPointCloud(**scan_it, maxrange);
00112     } else
00113       cout << "(SKIP) " << flush;
00114 
00115     if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no))
00116       break;
00117 
00118     currentScan++;
00119   }
00120 
00121   tree->expand();
00122 
00123   
00124   cout << "\nEvaluating scans\n===========================\n";
00125   currentScan = 1;
00126   unsigned num_points = 0;
00127   unsigned num_voxels_correct = 0;
00128   unsigned num_voxels_wrong = 0;
00129   unsigned num_voxels_unknown = 0;
00130 
00131 
00132   for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
00133 
00134     if (currentScan % skip_scan_eval == 0){
00135       if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush;
00136       else cout << "("<<currentScan << "/" << numScans << ") " << flush;
00137 
00138 
00139       pose6d frame_origin = (*scan_it)->pose;
00140       point3d sensor_origin = frame_origin.inv().transform((*scan_it)->pose.trans());
00141 
00142       // transform pointcloud:
00143       Pointcloud scan (*(*scan_it)->scan);
00144       scan.transform(frame_origin);
00145       point3d origin = frame_origin.transform(sensor_origin);
00146 
00147       KeySet free_cells, occupied_cells;
00148       tree->computeUpdate(scan, origin, free_cells, occupied_cells, maxrange);
00149 
00150       num_points += scan.size();
00151 
00152       // count free cells
00153       for (KeySet::iterator it = free_cells.begin(); it != free_cells.end(); ++it) {
00154         OcTreeNode* n = tree->search(*it);
00155         if (n){
00156           if (tree->isNodeOccupied(n))
00157             num_voxels_wrong++;
00158           else
00159             num_voxels_correct++;
00160         } else
00161           num_voxels_unknown++;
00162       } // count occupied cells
00163       for (KeySet::iterator it = occupied_cells.begin(); it != occupied_cells.end(); ++it) {
00164         OcTreeNode* n = tree->search(*it);
00165         if (n){
00166           if (tree->isNodeOccupied(n))
00167             num_voxels_correct++;
00168           else
00169             num_voxels_wrong++;
00170         } else
00171           num_voxels_unknown++;
00172       }
00173 
00174 
00175     }
00176 
00177     if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no))
00178       break;
00179 
00180     currentScan++;
00181 
00182 
00183   }
00184 
00185   cout << "\nFinished evaluating " << num_points <<"/"<< num_points_in_graph << " points.\n"
00186       <<"Voxels correct: "<<num_voxels_correct<<" #wrong: " <<num_voxels_wrong << " #unknown: " <<num_voxels_unknown
00187       <<". % correct: "<< num_voxels_correct/double(num_voxels_correct+num_voxels_wrong)<<"\n\n";
00188 
00189 
00190   delete graph;
00191   delete tree;
00192 }


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