int main(int argc, char * argv[]) { signal(SIGABRT, &sighandler); signal(SIGTERM, &sighandler); signal(SIGINT, &sighandler); /*for(int i=0; i<argc; i++) { printf("argv[%d] = %s\n", i, argv[i]); }*/ const ParametersMap & defaultParameters = Parameters::getDefaultParameters(); if(argc < 2) { showUsage(); } else if(argc == 2 && strcmp(argv[1], "-v") == 0) { printf("%s\n", Rtabmap::getVersion().c_str()); exit(0); } else if(argc == 2 && strcmp(argv[1], "-default_params") == 0) { for(ParametersMap::const_iterator iter = defaultParameters.begin(); iter!=defaultParameters.end(); ++iter) { printf("%s=%s\n", iter->first.c_str(), iter->second.c_str()); } exit(0); } printf("\n"); std::string path; float rate = 0.0; int loopDataset = 0; int repeat = 0; bool createGT = false; std::string inputDbPath; int startAt = 1; ParametersMap pm; ULogger::Level logLevel = ULogger::kError; ULogger::Level exitLevel = ULogger::kFatal; bool logConsole = false; for(int i=1; i<argc; ++i) { if(i == argc-1) { // The last must be the path path = argv[i]; if(!UDirectory::exists(path.c_str()) && !UFile::exists(path.c_str())) { printf("Path not valid : %s\n", path.c_str()); showUsage(); exit(1); } break; } if(strcmp(argv[i], "-rate") == 0) { ++i; if(i < argc) { rate = uStr2Float(argv[i]); if(rate < 0) { showUsage(); } } else { showUsage(); } continue; } if(strcmp(argv[i], "-rateHz") == 0) { ++i; if(i < argc) { rate = uStr2Float(argv[i]); if(rate < 0) { showUsage(); } else if(rate) { rate = 1/rate; } } else { showUsage(); } continue; } if(strcmp(argv[i], "-repeat") == 0) { ++i; if(i < argc) { repeat = std::atoi(argv[i]); if(repeat < 1) { showUsage(); } } else { showUsage(); } continue; } if(strcmp(argv[i], "-start_at") == 0) { ++i; if(i < argc) { startAt = std::atoi(argv[i]); if(startAt < 0) { showUsage(); } } else { showUsage(); } continue; } if(strcmp(argv[i], "-createGT") == 0) { createGT = true; continue; } if(strcmp(argv[i], "-input") == 0) { ++i; if(i < argc) { inputDbPath = argv[i]; } else { showUsage(); } continue; } if(strcmp(argv[i], "-debug") == 0) { logLevel = ULogger::kDebug; continue; } if(strcmp(argv[i], "-info") == 0) { logLevel = ULogger::kInfo; continue; } if(strcmp(argv[i], "-warn") == 0) { logLevel = ULogger::kWarning; continue; } if(strcmp(argv[i], "-exit_warn") == 0) { exitLevel = ULogger::kWarning; continue; } if(strcmp(argv[i], "-exit_error") == 0) { exitLevel = ULogger::kError; continue; } if(strcmp(argv[i], "-log_console") == 0) { logConsole = true; continue; } // Check for RTAB-Map's parameters std::string key = argv[i]; key = uSplit(key, '-').back(); if(defaultParameters.find(key) != defaultParameters.end()) { ++i; if(i < argc) { std::string value = argv[i]; if(value.empty()) { showUsage(); } else { value = uReplaceChar(value, ',', ' '); } std::pair<ParametersMap::iterator, bool> inserted = pm.insert(ParametersPair(key, value)); if(inserted.second == false) { inserted.first->second = value; } } else { showUsage(); } continue; } printf("Unrecognized option : %s\n", argv[i]); showUsage(); } if(repeat && createGT) { printf("Cannot create a Ground truth if repeat is on.\n"); showUsage(); } if(logConsole) { ULogger::setType(ULogger::kTypeConsole); } //ULogger::setType(ULogger::kTypeFile, rtabmap.getWorkingDir()+"/LogConsole.txt", false); //ULogger::setBuffered(true); ULogger::setLevel(logLevel); ULogger::setExitLevel(exitLevel); UTimer timer; timer.start(); std::queue<double> iterationMeanTime; Camera * camera = 0; if(UDirectory::exists(path)) { camera = new CameraImages(path, startAt, false, false, false, rate>0.0f?1.0f/rate:0.0f); } else { camera = new CameraVideo(path, false, rate>0.0f?1.0f/rate:0.0f); } if(!camera || !camera->init()) { printf("Camera init failed, using path \"%s\"\n", path.c_str()); exit(1); } std::map<int, int> groundTruth; // Create tasks Rtabmap rtabmap; if(inputDbPath.empty()) { inputDbPath = "rtabmapconsole.db"; if(UFile::erase(inputDbPath) == 0) { printf("Deleted database \"%s\".\n", inputDbPath.c_str()); } } else { printf("Loading database \"%s\".\n", inputDbPath.c_str()); } // Disable statistics (we don't need them) pm.insert(ParametersPair(Parameters::kRtabmapPublishStats(), "false")); pm.insert(ParametersPair(Parameters::kRGBDEnabled(), "false")); rtabmap.init(pm, inputDbPath); printf("Avpd init time = %fs\n", timer.ticks()); // Start thread's task int loopClosureId; int count = 0; int countLoopDetected=0; printf("\nParameters : \n"); printf(" Data set : %s\n", path.c_str()); printf(" Time threshold = %1.2f ms\n", rtabmap.getTimeThreshold()); printf(" Image rate = %1.2f s (%1.2f Hz)\n", rate, 1/rate); printf(" Repeating data set = %s\n", repeat?"true":"false"); printf(" Camera starts at image %d (default 1)\n", startAt); if(createGT) { printf(" Creating the ground truth matrix.\n"); } printf(" INFO: All other parameters are set to defaults\n"); if(pm.size()>1) { printf(" Overwritten parameters :\n"); for(ParametersMap::iterator iter = pm.begin(); iter!=pm.end(); ++iter) { printf(" %s=%s\n",iter->first.c_str(), iter->second.c_str()); } } if(rtabmap.getWM().size() || rtabmap.getSTM().size()) { printf("[Warning] RTAB-Map database is not empty (%s)\n", inputDbPath.c_str()); } printf("\nProcessing images...\n"); UTimer iterationTimer; UTimer rtabmapTimer; int imagesProcessed = 0; std::list<std::vector<float> > teleopActions; while(loopDataset <= repeat && g_forever) { SensorData data = camera->takeImage(); int i=0; double maxIterationTime = 0.0; int maxIterationTimeId = 0; while(!data.imageRaw().empty() && g_forever) { ++imagesProcessed; iterationTimer.start(); rtabmapTimer.start(); rtabmap.process(data.imageRaw()); double rtabmapTime = rtabmapTimer.elapsed(); loopClosureId = rtabmap.getLoopClosureId(); if(rtabmap.getLoopClosureId()) { ++countLoopDetected; } data = camera->takeImage(); if(++count % 100 == 0) { printf(" count = %d, loop closures = %d, max time (at %d) = %fs\n", count, countLoopDetected, maxIterationTimeId, maxIterationTime); maxIterationTime = 0.0; maxIterationTimeId = 0; std::map<int, int> wm = rtabmap.getWeights(); printf(" WM(%d)=[", (int)wm.size()); for(std::map<int, int>::iterator iter=wm.begin(); iter!=wm.end();++iter) { if(iter != wm.begin()) { printf(";"); } printf("%d,%d", iter->first, iter->second); } printf("]\n"); } // Update generated ground truth matrix if(createGT) { if(loopClosureId > 0) { groundTruth.insert(std::make_pair(i, loopClosureId-1)); } } ++i; double iterationTime = iterationTimer.ticks(); if(rtabmapTime > maxIterationTime) { maxIterationTime = rtabmapTime; maxIterationTimeId = count; } ULogger::flush(); if(rtabmap.getLoopClosureId()) { printf(" iteration(%d) loop(%d) hyp(%.2f) time=%fs/%fs *\n", count, rtabmap.getLoopClosureId(), rtabmap.getLoopClosureValue(), rtabmapTime, iterationTime); } else if(rtabmap.getHighestHypothesisId()) { printf(" iteration(%d) high(%d) hyp(%.2f) time=%fs/%fs\n", count, rtabmap.getHighestHypothesisId(), rtabmap.getHighestHypothesisValue(), rtabmapTime, iterationTime); } else { printf(" iteration(%d) time=%fs/%fs\n", count, rtabmapTime, iterationTime); } if(rtabmap.getTimeThreshold() && rtabmapTime > rtabmap.getTimeThreshold()*100.0f) { printf(" ERROR, there is problem, too much time taken... %fs", rtabmapTime); break; // there is problem, don't continue } } ++loopDataset; if(loopDataset <= repeat) { camera->init(); printf(" Beginning loop %d...\n", loopDataset); } } printf("Processing images completed. Loop closures found = %d\n", countLoopDetected); printf(" Total time = %fs\n", timer.ticks()); if(imagesProcessed && createGT) { cv::Mat groundTruthMat = cv::Mat::zeros(imagesProcessed, imagesProcessed, CV_8U); for(std::map<int, int>::iterator iter = groundTruth.begin(); iter!=groundTruth.end(); ++iter) { groundTruthMat.at<unsigned char>(iter->first, iter->second) = 255; } // Generate the ground truth file printf("Generate ground truth to file %s, size of %d\n", GENERATED_GT_NAME, groundTruthMat.rows); cv::imwrite(GENERATED_GT_NAME, groundTruthMat); printf(" Creating ground truth file = %fs\n", timer.ticks()); } if(camera) { delete camera; camera = 0 ; } rtabmap.close(); printf(" Cleanup time = %fs\n", timer.ticks()); printf("Database (\"%s\") and log files saved to current directory.\n", inputDbPath.c_str()); return 0; }