graph2tree.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 << " [options]\n\n";
00047   std::cerr << "This tool is part of OctoMap and inserts the data of a scan graph\n"
00048                "file (point clouds with poses) into an octree.\n"
00049                "The output is a compact maximum-likelihood binary octree file \n"
00050                "(.bt, bonsai tree) and general octree files (.ot) with the full\n"
00051                "information.\n\n";
00052 
00053 
00054   std::cerr << "OPTIONS:\n  -i <InputFile.graph> (required)\n"
00055             "  -o <OutputFile.bt> (required) \n"
00056             "  -res <resolution> (optional, default: 0.1 m)\n"
00057             "  -m <maxrange> (optional) \n"
00058             "  -n <max scan no.> (optional) \n"
00059             "  -log (enable a detailed log file with statistics) \n"
00060             "  -compressML (enable maximum-likelihood compression (lossy) after every scan)\n"
00061             "  -simple (simple scan insertion ray by ray instead of optimized) \n"
00062             "  -discretize (approximate raycasting on discretized coordinates, speeds up insertion) \n"
00063             "  -clamping <p_min> <p_max> (override default sensor model clamping probabilities between 0..1)\n"
00064             "  -sensor <p_miss> <p_hit> (override default sensor model hit and miss probabilities between 0..1)"
00065   "\n";
00066 
00067 
00068 
00069 
00070   exit(0);
00071 }
00072 
00073 void calcThresholdedNodes(const OcTree* tree,
00074                           unsigned int& num_thresholded,
00075                           unsigned int& num_other)
00076 {
00077   num_thresholded = 0;
00078   num_other = 0;
00079 
00080   for(OcTree::tree_iterator it = tree->begin_tree(), end=tree->end_tree(); it!= end; ++it){
00081     if (tree->isNodeAtThreshold(*it))
00082       num_thresholded++;
00083     else
00084       num_other++;
00085   }
00086 }
00087 
00088 void outputStatistics(const OcTree* tree){
00089   unsigned int numThresholded, numOther;
00090   calcThresholdedNodes(tree, numThresholded, numOther);
00091   size_t memUsage = tree->memoryUsage();
00092   unsigned long long memFullGrid = tree->memoryFullGrid();
00093   size_t numLeafNodes = tree->getNumLeafNodes();
00094 
00095   cout << "Tree size: " << tree->size() <<" nodes (" << numLeafNodes<< " leafs). " <<numThresholded <<" nodes thresholded, "<< numOther << " other\n";
00096   cout << "Memory: " << memUsage << " byte (" << memUsage/(1024.*1024.) << " MB)" << endl;
00097   cout << "Full grid: "<< memFullGrid << " byte (" << memFullGrid/(1024.*1024.) << " MB)" << endl;
00098   double x, y, z;
00099   tree->getMetricSize(x, y, z);
00100   cout << "Size: " << x << " x " << y << " x " << z << " m^3\n";
00101   cout << endl;
00102 }
00103 
00104 
00105 int main(int argc, char** argv) {
00106   // default values:
00107   double res = 0.1;
00108   string graphFilename = "";
00109   string treeFilename = "";
00110   double maxrange = -1;
00111   int max_scan_no = -1;
00112   bool detailedLog = false;
00113   bool simpleUpdate = false;
00114   bool discretize = false;
00115   unsigned char compression = 1;
00116 
00117   // get default sensor model values:
00118   OcTree emptyTree(0.1);
00119   double clampingMin = emptyTree.getClampingThresMin();
00120   double clampingMax = emptyTree.getClampingThresMax();
00121   double probMiss = emptyTree.getProbMiss();
00122   double probHit = emptyTree.getProbHit();
00123 
00124 
00125   timeval start; 
00126   timeval stop; 
00127 
00128   int arg = 0;
00129   while (++arg < argc) {
00130     if (! strcmp(argv[arg], "-i"))
00131       graphFilename = std::string(argv[++arg]);
00132     else if (!strcmp(argv[arg], "-o"))
00133       treeFilename = std::string(argv[++arg]);
00134     else if (! strcmp(argv[arg], "-res") && argc-arg < 2)
00135       printUsage(argv[0]);
00136     else if (! strcmp(argv[arg], "-res"))
00137       res = atof(argv[++arg]);
00138     else if (! strcmp(argv[arg], "-log"))
00139       detailedLog = true;
00140     else if (! strcmp(argv[arg], "-simple"))
00141       simpleUpdate = true;
00142     else if (! strcmp(argv[arg], "-discretize"))
00143       discretize = true;
00144     else if (! strcmp(argv[arg], "-compress"))
00145       OCTOMAP_WARNING("Argument -compress no longer has an effect, incremental pruning is done during each insertion.\n");
00146     else if (! strcmp(argv[arg], "-compressML"))
00147       compression = 2;
00148     else if (! strcmp(argv[arg], "-m"))
00149       maxrange = atof(argv[++arg]);
00150     else if (! strcmp(argv[arg], "-n"))
00151       max_scan_no = atoi(argv[++arg]);
00152     else if (! strcmp(argv[arg], "-clamping") && (argc-arg < 3))
00153       printUsage(argv[0]);
00154     else if (! strcmp(argv[arg], "-clamping")){
00155       clampingMin = atof(argv[++arg]);
00156       clampingMax = atof(argv[++arg]);
00157     }
00158     else if (! strcmp(argv[arg], "-sensor") && (argc-arg < 3))
00159       printUsage(argv[0]);
00160     else if (! strcmp(argv[arg], "-sensor")){
00161       probMiss = atof(argv[++arg]);
00162       probHit = atof(argv[++arg]);
00163     }
00164     else {
00165       printUsage(argv[0]);
00166     }
00167   }
00168 
00169   if (graphFilename == "" || treeFilename == "")
00170     printUsage(argv[0]);
00171 
00172   // verify input:
00173   if (res <= 0.0){
00174     OCTOMAP_ERROR("Resolution must be positive");
00175     exit(1);
00176   }
00177 
00178   if (clampingMin >= clampingMax || clampingMin < 0.0 || clampingMax > 1.0){
00179     OCTOMAP_ERROR("Error in clamping values:  0.0 <= [%f] < [%f] <= 1.0\n", clampingMin, clampingMax);
00180     exit(1);
00181   }
00182 
00183   if (probMiss >= probHit || probMiss < 0.0 || probHit > 1.0){
00184     OCTOMAP_ERROR("Error in sensor model (hit/miss prob.):  0.0 <= [%f] < [%f] <= 1.0\n", probMiss, probHit);
00185     exit(1);
00186   }
00187 
00188 
00189   std::string treeFilenameOT = treeFilename + ".ot";
00190   std::string treeFilenameMLOT = treeFilename + "_ml.ot";
00191 
00192   cout << "\nReading Graph file\n===========================\n";
00193   ScanGraph* graph = new ScanGraph();
00194   if (!graph->readBinary(graphFilename))
00195     exit(2);
00196 
00197   unsigned int num_points_in_graph = 0;
00198   if (max_scan_no > 0) {
00199     num_points_in_graph = graph->getNumPoints(max_scan_no-1);
00200     cout << "\n Data points in graph up to scan " << max_scan_no << ": " << num_points_in_graph << endl;
00201   }
00202   else {
00203     num_points_in_graph = graph->getNumPoints();
00204     cout << "\n Data points in graph: " << num_points_in_graph << endl;
00205   }
00206 
00207   // transform pointclouds first, so we can directly operate on them later
00208   for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
00209 
00210     pose6d frame_origin = (*scan_it)->pose;
00211     point3d sensor_origin = frame_origin.inv().transform((*scan_it)->pose.trans());
00212 
00213     (*scan_it)->scan->transform(frame_origin);
00214     point3d transformed_sensor_origin = frame_origin.transform(sensor_origin);
00215     (*scan_it)->pose = pose6d(transformed_sensor_origin, octomath::Quaternion());
00216 
00217   }
00218 
00219 
00220   std::ofstream logfile;
00221   if (detailedLog){
00222     logfile.open((treeFilename+".log").c_str());
00223     logfile << "# Memory of processing " << graphFilename << " over time\n";
00224     logfile << "# Resolution: "<< res <<"; compression: " << int(compression) << "; scan endpoints: "<< num_points_in_graph << std::endl;
00225     logfile << "# [scan number] [bytes octree] [bytes full 3D grid]\n";
00226   }
00227 
00228 
00229 
00230   cout << "\nCreating tree\n===========================\n";
00231   OcTree* tree = new OcTree(res);
00232 
00233   tree->setClampingThresMin(clampingMin);
00234   tree->setClampingThresMax(clampingMax);
00235   tree->setProbHit(probHit);
00236   tree->setProbMiss(probMiss);
00237 
00238 
00239   gettimeofday(&start, NULL);  // start timer
00240   unsigned int numScans = graph->size();
00241   unsigned int currentScan = 1;
00242   for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
00243     if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush;
00244     else cout << "("<<currentScan << "/" << numScans << ") " << flush;
00245 
00246     if (simpleUpdate)
00247       tree->insertPointCloudRays((*scan_it)->scan, (*scan_it)->pose.trans(), maxrange);
00248     else
00249       tree->insertPointCloud((*scan_it)->scan, (*scan_it)->pose.trans(), maxrange, false, discretize);
00250 
00251     if (compression == 2){
00252       tree->toMaxLikelihood();
00253       tree->prune();
00254     }
00255 
00256     if (detailedLog)
00257       logfile << currentScan << " " << tree->memoryUsage() << " " << tree->memoryFullGrid() << "\n";
00258 
00259     if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no))
00260       break;
00261 
00262     currentScan++;
00263   }
00264   gettimeofday(&stop, NULL);  // stop timer
00265   
00266   double time_to_insert = (stop.tv_sec - start.tv_sec) + 1.0e-6 *(stop.tv_usec - start.tv_usec);
00267 
00268   // get rid of graph in mem before doing anything fancy with tree (=> memory)
00269   delete graph;
00270   if (logfile.is_open())
00271     logfile.close();
00272 
00273 
00274 
00275   cout << "\nDone building tree.\n\n";
00276   cout << "time to insert scans: " << time_to_insert << " sec" << endl;
00277   cout << "time to insert 100.000 points took: " << time_to_insert/ ((double) num_points_in_graph / 100000) << " sec (avg)" << endl << endl;
00278 
00279 
00280   std::cout << "Pruned tree (lossless compression)\n" << "===========================\n";
00281   outputStatistics(tree);
00282 
00283   tree->write(treeFilenameOT);
00284 
00285   std::cout << "Pruned max-likelihood tree (lossy compression)\n" << "===========================\n";
00286   tree->toMaxLikelihood();
00287   tree->prune();
00288   outputStatistics(tree);
00289 
00290 
00291   cout << "\nWriting tree files\n===========================\n";
00292   tree->write(treeFilenameMLOT);
00293   std::cout << "Full Octree (pruned) written to "<< treeFilenameOT << std::endl;
00294   std::cout << "Full Octree (max.likelihood, pruned) written to "<< treeFilenameMLOT << std::endl;
00295   tree->writeBinary(treeFilename);
00296   std::cout << "Bonsai tree written to "<< treeFilename << std::endl;
00297   cout << endl;
00298 
00299   delete tree;
00300 
00301   exit(0);
00302 }


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Jun 6 2019 17:31:45