00001 #include <feature/Detector.h>
00002 #include <feature/ShapeContext.h>
00003 #include <feature/BetaGrid.h>
00004 #include <feature/RangeDetector.h>
00005 #include <feature/CurvatureDetector.h>
00006 #include <feature/NormalBlobDetector.h>
00007 #include <feature/NormalEdgeDetector.h>
00008 #include <feature/RansacFeatureSetMatcher.h>
00009 #include <feature/RansacMultiFeatureSetMatcher.h>
00010 #include <sensorstream/CarmenLog.h>
00011 #include <sensorstream/LogSensorStream.h>
00012 #include <sensorstream/SensorStream.h>
00013 #include <utils/SimpleMinMaxPeakFinder.h>
00014 #include <utils/HistogramDistances.h>
00015
00016 #include <iostream>
00017 #include <string>
00018 #include <string.h>
00019 #include <sstream>
00020 #include <utility>
00021
00022 #include <sys/time.h>
00023
00024 LogSensorStream m_sensorReference(NULL,NULL);
00025
00026 CurvatureDetector *m_detectorCurvature = NULL;
00027 NormalBlobDetector *m_detectorNormalBlob = NULL;
00028 NormalEdgeDetector *m_detectorNormalEdge = NULL;
00029 RangeDetector *m_detectorRange = NULL;
00030 Detector* m_detector = NULL;
00031
00032 BetaGridGenerator *m_betaGenerator = NULL;
00033 ShapeContextGenerator *m_shapeGenerator = NULL;
00034 DescriptorGenerator *m_descriptor = NULL;
00035
00036 RansacFeatureSetMatcher *m_ransac = NULL;
00037
00038 double angErrorTh = 0.2;
00039 double linErrorTh = 0.5;
00040
00041 std::vector< std::vector<InterestPoint *> > m_pointsReference;
00042 std::vector< OrientedPoint2D > m_posesReference;
00043
00044 unsigned int corresp[] = {0, 3, 5, 7, 9, 11, 13, 15};
00045
00046 double m_error[8] = {0.}, m_errorC[8] = {0.}, m_errorR[8] = {0.};
00047 unsigned int m_match[8] = {0}, m_matchC[8] = {0}, m_matchR[8] = {0};
00048 unsigned int m_valid[8] = {0};
00049
00050 struct timeval detectTime, describeTime, ransacTime;
00051
00052 void help(){
00053 std::cerr << "FLIRTLib version 0.9b - authors Gian Diego Tipaldi and Kai O. Arras" << std::endl
00054 << "Usage: ransacLoopClosureTest -filename <logfile> [options] " << std::endl
00055 << "Options:" << std::endl
00056
00057 << " -filename \t The logfile in CARMEN format to process (mandatory)." << std::endl
00058 << " -scale \t The number of scales to consider (default=5)." << std::endl
00059 << " -dmst \t The number of spanning tree for the curvature detector (deafult=2)." << std::endl
00060 << " -window \t The size of the local window for estimating the normal signal (default=3)." << std::endl
00061 << " -detectorType \t The type of detector to use. Available options are (default=0):" << std::endl
00062 << " \t 0 - Curvature detector;" << std::endl
00063 << " \t 1 - Normal edge detector;" << std::endl
00064 << " \t 2 - Normal blob detector;" << std::endl
00065 << " \t 3 - Range detector." << std::endl
00066 << " -descriptorType \t The type of descriptor to use. Available options are (default=0):" << std::endl
00067 << " \t 0 - Beta - grid;" << std::endl
00068 << " \t 1 - Shape context." << std::endl
00069 << " -distanceType \t The distance function to compare the descriptors. Available options are (default=2):" << std::endl
00070 << " \t 0 - Euclidean distance;" << std::endl
00071 << " \t 1 - Chi square distance;" << std::endl
00072 << " \t 2 - Symmetric Chi square distance;" << std::endl
00073 << " \t 3 - Bhattacharyya distance;" << std::endl
00074 << " \t 4 - Kullback Leibler divergence;" << std::endl
00075 << " \t 5 - Jensen Shannon divergence." << std::endl
00076 << " -baseSigma \t The initial standard deviation for the smoothing operator (default=0.2)." << std::endl
00077 << " -sigmaStep \t The incremental step for the scales of the smoothing operator. (default=1.4)." << std::endl
00078 << " -minPeak \t The minimum value for a peak to be detected (default=0.34)." << std::endl
00079 << " -minPeakDistance \t The minimum difference with the neighbors for a peak to be detected (default=0.001)." << std::endl
00080 << " -acceptanceSigma \t The standard deviation of the detection error (default=0.1)." << std::endl
00081 << " -success \t The success probability of RANSAC (default=0.95)." << std::endl
00082 << " -inlier \t The inlier probability of RANSAC (default=0.4)." << std::endl
00083 << " -matchingThreshold \t The threshold two descriptors are considered matched (default=0.4)." << std::endl
00084 << " -strategy \t The strategy for matching two descriptors. Available options are (default=0):" << std::endl
00085 << " \t 0 - Nearest Neighbour. Only the closest match above the threshold is considered;" << std::endl
00086 << " \t 1 - Threshold. All the matches above the threshold are considered." << std::endl
00087 << std::endl
00088 << "The program computes matches every scan with all the others in the logfile. To work properly, the logfile must" << std::endl
00089 << "be corrected by a SLAM algorithm beforehand (the provided logfiles are already corrected). The program writes" << std::endl
00090 << "the following matching statistics in the following files:" << std::endl
00091 << " correct matches: <logfile>_<detector>_<descriptor>_<distance>_match.dat" << std::endl
00092 << " matching error : <logfile>_<detector>_<descriptor>_<distance>_error.dat" << std::endl
00093 << " matching time : <logfile>_<detector>_<descriptor>_<distance>_time.dat" << std::endl
00094 << std::endl
00095 << std::endl;
00096 }
00097
00098 void match(unsigned int position)
00099 {
00100 m_sensorReference.seek(position);
00101
00102 std::vector<InterestPoint *> pointsLocal(m_pointsReference[position].size());
00103 const LaserReading* lreadReference = dynamic_cast<const LaserReading*>(m_sensorReference.current());
00104 for(unsigned int j = 0; j < m_pointsReference[position].size(); j++){
00105 InterestPoint * point = new InterestPoint(*m_pointsReference[position][j]);
00106 point->setPosition(lreadReference->getLaserPose().ominus(point->getPosition()));
00107 pointsLocal[j] = point;
00108 }
00109
00110 unsigned int inliers[m_pointsReference.size()];
00111 double results[m_pointsReference.size()];
00112 double linearErrors[m_pointsReference.size()];
00113 double angularErrors[m_pointsReference.size()];
00114 struct timeval start, end, diff, sum;
00115 for(unsigned int i = 0; i < m_pointsReference.size(); i++){
00116 if(i == position) {
00117 results[i] = 1e17;
00118 inliers[i] = 0;
00119 linearErrors[i] = 1e17;
00120 angularErrors[i] = 1e17;
00121 continue;
00122 }
00123 OrientedPoint2D transform;
00124 std::vector< std::pair<InterestPoint*, InterestPoint* > > correspondences;
00125 gettimeofday(&start,NULL);
00126
00127 results[i] = m_ransac->matchSets(m_pointsReference[i], pointsLocal, transform, correspondences);
00128 gettimeofday(&end,NULL);
00129 timersub(&end, &start, &diff);
00130 timeradd(&ransacTime, &diff, &sum);
00131 ransacTime = sum;
00132 inliers[i] = correspondences.size();
00133 OrientedPoint2D delta = m_posesReference[position] - transform;
00134 linearErrors[i] = correspondences.size() ? delta * delta : 1e17;
00135 angularErrors[i] = correspondences.size() ? delta.theta * delta.theta : 1e17;
00136 }
00137
00138 for(unsigned int c = 0; c < 8; c++){
00139 unsigned int maxCorres = 0;
00140 double maxResult = 1e17;
00141 double linError = 1e17, angError = 1e17;
00142 double linErrorC = 1e17, angErrorC = 1e17;
00143 double linErrorR = 1e17, angErrorR = 1e17;
00144 bool valid = false;
00145 for(unsigned int i = 0; i < m_pointsReference.size(); i++){
00146 if(linError + angError > linearErrors[i] + angularErrors[i]) {
00147 linError = linearErrors[i];
00148 angError = angularErrors[i];
00149 }
00150 if(maxCorres < inliers[i]){
00151 linErrorC = linearErrors[i];
00152 angErrorC = angularErrors[i];
00153 maxCorres = inliers[i];
00154 }
00155 if(maxResult > results[i]){
00156 linErrorR = linearErrors[i];
00157 angErrorR = angularErrors[i];
00158 maxResult = results[i];
00159 }
00160 valid = valid || inliers[i] >= corresp[c];
00161 }
00162
00163
00164 if(valid){
00165 m_match[c] += (linError <= (linErrorTh * linErrorTh) && angError <= (angErrorTh * angErrorTh) );
00166 m_matchC[c] += (linErrorC <= (linErrorTh * linErrorTh) && angErrorC <= (angErrorTh * angErrorTh) );
00167 m_matchR[c] += (linErrorR <= (linErrorTh * linErrorTh) && angErrorR <= (angErrorTh * angErrorTh) );
00168
00169 m_error[c] += sqrt(linError + angError);
00170 m_errorC[c] += sqrt(linErrorC + angErrorC);
00171 m_errorR[c] += sqrt(linErrorR + angErrorR);
00172
00173 m_valid[c]++;
00174 }
00175 }
00176
00177 }
00178
00179 void detectLog(){
00180 unsigned int i = 0;
00181 unsigned int position = m_sensorReference.tell();
00182 m_sensorReference.seek(0,END);
00183 unsigned int last = m_sensorReference.tell();
00184 m_sensorReference.seek(0);
00185
00186 std::string bar(50, ' ');
00187 bar[0] = '#';
00188 unsigned int progress = 0;
00189
00190 struct timeval start, end;
00191 gettimeofday(&start, NULL);
00192 while(!m_sensorReference.end()){
00193 unsigned int currentProgress = (m_sensorReference.tell()*100)/last;
00194 if (progress < currentProgress){
00195 progress = currentProgress;
00196 bar[progress/2] = '#';
00197 std::cout << "\rDetecting points [" << bar << "] " << (m_sensorReference.tell()*100)/last << "%" << std::flush;
00198 }
00199 const LaserReading* lreadReference = dynamic_cast<const LaserReading*>(m_sensorReference.next());
00200 if (lreadReference){
00201 m_detector->detect(*lreadReference, m_pointsReference[i]);
00202 m_posesReference[i] = lreadReference->getLaserPose();
00203 i++;
00204 }
00205 }
00206 gettimeofday(&end,NULL);
00207 timersub(&end,&start,&detectTime);
00208 m_sensorReference.seek(position);
00209 std::cout << " done." << std::endl;
00210 }
00211
00212 void countLog(){
00213 double flirtNum = 0.;
00214 uint count = 0;
00215
00216 std::string bar(50, ' ');
00217 bar[0] = '#';
00218
00219 unsigned int progress = 0;
00220 for(unsigned int i = 0; i < m_pointsReference.size(); i++){
00221 unsigned int currentProgress = (i*100)/(m_pointsReference.size() - 1);
00222 if (progress < currentProgress){
00223 progress = currentProgress;
00224 bar[progress/2] = '#';
00225 std::cout << "\rCounting points [" << bar << "] " << progress << "%" << std::flush;
00226 }
00227 flirtNum += m_pointsReference[i].size();
00228 count++;
00229 }
00230 flirtNum=count?flirtNum/double(count):0.;
00231 std::cout << " done.\nFound " << flirtNum << " FLIRT features per scan." << std::endl;
00232 }
00233
00234 void describeLog(){
00235 unsigned int i = 0;
00236 unsigned int position = m_sensorReference.tell();
00237 m_sensorReference.seek(0,END);
00238 unsigned int last = m_sensorReference.tell();
00239 m_sensorReference.seek(0);
00240
00241 std::string bar(50, ' ');
00242 bar[0] = '#';
00243 struct timeval start, end;
00244 gettimeofday(&start, NULL);
00245 unsigned int progress = 0;
00246
00247 while(!m_sensorReference.end()){
00248 unsigned int currentProgress = (m_sensorReference.tell()*100)/last;
00249 if (progress < currentProgress){
00250 progress = currentProgress;
00251 bar[progress/2] = '#';
00252 std::cout << "\rDescribing points [" << bar << "] " << progress << "%" << std::flush;
00253 }
00254 const LaserReading* lreadReference = dynamic_cast<const LaserReading*>(m_sensorReference.next());
00255 if (lreadReference){
00256 for(unsigned int j = 0; j < m_pointsReference[i].size(); j++){
00257 m_pointsReference[i][j]->setDescriptor(m_descriptor->describe(*m_pointsReference[i][j], *lreadReference));
00258 }
00259 i++;
00260 }
00261 }
00262 gettimeofday(&end,NULL);
00263 timersub(&end,&start,&describeTime);
00264 m_sensorReference.seek(position);
00265 std::cout << " done." << std::endl;
00266 }
00267
00268 int main(int argc, char **argv){
00269
00270 std::string filename("");
00271 unsigned int scale = 5, dmst = 2, window = 3, detectorType = 0, descriptorType = 0, distanceType = 2, strategy = 0;
00272 double baseSigma = 0.2, sigmaStep = 1.4, minPeak = 0.34, minPeakDistance = 0.001, acceptanceSigma = 0.1, success = 0.95, inlier = 0.4, matchingThreshold = 0.4;
00273 bool useMaxRange = false;
00274
00275 int i = 1;
00276 while(i < argc){
00277 if(strncmp("-filename", argv[i], sizeof("-filename")) == 0 ){
00278 filename = argv[++i];
00279 i++;
00280 } else if(strncmp("-detector", argv[i], sizeof("-detector")) == 0 ){
00281 detectorType = atoi(argv[++i]);
00282 i++;
00283 } else if(strncmp("-descriptor", argv[i], sizeof("-descriptor")) == 0 ){
00284 descriptorType = atoi(argv[++i]);
00285 i++;
00286 } else if(strncmp("-distance", argv[i], sizeof("-distance")) == 0 ){
00287 distanceType = atoi(argv[++i]);
00288 i++;
00289 } else if(strncmp("-baseSigma", argv[i], sizeof("-baseSigma")) == 0 ){
00290 baseSigma = strtod(argv[++i], NULL);
00291 i++;
00292 } else if(strncmp("-sigmaStep", argv[i], sizeof("-sigmaStep")) == 0 ){
00293 sigmaStep = strtod(argv[++i], NULL);
00294 i++;
00295 } else if(strncmp("-minPeak", argv[i], sizeof("-minPeak")) == 0 ){
00296 minPeak = strtod(argv[++i], NULL);
00297 i++;
00298 } else if(strncmp("-minPeakDistance", argv[i], sizeof("-minPeakDistance")) == 0 ){
00299 minPeakDistance = strtod(argv[++i], NULL);
00300 i++;
00301 } else if(strncmp("-scale", argv[i], sizeof("-scale")) == 0 ){
00302 scale = atoi(argv[++i]);
00303 i++;
00304 } else if(strncmp("-dmst", argv[i], sizeof("-dmst")) == 0 ){
00305 dmst = atoi(argv[++i]);
00306 i++;
00307 } else if(strncmp("-window", argv[i], sizeof("-window")) == 0 ){
00308 scale = atoi(argv[++i]);
00309 i++;
00310 } else if(strncmp("-acceptanceSigma", argv[i], sizeof("-acceptanceSigma")) == 0 ){
00311 acceptanceSigma = strtod(argv[++i], NULL);
00312 i++;
00313 } else if(strncmp("-success", argv[i], sizeof("-success")) == 0 ){
00314 success = strtod(argv[++i], NULL);
00315 i++;
00316 } else if(strncmp("-inlier", argv[i], sizeof("-inlier")) == 0 ){
00317 inlier = strtod(argv[++i], NULL);
00318 i++;
00319 } else if(strncmp("-matchingThreshold", argv[i], sizeof("-matchingThreshold")) == 0 ){
00320 matchingThreshold = strtod(argv[++i], NULL);
00321 i++;
00322 } else {
00323 i++;
00324 }
00325 }
00326
00327 if(!filename.size()){
00328 help();
00329 exit(-1);
00330 }
00331
00332
00333 CarmenLogWriter writer;
00334 CarmenLogReader reader;
00335
00336 m_sensorReference = LogSensorStream(&reader, &writer);
00337
00338 m_sensorReference.load(filename);
00339
00340 SimpleMinMaxPeakFinder *m_peakMinMax = new SimpleMinMaxPeakFinder(minPeak, minPeakDistance);
00341
00342
00343 std::string detector("");
00344 switch(detectorType){
00345 case 0:
00346 m_detectorCurvature = new CurvatureDetector(m_peakMinMax, scale, baseSigma, sigmaStep, dmst);
00347 m_detectorCurvature->setUseMaxRange(useMaxRange);
00348 m_detector = m_detectorCurvature;
00349 detector = "curvature";
00350 break;
00351 case 1:
00352 m_detectorNormalEdge = new NormalEdgeDetector(m_peakMinMax, scale, baseSigma, sigmaStep, window);
00353 m_detectorNormalEdge->setUseMaxRange(useMaxRange);
00354 m_detector = m_detectorNormalEdge;
00355 detector = "edge";
00356 break;
00357 case 2:
00358 m_detectorNormalBlob = new NormalBlobDetector(m_peakMinMax, scale, baseSigma, sigmaStep, window);
00359 m_detectorNormalBlob->setUseMaxRange(useMaxRange);
00360 m_detector = m_detectorNormalBlob;
00361 detector = "blob";
00362 break;
00363 case 3:
00364 m_detectorRange = new RangeDetector(m_peakMinMax, scale, baseSigma, sigmaStep);
00365 m_detectorRange->setUseMaxRange(useMaxRange);
00366 m_detector = m_detectorRange;
00367 detector = "range";
00368 break;
00369 default:
00370 std::cerr << "Wrong detector type" << std::endl;
00371 exit(-1);
00372 }
00373
00374 HistogramDistance<double> *dist = NULL;
00375
00376 std::string distance("");
00377 switch(distanceType){
00378 case 0:
00379 dist = new EuclideanDistance<double>();
00380 distance = "euclid";
00381 break;
00382 case 1:
00383 dist = new Chi2Distance<double>();
00384 distance = "chi2";
00385 break;
00386 case 2:
00387 dist = new SymmetricChi2Distance<double>();
00388 distance = "symchi2";
00389 break;
00390 case 3:
00391 dist = new BatthacharyyaDistance<double>();
00392 distance = "batt";
00393 break;
00394 case 4:
00395 dist = new KullbackLeiblerDistance<double>();
00396 distance = "kld";
00397 break;
00398 case 5:
00399 dist = new JensenShannonDistance<double>();
00400 distance = "jsd";
00401 break;
00402 default:
00403 std::cerr << "Wrong distance type" << std::endl;
00404 exit(-1);
00405 }
00406
00407 std::string descriptor("");
00408 switch(descriptorType){
00409 case 0:
00410 m_betaGenerator = new BetaGridGenerator(0.02, 0.5, 4, 12);
00411 m_betaGenerator->setDistanceFunction(dist);
00412 m_descriptor = m_betaGenerator;
00413 descriptor = "beta";
00414 break;
00415 case 1:
00416 m_shapeGenerator = new ShapeContextGenerator(0.02, 0.5, 4, 12);
00417 m_shapeGenerator->setDistanceFunction(dist);
00418 m_descriptor = m_shapeGenerator;
00419 descriptor = "shape";
00420 break;
00421 default:
00422 std::cerr << "Wrong descriptor type" << std::endl;
00423 exit(-1);
00424 }
00425
00426 switch(strategy){
00427 case 0:
00428 m_ransac = new RansacFeatureSetMatcher(acceptanceSigma * acceptanceSigma * 5.99, success, inlier, matchingThreshold, acceptanceSigma * acceptanceSigma * 3.84, false);
00429 break;
00430 case 1:
00431 m_ransac = new RansacMultiFeatureSetMatcher(acceptanceSigma * acceptanceSigma * 5.99, success, inlier, matchingThreshold, acceptanceSigma * acceptanceSigma * 3.84, false);
00432 break;
00433 default:
00434 std::cerr << "Wrong strategy type" << std::endl;
00435 exit(-1);
00436 }
00437
00438 std::cerr << "Processing file:\t" << filename << "\nDetector:\t\t" << detector << "\nDescriptor:\t\t" << descriptor << "\nDistance:\t\t" << distance << std::endl;
00439
00440 m_sensorReference.seek(0,END);
00441 unsigned int end = m_sensorReference.tell();
00442 m_sensorReference.seek(0,BEGIN);
00443
00444 m_pointsReference.resize(end + 1);
00445 m_posesReference.resize(end + 1);
00446
00447 detectLog();
00448
00449 countLog();
00450
00451 describeLog();
00452
00453 std::string outfile = filename;
00454
00455 timerclear(&ransacTime);
00456
00457 std::string bar(50,' ');
00458 bar[0] = '#';
00459 unsigned int progress = 0;
00460
00461 for(unsigned int i =0; i < m_pointsReference.size(); i++){
00462 unsigned int currentProgress = (i*100)/(m_pointsReference.size() - 1);
00463 if (progress < currentProgress){
00464 progress = currentProgress;
00465 bar[progress/2] = '#';
00466 std::cout << "\rMatching [" << bar << "] " << progress << "%" << std::flush;
00467 }
00468 match(i);
00469 }
00470 std::cout << " done." << std::endl;
00471
00472 std::stringstream matchFile;
00473 std::stringstream errorFile;
00474 std::stringstream timeFile;
00475 matchFile << outfile << "_" << detector << "_" << descriptor << "_" << distance << "_match.dat";
00476 errorFile << outfile << "_" << detector << "_" << descriptor << "_" << distance << "_error.dat";
00477 timeFile << outfile << "_" << detector << "_" << descriptor << "_" << distance << "_time.dat";
00478
00479 std::ofstream matchOut(matchFile.str().c_str());
00480 std::ofstream errorOut(errorFile.str().c_str());
00481 std::ofstream timeOut(timeFile.str().c_str());
00482
00483 matchOut << "# Number of matches according to various strategies" << std::endl;
00484 matchOut << "# The valid matches are the one with at least n correspondences in the inlier set " << std::endl;
00485 matchOut << "# where n = {0, 3, 5, 7, 9, 11, 13, 15}, one for each line " << std::endl;
00486 matchOut << "# optimal \t correspondence \t residual \v valid" << std::endl;
00487
00488 errorOut << "# Mean error according to various strategies" << std::endl;
00489 errorOut << "# The valid matches are the one with at least n correspondences in the inlier set " << std::endl;
00490 errorOut << "# where n = {0, 3, 5, 7, 9, 11, 13, 15}, one for each line " << std::endl;
00491 errorOut << "# optimal \t correspondence \t residual \v valid" << std::endl;
00492
00493 timeOut << "# Total time spent for the various steps" << std::endl;
00494 timeOut << "# detection \t description \t RANSAC" << std::endl;
00495
00496 for(unsigned int c = 0; c < 8; c++){
00497 matchOut << m_match[c] << "\t" << m_matchC[c] << "\t" << m_matchR[c] << "\t" << m_valid[c] << std::endl;
00498 errorOut << m_error[c]/m_valid[c] << "\t" << m_errorC[c]/m_valid[c] << "\t" << m_errorR[c]/m_valid[c] << "\t" << m_valid[c] << std::endl;
00499 }
00500 timeOut << double(detectTime.tv_sec) + 1e-06 * double(detectTime.tv_usec) << "\t"
00501 << double(describeTime.tv_sec) + 1e-06 * double(describeTime.tv_usec) << "\t"
00502 << double(ransacTime.tv_sec) + 1e-06 * double(ransacTime.tv_usec) << std::endl;
00503 }
00504
