int main(int argc, char * argv[])
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kError);
if(argc < 8)
{
showUsage();
}
int argIndex = 1;
int cameraRate = atoi(argv[argIndex++]);
if(cameraRate <= 0)
{
printf("camera_rate should be > 0\n");
showUsage();
}
int odomUpdate = atoi(argv[argIndex++]);
if(odomUpdate <= 0)
{
printf("odom_update should be > 0\n");
showUsage();
}
int mapUpdate = atoi(argv[argIndex++]);
if(mapUpdate <= 0)
{
printf("map_update should be > 0\n");
showUsage();
}
printf("Camera rate = %d Hz\n", cameraRate);
printf("Odometry update rate = %d Hz\n", cameraRate/odomUpdate);
printf("Map update rate = %d Hz\n", (cameraRate/odomUpdate)/mapUpdate);
std::string calibrationDir = argv[argIndex++];
std::string calibrationName = argv[argIndex++];
std::string pathLeftImages = argv[argIndex++];
std::string pathRightImages = argv[argIndex++];
Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
CameraStereoImages camera(
pathLeftImages,
pathRightImages,
false, // assume that images are already rectified
(float)cameraRate,
opticalRotation);
if(camera.init(calibrationDir, calibrationName))
{
OdometryF2M odom;
Rtabmap rtabmap;
rtabmap.init();
QApplication app(argc, argv);
MapBuilder mapBuilder;
mapBuilder.show();
QApplication::processEvents();
SensorData data = camera.takeImage();
int cameraIteration = 0;
int odometryIteration = 0;
printf("Press \"Space\" in the window to pause\n");
while(data.isValid() && mapBuilder.isVisible())
{
if(cameraIteration++ % odomUpdate == 0)
{
OdometryInfo info;
Transform pose = odom.process(data, &info);
if(odometryIteration++ % mapUpdate == 0)
{
if(rtabmap.process(data, pose))
{
mapBuilder.processStatistics(rtabmap.getStatistics());
if(rtabmap.getLoopClosureId() > 0)
{
printf("Loop closure detected!\n");
}
}
}
mapBuilder.processOdometry(data, pose, info);
}
QApplication::processEvents();
while(mapBuilder.isPaused() && mapBuilder.isVisible())
{
uSleep(100);
QApplication::processEvents();
}
data = camera.takeImage();
}
if(mapBuilder.isVisible())
{
printf("Processed all frames\n");
app.exec();
}
}
else
{
UERROR("Camera init failed!");
}
return 0;
}
int main(int argc, char * argv[])
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
ParametersMap pm = Parameters::parseArguments(argc, argv);
pm.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), "."));
int argIndex = 1;
int cameraRate = atoi(argv[argIndex++]);
if(cameraRate <= 0)
{
printf("camera_rate should be > 0\n");
showUsage();
}
int odomUpdate = atoi(argv[argIndex++]);
if(odomUpdate <= 0)
{
printf("odom_update should be > 0\n");
showUsage();
}
int mapUpdate = atoi(argv[argIndex++]);
if(mapUpdate <= 0)
{
printf("map_update should be > 0\n");
showUsage();
}
printf("Camera rate = %d Hz\n", cameraRate);
printf("Odometry update rate = %d Hz\n", cameraRate/odomUpdate);
printf("Map update rate = %d Hz\n", (cameraRate/odomUpdate)/mapUpdate);
std::string calibrationDir = argv[argIndex++];
std::string calibrationName = argv[argIndex++];
std::string pathRGBImages = argv[argIndex++];
std::string pathDepthImages = argv[argIndex++];
Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
//CameraStereoImages camera(
// pathLeftImages,
// pathRightImages,
// false, // assume that images are already rectified
// (float)cameraRate,
// opticalRotation);
// CameraFreenect2 camera(0, CameraFreenect2::Type::kTypeColor2DepthSD, 0.0, opticalRotation);
CameraRGBDImages camera(pathRGBImages, pathDepthImages, 1, float(cameraRate), opticalRotation);
std::ofstream stats_file;
stats_file.open("statistics.txt");
stats_file << "[" << std::endl;
if(camera.init(calibrationDir, calibrationName))
{
OdometryF2M odom;
Rtabmap rtabmap;
rtabmap.init(pm);
QApplication app(argc, argv);
MapBuilder mapBuilder;
mapBuilder.show();
QApplication::processEvents();
SensorData data = camera.takeImage();
int cameraIteration = 0;
int odometryIteration = 0;
printf("Press \"Space\" in the window to pause\n");
while(data.isValid() && mapBuilder.isVisible())
{
if(cameraIteration++ % odomUpdate == 0)
{
OdometryInfo info;
Transform pose = odom.process(data, &info);
if(odometryIteration++ % mapUpdate == 0)
{
if(rtabmap.process(data, pose))
{
mapBuilder.processStatistics(rtabmap.getStatistics());
if(rtabmap.getLoopClosureId() > 0)
{
printf("Loop closure detected!\n");
}
std::map<std::string, float> statz = rtabmap.getStatistics().data();
stats_file << " {" << std::endl;
for(std::map<std::string,float>::iterator it = statz.begin(); it != statz.end(); ++it) {
std::string name = it->first;
std::replace(name.begin(), name.end(), '/', '.');
stats_file << " \"" << name << "\": " << it->second << "," << std::endl;
}
stats_file << " }," << std::endl;
}
}
mapBuilder.processOdometry(data, pose, info);
}
QApplication::processEvents();
while(mapBuilder.isPaused() && mapBuilder.isVisible())
{
uSleep(100);
QApplication::processEvents();
}
data = camera.takeImage();
}
if(mapBuilder.isVisible())
{
printf("Processed all frames\n");
app.exec();
}
}
else
{
UERROR("Camera init failed!");
}
stats_file << "]" << std::endl;
stats_file.close();
return 0;
}
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 timeThreshold = 0.0;
float rate = 0.0;
int loopDataset = 0;
int repeat = 0;
bool createGT = false;
int imageWidth = 0;
int imageHeight = 0;
int startAt = 1;
ParametersMap pm;
ULogger::Level logLevel = ULogger::kError;
ULogger::Level exitLevel = ULogger::kFatal;
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], "-t") == 0)
{
++i;
if(i < argc)
{
timeThreshold = std::atof(argv[i]);
if(timeThreshold < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-rate") == 0)
{
++i;
if(i < argc)
{
rate = std::atof(argv[i]);
if(rate < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-rateHz") == 0)
{
++i;
if(i < argc)
{
rate = std::atof(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], "-image_width") == 0)
{
++i;
if(i < argc)
{
imageWidth = std::atoi(argv[i]);
if(imageWidth < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-image_height") == 0)
{
++i;
if(i < argc)
{
imageHeight = std::atoi(argv[i]);
if(imageHeight < 0)
{
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], "-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;
}
// 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();
}
else if((imageWidth && imageHeight == 0) ||
(imageHeight && imageWidth == 0))
{
printf("If imageWidth is set, imageHeight must be too.\n");
showUsage();
}
UTimer timer;
timer.start();
std::queue<double> iterationMeanTime;
Camera * camera = 0;
if(UDirectory::exists(path))
{
camera = new CameraImages(path, startAt, false, 1/rate, imageWidth, imageHeight);
}
else
{
camera = new CameraVideo(path, 1/rate, imageWidth, imageHeight);
}
if(!camera || !camera->init())
{
printf("Camera init failed, using path \"%s\"\n", path.c_str());
exit(1);
}
std::map<int, int> groundTruth;
ULogger::setType(ULogger::kTypeConsole);
//ULogger::setType(ULogger::kTypeFile, rtabmap.getWorkingDir()+"/LogConsole.txt", false);
//ULogger::setBuffered(true);
ULogger::setLevel(logLevel);
ULogger::setExitLevel(exitLevel);
// Create tasks : memory is deleted
Rtabmap rtabmap;
// Disable statistics (we don't need them)
pm.insert(ParametersPair(Parameters::kRtabmapPublishStats(), "false"));
// use default working directory
pm.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), Parameters::defaultRtabmapWorkingDirectory()));
pm.insert(ParametersPair(Parameters::kRGBDEnabled(), "false"));
rtabmap.init(pm);
rtabmap.setTimeThreshold(timeThreshold); // in ms
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", timeThreshold);
printf(" Image rate = %1.2f s (%1.2f Hz)\n", rate, 1/rate);
printf(" Repeating data set = %s\n", repeat?"true":"false");
printf(" Camera width=%d, height=%d (0 is default)\n", imageWidth, imageHeight);
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", (rtabmap.getWorkingDir()+Parameters::getDefaultDatabaseName()).c_str());
}
printf("\nProcessing images...\n");
UTimer iterationTimer;
UTimer rtabmapTimer;
int imagesProcessed = 0;
std::list<std::vector<float> > teleopActions;
while(loopDataset <= repeat && g_forever)
{
cv::Mat img = camera->takeImage();
int i=0;
double maxIterationTime = 0.0;
int maxIterationTimeId = 0;
while(!img.empty() && g_forever)
{
++imagesProcessed;
iterationTimer.start();
rtabmapTimer.start();
rtabmap.process(img);
double rtabmapTime = rtabmapTimer.elapsed();
loopClosureId = rtabmap.getLoopClosureId();
if(rtabmap.getLoopClosureId())
{
++countLoopDetected;
}
img = 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.getLcHypValue(), rtabmapTime, iterationTime);
}
else if(rtabmap.getRetrievedId())
{
printf(" iteration(%d) high(%d) hyp(%.2f) time=%fs/%fs\n",
count, rtabmap.getRetrievedId(), rtabmap.getLcHypValue(), rtabmapTime, iterationTime);
}
else
{
printf(" iteration(%d) time=%fs/%fs\n", count, rtabmapTime, iterationTime);
}
if(timeThreshold && rtabmapTime > timeThreshold*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", (rtabmap.getWorkingDir()+GENERATED_GT_NAME).c_str(), groundTruthMat.rows);
IplImage img = groundTruthMat;
cvSaveImage((rtabmap.getWorkingDir()+GENERATED_GT_NAME).c_str(), &img);
printf(" Creating ground truth file = %fs\n", timer.ticks());
}
if(camera)
{
delete camera;
camera = 0 ;
}
rtabmap.close();
printf(" Cleanup time = %fs\n", timer.ticks());
return 0;
}
int main(int argc, char * argv[])
{
signal(SIGABRT, &sighandler);
signal(SIGTERM, &sighandler);
signal(SIGINT, &sighandler);
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kError);
ParametersMap customParameters = Parameters::parseArguments(argc, argv);
if(argc < 3)
{
showUsage();
}
bool assemble2dMap = false;
bool assemble3dMap = false;
bool assemble2dOctoMap = false;
bool assemble3dOctoMap = false;
bool useDatabaseRate = false;
ParametersMap configParameters;
for(int i=1; i<argc-2; ++i)
{
if(strcmp(argv[i], "-r") == 0 || strcmp(argv[i], "--r") == 0)
{
useDatabaseRate = true;
printf("Using database stamps as input rate.\n");
}
else if (strcmp(argv[i], "-c") == 0 || strcmp(argv[i], "--c") == 0)
{
++i;
if (i < argc - 2 && UFile::exists(argv[i]) && UFile::getExtension(argv[i]).compare("ini") == 0)
{
Parameters::readINI(argv[i], configParameters);
printf("Using %d parameters from config file \"%s\"\n", (int)configParameters.size(), argv[i]);
}
else if(i < argc - 2)
{
printf("Config file \"%s\" is not valid or doesn't exist!\n", argv[i]);
}
else
{
printf("Config file is not set!\n");
}
}
else if(strcmp(argv[i], "-g2") == 0 || strcmp(argv[i], "--g2") == 0)
{
assemble2dMap = true;
printf("2D occupancy grid will be assembled (-g2 option).\n");
}
else if(strcmp(argv[i], "-g3") == 0 || strcmp(argv[i], "--g3") == 0)
{
assemble3dMap = true;
printf("3D cloud map will be assembled (-g3 option).\n");
}
else if(strcmp(argv[i], "-o2") == 0 || strcmp(argv[i], "--o2") == 0)
{
#ifdef RTABMAP_OCTOMAP
assemble2dOctoMap = true;
printf("OctoMap will be assembled (-o2 option).\n");
#else
printf("RTAB-Map is not built with OctoMap support, cannot set -o2 option!\n");
#endif
}
else if(strcmp(argv[i], "-o3") == 0 || strcmp(argv[i], "--o3") == 0)
{
#ifdef RTABMAP_OCTOMAP
assemble3dOctoMap = true;
printf("OctoMap will be assembled (-o3 option).\n");
#else
printf("RTAB-Map is not built with OctoMap support, cannot set -o3 option!\n");
#endif
}
}
std::string inputDatabasePath = uReplaceChar(argv[argc-2], '~', UDirectory::homeDir());
std::string outputDatabasePath = uReplaceChar(argv[argc-1], '~', UDirectory::homeDir());
std::list<std::string> databases = uSplit(inputDatabasePath, ';');
if (databases.empty())
{
printf("No input database \"%s\" detected!\n", inputDatabasePath.c_str());
return -1;
}
for (std::list<std::string>::iterator iter = databases.begin(); iter != databases.end(); ++iter)
{
if (!UFile::exists(*iter))
{
printf("Input database \"%s\" doesn't exist!\n", iter->c_str());
return -1;
}
if (UFile::getExtension(*iter).compare("db") != 0)
{
printf("File \"%s\" is not a database format (*.db)!\n", iter->c_str());
return -1;
}
}
if(UFile::getExtension(outputDatabasePath).compare("db") != 0)
{
printf("File \"%s\" is not a database format (*.db)!\n", outputDatabasePath.c_str());
return -1;
}
if(UFile::exists(outputDatabasePath))
{
UFile::erase(outputDatabasePath);
}
// Get parameters of the first database
DBDriver * dbDriver = DBDriver::create();
if(!dbDriver->openConnection(databases.front(), false))
{
printf("Failed opening input database!\n");
delete dbDriver;
return -1;
}
ParametersMap parameters = dbDriver->getLastParameters();
if(parameters.empty())
{
printf("WARNING: Failed getting parameters from database, reprocessing will be done with default parameters! Database version may be too old (%s).\n", dbDriver->getDatabaseVersion().c_str());
}
if(customParameters.size())
{
printf("Custom parameters:\n");
for(ParametersMap::iterator iter=customParameters.begin(); iter!=customParameters.end(); ++iter)
{
printf(" %s\t= %s\n", iter->first.c_str(), iter->second.c_str());
}
}
uInsert(parameters, configParameters);
uInsert(parameters, customParameters);
bool incrementalMemory = Parameters::defaultMemIncrementalMemory();
Parameters::parse(parameters, Parameters::kMemIncrementalMemory(), incrementalMemory);
int totalIds = 0;
std::set<int> ids;
dbDriver->getAllNodeIds(ids);
if(ids.empty())
{
printf("Input database doesn't have any nodes saved in it.\n");
dbDriver->closeConnection(false);
delete dbDriver;
return -1;
}
if(!(!incrementalMemory && databases.size() > 1))
{
totalIds = ids.size();
}
dbDriver->closeConnection(false);
// Count remaining ids in the other databases
for (std::list<std::string>::iterator iter = ++databases.begin(); iter != databases.end(); ++iter)
{
if (!dbDriver->openConnection(*iter, false))
{
printf("Failed opening input database!\n");
delete dbDriver;
return -1;
}
ids.clear();
dbDriver->getAllNodeIds(ids);
totalIds += ids.size();
dbDriver->closeConnection(false);
}
delete dbDriver;
dbDriver = 0;
std::string workingDirectory = UDirectory::getDir(outputDatabasePath);
printf("Set working directory to \"%s\".\n", workingDirectory.c_str());
uInsert(parameters, ParametersPair(Parameters::kRtabmapWorkingDirectory(), workingDirectory));
uInsert(parameters, ParametersPair(Parameters::kRtabmapPublishStats(), "true")); // to log status below
if(!incrementalMemory && databases.size() > 1)
{
UFile::copy(databases.front(), outputDatabasePath);
printf("Parameter \"%s\" is set to false, initializing RTAB-Map with \"%s\" for localization...\n", Parameters::kMemIncrementalMemory().c_str(), databases.front().c_str());
databases.pop_front();
inputDatabasePath = uJoin(databases, ";");
}
Rtabmap rtabmap;
rtabmap.init(parameters, outputDatabasePath);
bool rgbdEnabled = Parameters::defaultRGBDEnabled();
Parameters::parse(parameters, Parameters::kRGBDEnabled(), rgbdEnabled);
bool odometryIgnored = !rgbdEnabled;
DBReader dbReader(inputDatabasePath, useDatabaseRate?-1:0, odometryIgnored);
dbReader.init();
OccupancyGrid grid(parameters);
grid.setCloudAssembling(assemble3dMap);
#ifdef RTABMAP_OCTOMAP
OctoMap octomap(parameters);
#endif
printf("Reprocessing data of \"%s\"...\n", inputDatabasePath.c_str());
std::map<std::string, float> globalMapStats;
int processed = 0;
CameraInfo info;
SensorData data = dbReader.takeImage(&info);
Transform lastLocalizationPose = info.odomPose;
while(data.isValid() && g_loopForever)
{
UTimer iterationTime;
std::string status;
if(!odometryIgnored && info.odomPose.isNull())
{
printf("Skipping node %d as it doesn't have odometry pose set.\n", data.id());
}
else
{
if(!odometryIgnored && !info.odomCovariance.empty() && info.odomCovariance.at<double>(0,0)>=9999)
{
printf("High variance detected, triggering a new map...\n");
if(!incrementalMemory && processed>0)
{
showLocalizationStats();
lastLocalizationPose = info.odomPose;
}
if(incrementalMemory)
{
rtabmap.triggerNewMap();
}
}
UTimer t;
if(!rtabmap.process(data, info.odomPose, info.odomCovariance, info.odomVelocity, globalMapStats))
{
printf("Failed processing node %d.\n", data.id());
globalMapStats.clear();
}
else if(assemble2dMap || assemble3dMap || assemble2dOctoMap || assemble3dOctoMap)
{
globalMapStats.clear();
double timeRtabmap = t.ticks();
double timeUpdateInit = 0.0;
double timeUpdateGrid = 0.0;
#ifdef RTABMAP_OCTOMAP
double timeUpdateOctoMap = 0.0;
#endif
const rtabmap::Statistics & stats = rtabmap.getStatistics();
if(stats.poses().size() && stats.getLastSignatureData().id())
{
int id = stats.poses().rbegin()->first;
if(id == stats.getLastSignatureData().id() &&
stats.getLastSignatureData().sensorData().gridCellSize() > 0.0f)
{
bool updateGridMap = false;
bool updateOctoMap = false;
if((assemble2dMap || assemble3dMap) && grid.addedNodes().find(id) == grid.addedNodes().end())
{
updateGridMap = true;
}
#ifdef RTABMAP_OCTOMAP
if((assemble2dOctoMap || assemble3dOctoMap) && octomap.addedNodes().find(id) == octomap.addedNodes().end())
{
updateOctoMap = true;
}
#endif
if(updateGridMap || updateOctoMap)
{
cv::Mat ground, obstacles, empty;
stats.getLastSignatureData().sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
timeUpdateInit = t.ticks();
if(updateGridMap)
{
grid.addToCache(id, ground, obstacles, empty);
grid.update(stats.poses());
timeUpdateGrid = t.ticks() + timeUpdateInit;
}
#ifdef RTABMAP_OCTOMAP
if(updateOctoMap)
{
const cv::Point3f & viewpoint = stats.getLastSignatureData().sensorData().gridViewPoint();
octomap.addToCache(id, ground, obstacles, empty, viewpoint);
octomap.update(stats.poses());
timeUpdateOctoMap = t.ticks() + timeUpdateInit;
}
#endif
}
}
}
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/GridUpdate/ms"), timeUpdateGrid*1000.0f));
#ifdef RTABMAP_OCTOMAP
//Simulate publishing
double timePub2dOctoMap = 0.0;
double timePub3dOctoMap = 0.0;
if(assemble2dOctoMap)
{
float xMin, yMin, size;
octomap.createProjectionMap(xMin, yMin, size);
timePub2dOctoMap = t.ticks();
}
if(assemble3dOctoMap)
{
octomap.createCloud();
timePub3dOctoMap = t.ticks();
}
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapUpdate/ms"), timeUpdateOctoMap*1000.0f));
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapProjection/ms"), timePub2dOctoMap*1000.0f));
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctomapToCloud/ms"), timePub3dOctoMap*1000.0f));
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeUpdateOctoMap+timePub2dOctoMap+timePub3dOctoMap+timeRtabmap)*1000.0f));
#else
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeRtabmap)*1000.0f));
#endif
}
}
const rtabmap::Statistics & stats = rtabmap.getStatistics();
int loopId = stats.loopClosureId() > 0? stats.loopClosureId(): stats.proximityDetectionId() > 0?stats.proximityDetectionId() :0;
int landmarkId = (int)uValue(stats.data(), rtabmap::Statistics::kLoopLandmark_detected(), 0.0f);
int refMapId = stats.refImageMapId();
++totalFrames;
if (loopId>0)
{
++loopCount;
int loopMapId = stats.loopClosureId() > 0? stats.loopClosureMapId(): stats.proximityDetectionMapId();
printf("Processed %d/%d nodes [%d]... %dms Loop on %d [%d]\n", ++processed, totalIds, refMapId, int(iterationTime.ticks() * 1000), loopId, loopMapId);
}
else if(landmarkId != 0)
{
printf("Processed %d/%d nodes [%d]... %dms Loop on landmark %d\n", ++processed, totalIds, refMapId, int(iterationTime.ticks() * 1000), landmarkId);
}
else
{
printf("Processed %d/%d nodes [%d]... %dms\n", ++processed, totalIds, refMapId, int(iterationTime.ticks() * 1000));
}
// Here we accumulate statistics about distance from last localization
if(!incrementalMemory &&
!lastLocalizationPose.isNull() &&
!info.odomPose.isNull())
{
if(loopId>0 || landmarkId != 0)
{
previousLocalizationDistances.push_back(lastLocalizationPose.getDistance(info.odomPose));
lastLocalizationPose = info.odomPose;
}
}
if(!incrementalMemory)
{
float totalTime = uValue(stats.data(), rtabmap::Statistics::kTimingTotal(), 0.0f);
localizationTime.push_back(totalTime);
if(loopId>0)
{
localizationDistances.push_back(stats.loopClosureTransform().getNorm());
}
}
Transform odomPose = info.odomPose;
data = dbReader.takeImage(&info);
if(!incrementalMemory &&
!odomPose.isNull() &&
!info.odomPose.isNull())
{
odomDistances.push_back(odomPose.getDistance(info.odomPose));
}
}
if(!incrementalMemory)
{
showLocalizationStats();
}
else
{
printf("Total loop closures = %d\n", loopCount);
}
printf("Closing database \"%s\"...\n", outputDatabasePath.c_str());
rtabmap.close(true);
printf("Closing database \"%s\"... done!\n", outputDatabasePath.c_str());
if(assemble2dMap)
{
std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_map.pgm";
float xMin,yMin;
cv::Mat map = grid.getMap(xMin, yMin);
if(!map.empty())
{
cv::Mat map8U(map.rows, map.cols, CV_8U);
//convert to gray scaled map
for (int i = 0; i < map.rows; ++i)
{
for (int j = 0; j < map.cols; ++j)
{
char v = map.at<char>(i, j);
unsigned char gray;
if(v == 0)
{
gray = 178;
}
else if(v == 100)
{
gray = 0;
}
else // -1
{
gray = 89;
}
map8U.at<unsigned char>(i, j) = gray;
}
}
if(cv::imwrite(outputPath, map8U))
{
printf("Saving occupancy grid \"%s\"... done!\n", outputPath.c_str());
}
else
{
printf("Saving occupancy grid \"%s\"... failed!\n", outputPath.c_str());
}
}
else
{
printf("2D map is empty! Cannot save it!\n");
}
}
if(assemble3dMap)
{
std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_obstacles.pcd";
if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapObstacles()) == 0)
{
printf("Saving 3d obstacles \"%s\"... done!\n", outputPath.c_str());
}
else
{
printf("Saving 3d obstacles \"%s\"... failed!\n", outputPath.c_str());
}
if(grid.getMapGround()->size())
{
outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_ground.pcd";
if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapGround()) == 0)
{
printf("Saving 3d ground \"%s\"... done!\n", outputPath.c_str());
}
else
{
printf("Saving 3d ground \"%s\"... failed!\n", outputPath.c_str());
}
}
if(grid.getMapEmptyCells()->size())
{
outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_empty.pcd";
if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapEmptyCells()) == 0)
{
printf("Saving 3d empty cells \"%s\"... done!\n", outputPath.c_str());
}
else
{
printf("Saving 3d empty cells \"%s\"... failed!\n", outputPath.c_str());
}
}
}
#ifdef RTABMAP_OCTOMAP
if(assemble2dOctoMap)
{
std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap.pgm";
float xMin,yMin,cellSize;
cv::Mat map = octomap.createProjectionMap(xMin, yMin, cellSize);
if(!map.empty())
{
cv::Mat map8U(map.rows, map.cols, CV_8U);
//convert to gray scaled map
for (int i = 0; i < map.rows; ++i)
{
for (int j = 0; j < map.cols; ++j)
{
char v = map.at<char>(i, j);
unsigned char gray;
if(v == 0)
{
gray = 178;
}
else if(v == 100)
{
gray = 0;
}
else // -1
{
gray = 89;
}
map8U.at<unsigned char>(i, j) = gray;
}
}
if(cv::imwrite(outputPath, map8U))
{
printf("Saving octomap 2D projection \"%s\"... done!\n", outputPath.c_str());
}
else
{
printf("Saving octomap 2D projection \"%s\"... failed!\n", outputPath.c_str());
}
}
else
{
printf("OctoMap 2D projection map is empty! Cannot save it!\n");
}
}
if(assemble3dOctoMap)
{
std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap_occupied.pcd";
std::vector<int> obstacles, emptySpace, ground;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap.createCloud(0, &obstacles, &emptySpace, &ground);
if(pcl::io::savePCDFile(outputPath, *cloud, obstacles, true) == 0)
{
printf("Saving obstacles cloud \"%s\"... done!\n", outputPath.c_str());
}
else
{
printf("Saving obstacles cloud \"%s\"... failed!\n", outputPath.c_str());
}
if(ground.size())
{
outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap_ground.pcd";
if(pcl::io::savePCDFile(outputPath, *cloud, ground, true) == 0)
{
printf("Saving empty space cloud \"%s\"... done!\n", outputPath.c_str());
}
else
{
printf("Saving empty space cloud \"%s\"... failed!\n", outputPath.c_str());
}
}
if(emptySpace.size())
{
outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap_empty.pcd";
if(pcl::io::savePCDFile(outputPath, *cloud, emptySpace, true) == 0)
{
printf("Saving empty space cloud \"%s\"... done!\n", outputPath.c_str());
}
else
{
printf("Saving empty space cloud \"%s\"... failed!\n", outputPath.c_str());
}
}
}
#endif
return 0;
}