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 #include <rtabmap/utilite/ULogger.h>
00029 #include <rtabmap/utilite/UTimer.h>
00030 #include "rtabmap/core/Rtabmap.h"
00031 #include "rtabmap/core/Camera.h"
00032 #include <rtabmap/utilite/UDirectory.h>
00033 #include <rtabmap/utilite/UFile.h>
00034 #include <rtabmap/utilite/UConversion.h>
00035 #include <rtabmap/utilite/UStl.h>
00036 #include <fstream>
00037 #include <queue>
00038 #include <opencv2/core/core.hpp>
00039 #include <signal.h>
00040
00041 using namespace rtabmap;
00042
00043 #define GENERATED_GT_NAME "GroundTruth_generated.bmp"
00044
00045 void showUsage()
00046 {
00047 printf("\nUsage:\n"
00048 "rtabmap-console [options] \"path\"\n"
00049 " path For images, use the directory path. For videos or databases, use full\n "
00050 " path name\n"
00051 "Options:\n"
00052 " -rate #.## Acquisition time (seconds)\n"
00053 " -rateHz #.## Acquisition rate (Hz), for convenience\n"
00054 " -repeat # Repeat the process on the data set # times (minimum of 1)\n"
00055 " -createGT Generate a ground truth file\n"
00056 " -image_width # Force an image width (Default 0: original size used).\n"
00057 " The height must be also specified if changed.\n"
00058 " -image_height # Force an image height (Default 0: original size used)\n"
00059 " The height must be also specified if changed.\n"
00060 " -start_at # When \"path\" is a directory of images, set this parameter\n"
00061 " to start processing at image # (default 1)."
00062 " -\"parameter name\" \"value\" Overwrite a specific RTAB-Map's parameter :\n"
00063 " -SURF/HessianThreshold 150\n"
00064 " For parameters in table format, add ',' between values :\n"
00065 " -Kp/RoiRatios 0,0,0.1,0\n"
00066 " -default_params Show default RTAB-Map's parameters\n"
00067 " -debug Set Log level to Debug (Default Error)\n"
00068 " -info Set Log level to Info (Default Error)\n"
00069 " -warn Set Log level to Warning (Default Error)\n"
00070 " -exit_warn Set exit level to Warning (Default Fatal)\n"
00071 " -exit_error Set exit level to Error (Default Fatal)\n"
00072 " -v Get version of RTAB-Map\n"
00073 " -input \"path\" Load previous database if it exists.\n");
00074 exit(1);
00075 }
00076
00077
00078 bool g_forever = true;
00079 void sighandler(int sig)
00080 {
00081 printf("\nSignal %d caught...\n", sig);
00082 g_forever = false;
00083 }
00084
00085 int main(int argc, char * argv[])
00086 {
00087 signal(SIGABRT, &sighandler);
00088 signal(SIGTERM, &sighandler);
00089 signal(SIGINT, &sighandler);
00090
00091
00092
00093
00094
00095 const ParametersMap & defaultParameters = Parameters::getDefaultParameters();
00096 if(argc < 2)
00097 {
00098 showUsage();
00099 }
00100 else if(argc == 2 && strcmp(argv[1], "-v") == 0)
00101 {
00102 printf("%s\n", Rtabmap::getVersion().c_str());
00103 exit(0);
00104 }
00105 else if(argc == 2 && strcmp(argv[1], "-default_params") == 0)
00106 {
00107 for(ParametersMap::const_iterator iter = defaultParameters.begin(); iter!=defaultParameters.end(); ++iter)
00108 {
00109 printf("%s=%s\n", iter->first.c_str(), iter->second.c_str());
00110 }
00111 exit(0);
00112 }
00113 printf("\n");
00114
00115 std::string path;
00116 float rate = 0.0;
00117 int loopDataset = 0;
00118 int repeat = 0;
00119 bool createGT = false;
00120 std::string inputDbPath;
00121 int imageWidth = 0;
00122 int imageHeight = 0;
00123 int startAt = 1;
00124 ParametersMap pm;
00125 ULogger::Level logLevel = ULogger::kError;
00126 ULogger::Level exitLevel = ULogger::kFatal;
00127
00128 for(int i=1; i<argc; ++i)
00129 {
00130 if(i == argc-1)
00131 {
00132
00133 path = argv[i];
00134 if(!UDirectory::exists(path.c_str()) && !UFile::exists(path.c_str()))
00135 {
00136 printf("Path not valid : %s\n", path.c_str());
00137 showUsage();
00138 exit(1);
00139 }
00140 break;
00141 }
00142 if(strcmp(argv[i], "-rate") == 0)
00143 {
00144 ++i;
00145 if(i < argc)
00146 {
00147 rate = uStr2Float(argv[i]);
00148 if(rate < 0)
00149 {
00150 showUsage();
00151 }
00152 }
00153 else
00154 {
00155 showUsage();
00156 }
00157 continue;
00158 }
00159 if(strcmp(argv[i], "-rateHz") == 0)
00160 {
00161 ++i;
00162 if(i < argc)
00163 {
00164 rate = uStr2Float(argv[i]);
00165 if(rate < 0)
00166 {
00167 showUsage();
00168 }
00169 else if(rate)
00170 {
00171 rate = 1/rate;
00172 }
00173 }
00174 else
00175 {
00176 showUsage();
00177 }
00178 continue;
00179 }
00180 if(strcmp(argv[i], "-repeat") == 0)
00181 {
00182 ++i;
00183 if(i < argc)
00184 {
00185 repeat = std::atoi(argv[i]);
00186 if(repeat < 1)
00187 {
00188 showUsage();
00189 }
00190 }
00191 else
00192 {
00193 showUsage();
00194 }
00195 continue;
00196 }
00197 if(strcmp(argv[i], "-image_width") == 0)
00198 {
00199 ++i;
00200 if(i < argc)
00201 {
00202 imageWidth = std::atoi(argv[i]);
00203 if(imageWidth < 0)
00204 {
00205 showUsage();
00206 }
00207 }
00208 else
00209 {
00210 showUsage();
00211 }
00212 continue;
00213 }
00214 if(strcmp(argv[i], "-image_height") == 0)
00215 {
00216 ++i;
00217 if(i < argc)
00218 {
00219 imageHeight = std::atoi(argv[i]);
00220 if(imageHeight < 0)
00221 {
00222 showUsage();
00223 }
00224 }
00225 else
00226 {
00227 showUsage();
00228 }
00229 continue;
00230 }
00231 if(strcmp(argv[i], "-start_at") == 0)
00232 {
00233 ++i;
00234 if(i < argc)
00235 {
00236 startAt = std::atoi(argv[i]);
00237 if(startAt < 0)
00238 {
00239 showUsage();
00240 }
00241 }
00242 else
00243 {
00244 showUsage();
00245 }
00246 continue;
00247 }
00248 if(strcmp(argv[i], "-createGT") == 0)
00249 {
00250 createGT = true;
00251 continue;
00252 }
00253 if(strcmp(argv[i], "-input") == 0)
00254 {
00255 ++i;
00256 if(i < argc)
00257 {
00258 inputDbPath = argv[i];
00259 }
00260 else
00261 {
00262 showUsage();
00263 }
00264 continue;
00265 }
00266 if(strcmp(argv[i], "-debug") == 0)
00267 {
00268 logLevel = ULogger::kDebug;
00269 continue;
00270 }
00271 if(strcmp(argv[i], "-info") == 0)
00272 {
00273 logLevel = ULogger::kInfo;
00274 continue;
00275 }
00276 if(strcmp(argv[i], "-warn") == 0)
00277 {
00278 logLevel = ULogger::kWarning;
00279 continue;
00280 }
00281 if(strcmp(argv[i], "-exit_warn") == 0)
00282 {
00283 exitLevel = ULogger::kWarning;
00284 continue;
00285 }
00286 if(strcmp(argv[i], "-exit_error") == 0)
00287 {
00288 exitLevel = ULogger::kError;
00289 continue;
00290 }
00291
00292
00293 std::string key = argv[i];
00294 key = uSplit(key, '-').back();
00295 if(defaultParameters.find(key) != defaultParameters.end())
00296 {
00297 ++i;
00298 if(i < argc)
00299 {
00300 std::string value = argv[i];
00301 if(value.empty())
00302 {
00303 showUsage();
00304 }
00305 else
00306 {
00307 value = uReplaceChar(value, ',', ' ');
00308 }
00309 std::pair<ParametersMap::iterator, bool> inserted = pm.insert(ParametersPair(key, value));
00310 if(inserted.second == false)
00311 {
00312 inserted.first->second = value;
00313 }
00314 }
00315 else
00316 {
00317 showUsage();
00318 }
00319 continue;
00320 }
00321
00322 printf("Unrecognized option : %s\n", argv[i]);
00323 showUsage();
00324 }
00325
00326 if(repeat && createGT)
00327 {
00328 printf("Cannot create a Ground truth if repeat is on.\n");
00329 showUsage();
00330 }
00331 else if((imageWidth && imageHeight == 0) ||
00332 (imageHeight && imageWidth == 0))
00333 {
00334 printf("If imageWidth is set, imageHeight must be too.\n");
00335 showUsage();
00336 }
00337
00338 UTimer timer;
00339 timer.start();
00340 std::queue<double> iterationMeanTime;
00341
00342 Camera * camera = 0;
00343 if(UDirectory::exists(path))
00344 {
00345 camera = new CameraImages(path, startAt, false, 1/rate, imageWidth, imageHeight);
00346 }
00347 else
00348 {
00349 camera = new CameraVideo(path, 1/rate, imageWidth, imageHeight);
00350 }
00351
00352 if(!camera || !camera->init())
00353 {
00354 printf("Camera init failed, using path \"%s\"\n", path.c_str());
00355 exit(1);
00356 }
00357
00358 std::map<int, int> groundTruth;
00359
00360 ULogger::setType(ULogger::kTypeConsole);
00361
00362
00363 ULogger::setLevel(logLevel);
00364 ULogger::setExitLevel(exitLevel);
00365
00366
00367 Rtabmap rtabmap;
00368 if(inputDbPath.empty())
00369 {
00370 inputDbPath = "rtabmapconsole.db";
00371 if(UFile::erase(inputDbPath) == 0)
00372 {
00373 printf("Deleted database \"%s\".\n", inputDbPath.c_str());
00374 }
00375 }
00376 else
00377 {
00378 printf("Loading database \"%s\".\n", inputDbPath.c_str());
00379 }
00380
00381 pm.insert(ParametersPair(Parameters::kRtabmapPublishStats(), "false"));
00382 pm.insert(ParametersPair(Parameters::kRGBDEnabled(), "false"));
00383
00384 rtabmap.init(pm, inputDbPath);
00385
00386 printf("Avpd init time = %fs\n", timer.ticks());
00387
00388
00389 int loopClosureId;
00390 int count = 0;
00391 int countLoopDetected=0;
00392
00393 printf("\nParameters : \n");
00394 printf(" Data set : %s\n", path.c_str());
00395 printf(" Time threshold = %1.2f ms\n", rtabmap.getTimeThreshold());
00396 printf(" Image rate = %1.2f s (%1.2f Hz)\n", rate, 1/rate);
00397 printf(" Repeating data set = %s\n", repeat?"true":"false");
00398 printf(" Camera width=%d, height=%d (0 is default)\n", imageWidth, imageHeight);
00399 printf(" Camera starts at image %d (default 1)\n", startAt);
00400 if(createGT)
00401 {
00402 printf(" Creating the ground truth matrix.\n");
00403 }
00404 printf(" INFO: All other parameters are set to defaults\n");
00405 if(pm.size()>1)
00406 {
00407 printf(" Overwritten parameters :\n");
00408 for(ParametersMap::iterator iter = pm.begin(); iter!=pm.end(); ++iter)
00409 {
00410 printf(" %s=%s\n",iter->first.c_str(), iter->second.c_str());
00411 }
00412 }
00413 if(rtabmap.getWM().size() || rtabmap.getSTM().size())
00414 {
00415 printf("[Warning] RTAB-Map database is not empty (%s)\n", inputDbPath.c_str());
00416 }
00417 printf("\nProcessing images...\n");
00418
00419 UTimer iterationTimer;
00420 UTimer rtabmapTimer;
00421 int imagesProcessed = 0;
00422 std::list<std::vector<float> > teleopActions;
00423 while(loopDataset <= repeat && g_forever)
00424 {
00425 cv::Mat img = camera->takeImage();
00426 int i=0;
00427 double maxIterationTime = 0.0;
00428 int maxIterationTimeId = 0;
00429 while(!img.empty() && g_forever)
00430 {
00431 ++imagesProcessed;
00432 iterationTimer.start();
00433 rtabmapTimer.start();
00434 rtabmap.process(img);
00435 double rtabmapTime = rtabmapTimer.elapsed();
00436 loopClosureId = rtabmap.getLoopClosureId();
00437 if(rtabmap.getLoopClosureId())
00438 {
00439 ++countLoopDetected;
00440 }
00441 img = camera->takeImage();
00442 if(++count % 100 == 0)
00443 {
00444 printf(" count = %d, loop closures = %d, max time (at %d) = %fs\n",
00445 count, countLoopDetected, maxIterationTimeId, maxIterationTime);
00446 maxIterationTime = 0.0;
00447 maxIterationTimeId = 0;
00448 std::map<int, int> wm = rtabmap.getWeights();
00449 printf(" WM(%d)=[", (int)wm.size());
00450 for(std::map<int, int>::iterator iter=wm.begin(); iter!=wm.end();++iter)
00451 {
00452 if(iter != wm.begin())
00453 {
00454 printf(";");
00455 }
00456 printf("%d,%d", iter->first, iter->second);
00457 }
00458 printf("]\n");
00459 }
00460
00461
00462 if(createGT)
00463 {
00464 if(loopClosureId > 0)
00465 {
00466 groundTruth.insert(std::make_pair(i, loopClosureId-1));
00467 }
00468 }
00469
00470 ++i;
00471
00472 double iterationTime = iterationTimer.ticks();
00473
00474 if(rtabmapTime > maxIterationTime)
00475 {
00476 maxIterationTime = rtabmapTime;
00477 maxIterationTimeId = count;
00478 }
00479
00480 ULogger::flush();
00481
00482 if(rtabmap.getLoopClosureId())
00483 {
00484 printf(" iteration(%d) loop(%d) hyp(%.2f) time=%fs/%fs *\n",
00485 count, rtabmap.getLoopClosureId(), rtabmap.getLoopClosureValue(), rtabmapTime, iterationTime);
00486 }
00487 else if(rtabmap.getHighestHypothesisId())
00488 {
00489 printf(" iteration(%d) high(%d) hyp(%.2f) time=%fs/%fs\n",
00490 count, rtabmap.getHighestHypothesisId(), rtabmap.getHighestHypothesisValue(), rtabmapTime, iterationTime);
00491 }
00492 else
00493 {
00494 printf(" iteration(%d) time=%fs/%fs\n", count, rtabmapTime, iterationTime);
00495 }
00496
00497 if(rtabmap.getTimeThreshold() && rtabmapTime > rtabmap.getTimeThreshold()*100.0f)
00498 {
00499 printf(" ERROR, there is problem, too much time taken... %fs", rtabmapTime);
00500 break;
00501 }
00502 }
00503 ++loopDataset;
00504 if(loopDataset <= repeat)
00505 {
00506 camera->init();
00507 printf(" Beginning loop %d...\n", loopDataset);
00508 }
00509 }
00510 printf("Processing images completed. Loop closures found = %d\n", countLoopDetected);
00511 printf(" Total time = %fs\n", timer.ticks());
00512
00513 if(imagesProcessed && createGT)
00514 {
00515 cv::Mat groundTruthMat = cv::Mat::zeros(imagesProcessed, imagesProcessed, CV_8U);
00516
00517 for(std::map<int, int>::iterator iter = groundTruth.begin(); iter!=groundTruth.end(); ++iter)
00518 {
00519 groundTruthMat.at<unsigned char>(iter->first, iter->second) = 255;
00520 }
00521
00522
00523 printf("Generate ground truth to file %s, size of %d\n", GENERATED_GT_NAME, groundTruthMat.rows);
00524 IplImage img = groundTruthMat;
00525 cvSaveImage(GENERATED_GT_NAME, &img);
00526 printf(" Creating ground truth file = %fs\n", timer.ticks());
00527 }
00528
00529 if(camera)
00530 {
00531 delete camera;
00532 camera = 0 ;
00533 }
00534
00535 rtabmap.close();
00536
00537 printf(" Cleanup time = %fs\n", timer.ticks());
00538
00539 printf("Database (\"%s\") and log files saved to current directory.\n", inputDbPath.c_str());
00540
00541 return 0;
00542 }