Go to the documentation of this file.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 << " <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
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
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
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 }
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 }