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 <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
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
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
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
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);
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);
00265
00266 double time_to_insert = (stop.tv_sec - start.tv_sec) + 1.0e-6 *(stop.tv_usec - start.tv_usec);
00267
00268
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 }