31 #ifdef RTABMAP_OCTOMAP 46 #include <pcl/io/pcd_io.h> 54 " rtabmap-reprocess [options] \"input.db\" \"output.db\"\n" 55 " rtabmap-reprocess [options] \"input1.db;input2.db;input3.db\" \"output.db\"\n" 56 " For the second example, only parameters from the first database are used.\n" 57 " If Mem/IncrementalMemory is false, RTAB-Map is initialized with the first input database.\n" 58 " To see warnings when loop closures are rejected, add \"--uwarn\" argument.\n" 60 " -r Use database stamps as input rate.\n" 61 " -skip # Skip # frames after each processed frame (default 0=don't skip any frames).\n" 62 " -c \"path.ini\" Configuration file, overwriting parameters read \n" 63 " from the database. If custom parameters are also set as \n" 64 " arguments, they overwrite those in config file and the database.\n" 65 " -start # Start from this node ID.\n" 66 " -stop # Last node to process.\n" 67 " -g2 Assemble 2D occupancy grid map and save it to \"[output]_map.pgm\".\n" 68 " -g3 Assemble 3D cloud map and save it to \"[output]_map.pcd\".\n" 69 " -o2 Assemble OctoMap 2D projection and save it to \"[output]_octomap.pgm\".\n" 70 " -o3 Assemble OctoMap 3D cloud and save it to \"[output]_octomap.pcd\".\n" 71 " -p Save odometry and localization poses (*.g2o).\n" 72 " -scan_from_depth Generate scans from depth images (overwrite previous\n" 73 " scans if they exist).\n" 74 " -scan_downsample # Downsample input scans.\n" 75 " -scan_range_min #.# Filter input scans with minimum range (m).\n" 76 " -scan_range_max #.# Filter input scans with maximum range (m).\n" 77 " -scan_voxel_size #.# Voxel filter input scans (m).\n" 78 " -scan_normal_k # Compute input scan normals (k-neighbors approach).\n" 79 " -scan_normal_radius #.# Compute input scan normals (radius(m)-neighbors approach).\n\n" 89 printf(
"\nSignal %d caught...\n", sig);
119 printf(
"Average localization time = %f ms (stddev=%f ms)\n", m, stddev);
135 float mA =
uMean(localizationAngleVariations);
136 float maxA =
uMax(localizationAngleVariations);
137 float varA =
uVariance(localizationAngleVariations, mA);
141 stddevA =
sqrt(varA);
143 printf(
"Average localization variations = %f m, %f deg (stddev=%f m, %f deg) (max=%f m, %f deg)\n", m, mA, stddev, stddevA, max, maxA);
154 printf(
"Average distance from previous localization = %f m (stddev=%f m)\n", m, stddev);
165 printf(
"Average odometry distances = %f m (stddev=%f m)\n", m, stddev);
170 std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3);
175 printf(
"Exported %s and %s\n", oName.c_str(), lName.c_str());
210 bool assemble2dMap =
false;
211 bool assemble3dMap =
false;
212 bool assemble2dOctoMap =
false;
213 bool assemble3dOctoMap =
false;
214 bool useDatabaseRate =
false;
217 int framesToSkip = 0;
218 bool scanFromDepth =
false;
219 int scanDecimation = 1;
220 float scanRangeMin = 0.0f;
221 float scanRangeMax = 0.0f;
222 float scanVoxelSize = 0;
224 float scanNormalRadius = 0.0f;
226 for(
int i=1; i<argc-2; ++i)
228 if(strcmp(argv[i],
"-r") == 0 || strcmp(argv[i],
"--r") == 0)
230 useDatabaseRate =
true;
231 printf(
"Using database stamps as input rate.\n");
233 else if (strcmp(argv[i],
"-c") == 0 || strcmp(argv[i],
"--c") == 0)
239 printf(
"Using %d parameters from config file \"%s\"\n", (
int)configParameters.size(), argv[i]);
241 else if(i < argc - 2)
243 printf(
"Config file \"%s\" is not valid or doesn't exist!\n", argv[i]);
248 printf(
"Config file is not set!\n");
252 else if (strcmp(argv[i],
"-start") == 0 || strcmp(argv[i],
"--start") == 0)
257 startId = atoi(argv[i]);
258 printf(
"Start at node ID = %d.\n", startId);
262 printf(
"-start option requires a value\n");
266 else if (strcmp(argv[i],
"-stop") == 0 || strcmp(argv[i],
"--stop") == 0)
271 stopId = atoi(argv[i]);
272 printf(
"Stop at node ID = %d.\n", stopId);
276 printf(
"-stop option requires a value\n");
280 else if (strcmp(argv[i],
"-skip") == 0 || strcmp(argv[i],
"--skip") == 0)
285 framesToSkip = atoi(argv[i]);
286 printf(
"Will skip %d frames.\n", framesToSkip);
290 printf(
"-skip option requires a value\n");
294 else if(strcmp(argv[i],
"-p") == 0 || strcmp(argv[i],
"--p") == 0)
297 printf(
"Odometry trajectory and localization poses will be exported in g2o format (-p option).\n");
299 else if(strcmp(argv[i],
"-g2") == 0 || strcmp(argv[i],
"--g2") == 0)
301 assemble2dMap =
true;
302 printf(
"2D occupancy grid will be assembled (-g2 option).\n");
304 else if(strcmp(argv[i],
"-g3") == 0 || strcmp(argv[i],
"--g3") == 0)
306 assemble3dMap =
true;
307 printf(
"3D cloud map will be assembled (-g3 option).\n");
309 else if(strcmp(argv[i],
"-o2") == 0 || strcmp(argv[i],
"--o2") == 0)
311 #ifdef RTABMAP_OCTOMAP 312 assemble2dOctoMap =
true;
313 printf(
"OctoMap will be assembled (-o2 option).\n");
315 printf(
"RTAB-Map is not built with OctoMap support, cannot set -o2 option!\n");
318 else if(strcmp(argv[i],
"-o3") == 0 || strcmp(argv[i],
"--o3") == 0)
320 #ifdef RTABMAP_OCTOMAP 321 assemble3dOctoMap =
true;
322 printf(
"OctoMap will be assembled (-o3 option).\n");
324 printf(
"RTAB-Map is not built with OctoMap support, cannot set -o3 option!\n");
327 else if (strcmp(argv[i],
"-scan_from_depth") == 0 || strcmp(argv[i],
"--scan_from_depth") == 0)
329 scanFromDepth =
true;
331 else if (strcmp(argv[i],
"-scan_downsample") == 0 || strcmp(argv[i],
"--scan_downsample") == 0 ||
332 strcmp(argv[i],
"-scan_decimation") == 0 || strcmp(argv[i],
"--scan_decimation") == 0)
337 scanDecimation = atoi(argv[i]);
338 printf(
"Scan from depth decimation = %d.\n", scanDecimation);
342 printf(
"-scan_downsample option requires 1 value\n");
346 else if (strcmp(argv[i],
"-scan_range_min") == 0 || strcmp(argv[i],
"--scan_range_min") == 0)
351 scanRangeMin = atof(argv[i]);
352 printf(
"Scan range min = %f m.\n", scanRangeMin);
356 printf(
"-scan_range_min option requires 1 value\n");
360 else if (strcmp(argv[i],
"-scan_range_max") == 0 || strcmp(argv[i],
"--scan_range_max") == 0)
365 scanRangeMax = atof(argv[i]);
366 printf(
"Scan range max = %f m.\n", scanRangeMax);
370 printf(
"-scan_range_max option requires 1 value\n");
374 else if (strcmp(argv[i],
"-scan_voxel_size") == 0 || strcmp(argv[i],
"--scan_voxel_size") == 0)
379 scanVoxelSize = atof(argv[i]);
380 printf(
"Scan voxel size = %f m.\n", scanVoxelSize);
384 printf(
"-scan_voxel_size option requires 1 value\n");
388 else if (strcmp(argv[i],
"-scan_normal_k") == 0 || strcmp(argv[i],
"--scan_normal_k") == 0)
393 scanNormalK = atoi(argv[i]);
394 printf(
"Scan normal k = %d.\n", scanNormalK);
398 printf(
"-scan_normal_k option requires 1 value\n");
402 else if (strcmp(argv[i],
"-scan_normal_radius") == 0 || strcmp(argv[i],
"--scan_normal_radius") == 0)
407 scanNormalRadius = atof(argv[i]);
408 printf(
"Scan normal radius = %f m.\n", scanNormalRadius);
412 printf(
"-scan_normal_radius option requires 1 value\n");
421 std::list<std::string> databases =
uSplit(inputDatabasePath,
';');
422 if (databases.empty())
424 printf(
"No input database \"%s\" detected!\n", inputDatabasePath.c_str());
427 for (std::list<std::string>::iterator iter = databases.begin(); iter != databases.end(); ++iter)
431 printf(
"Input database \"%s\" doesn't exist!\n", iter->c_str());
437 printf(
"File \"%s\" is not a database format (*.db)!\n", iter->c_str());
444 printf(
"File \"%s\" is not a database format (*.db)!\n", outputDatabasePath.c_str());
457 printf(
"Failed opening input database!\n");
464 parameters.insert(
ParametersPair(Parameters::kDbTargetVersion(), targetVersion));
465 if(parameters.empty())
467 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());
469 if(customParameters.size())
471 printf(
"Custom parameters:\n");
472 for(ParametersMap::iterator iter=customParameters.begin(); iter!=customParameters.end(); ++iter)
474 printf(
" %s\t= %s\n", iter->first.c_str(), iter->second.c_str());
477 if((configParameters.find(Parameters::kKpDetectorStrategy())!=configParameters.end() ||
478 configParameters.find(Parameters::kVisFeatureType())!=configParameters.end() ||
479 customParameters.find(Parameters::kKpDetectorStrategy())!=customParameters.end() ||
480 customParameters.find(Parameters::kVisFeatureType())!=customParameters.end()) &&
481 configParameters.find(Parameters::kMemUseOdomFeatures())==configParameters.end() &&
482 customParameters.find(Parameters::kMemUseOdomFeatures())==customParameters.end())
484 bool useOdomFeatures = Parameters::defaultMemUseOdomFeatures();
485 Parameters::parse(parameters, Parameters::kMemUseOdomFeatures(), useOdomFeatures);
488 printf(
"[Warning] %s and/or %s are overwritten but parameter %s is true in the opened database. " 489 "Setting it to false for convenience to use the new selected feature detector. Set %s " 490 "explicitly to suppress this warning.\n",
491 Parameters::kKpDetectorStrategy().c_str(),
492 Parameters::kVisFeatureType().c_str(),
493 Parameters::kMemUseOdomFeatures().c_str(),
494 Parameters::kMemUseOdomFeatures().c_str());
498 uInsert(parameters, configParameters);
499 uInsert(parameters, customParameters);
501 bool incrementalMemory = Parameters::defaultMemIncrementalMemory();
502 Parameters::parse(parameters, Parameters::kMemIncrementalMemory(), incrementalMemory);
510 printf(
"Input database doesn't have any nodes saved in it.\n");
515 if(!(!incrementalMemory && databases.size() > 1))
517 totalIds = ids.size();
522 for (std::list<std::string>::iterator iter = ++databases.begin(); iter != databases.end(); ++iter)
526 printf(
"Failed opening input database!\n");
532 totalIds += ids.size();
540 totalIds/=framesToSkip+1;
544 printf(
"Set working directory to \"%s\".\n", workingDirectory.c_str());
545 if(!targetVersion.empty())
547 printf(
"Target database version: \"%s\" (set explicitly --%s \"\" to output with latest version.\n", targetVersion.c_str(), Parameters::kDbTargetVersion().c_str());
552 if(!incrementalMemory && databases.size() > 1)
554 UFile::copy(databases.front(), outputDatabasePath);
555 printf(
"Parameter \"%s\" is set to false, initializing RTAB-Map with \"%s\" for localization...\n", Parameters::kMemIncrementalMemory().c_str(), databases.front().c_str());
556 databases.pop_front();
557 inputDatabasePath =
uJoin(databases,
";");
561 rtabmap.
init(parameters, outputDatabasePath);
563 bool rgbdEnabled = Parameters::defaultRGBDEnabled();
565 bool odometryIgnored = !rgbdEnabled;
566 DBReader * dbReader =
new DBReader(inputDatabasePath, useDatabaseRate?-1:0, odometryIgnored,
false,
false, startId, -1, stopId);
571 #ifdef RTABMAP_OCTOMAP 575 float linearUpdate = Parameters::defaultRGBDLinearUpdate();
576 float angularUpdate = Parameters::defaultRGBDAngularUpdate();
580 printf(
"Reprocessing data of \"%s\"...\n", inputDatabasePath.c_str());
581 std::map<std::string, float> globalMapStats;
586 camThread.
setScanParameters(scanFromDepth, scanDecimation, scanRangeMin, scanRangeMax, scanVoxelSize, scanNormalK, scanNormalRadius);
593 bool inMotion =
true;
600 printf(
"Skipping node %d as it doesn't have odometry pose set.\n", data.
id());
606 printf(
"High variance detected, triggering a new map...\n");
607 if(!incrementalMemory && processed>0)
610 lastLocalizationOdomPose = info.
odomPose;
618 printf(
"Failed processing node %d.\n", data.
id());
619 globalMapStats.clear();
621 else if(assemble2dMap || assemble3dMap || assemble2dOctoMap || assemble3dOctoMap)
623 globalMapStats.clear();
624 double timeRtabmap = t.
ticks();
625 double timeUpdateInit = 0.0;
626 double timeUpdateGrid = 0.0;
627 #ifdef RTABMAP_OCTOMAP 628 double timeUpdateOctoMap = 0.0;
633 int id = stats.
poses().rbegin()->first;
637 bool updateGridMap =
false;
638 bool updateOctoMap =
false;
641 updateGridMap =
true;
643 #ifdef RTABMAP_OCTOMAP 644 if((assemble2dOctoMap || assemble3dOctoMap) && octomap.
addedNodes().find(
id) == octomap.
addedNodes().end())
646 updateOctoMap =
true;
649 if(updateGridMap || updateOctoMap)
651 cv::Mat ground, obstacles, empty;
654 timeUpdateInit = t.
ticks();
658 grid.
addToCache(
id, ground, obstacles, empty);
660 timeUpdateGrid = t.
ticks() + timeUpdateInit;
662 #ifdef RTABMAP_OCTOMAP 666 octomap.
addToCache(
id, ground, obstacles, empty, viewpoint);
668 timeUpdateOctoMap = t.
ticks() + timeUpdateInit;
675 globalMapStats.insert(std::make_pair(std::string(
"GlobalGrid/GridUpdate/ms"), timeUpdateGrid*1000.0
f));
676 #ifdef RTABMAP_OCTOMAP 678 double timePub2dOctoMap = 0.0;
679 double timePub3dOctoMap = 0.0;
680 if(assemble2dOctoMap)
682 float xMin, yMin, size;
684 timePub2dOctoMap = t.
ticks();
686 if(assemble3dOctoMap)
689 timePub3dOctoMap = t.
ticks();
692 globalMapStats.insert(std::make_pair(std::string(
"GlobalGrid/OctoMapUpdate/ms"), timeUpdateOctoMap*1000.0f));
693 globalMapStats.insert(std::make_pair(std::string(
"GlobalGrid/OctoMapProjection/ms"), timePub2dOctoMap*1000.0f));
694 globalMapStats.insert(std::make_pair(std::string(
"GlobalGrid/OctomapToCloud/ms"), timePub3dOctoMap*1000.0f));
695 globalMapStats.insert(std::make_pair(std::string(
"GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeUpdateOctoMap+timePub2dOctoMap+timePub3dOctoMap+timeRtabmap)*1000.0f));
697 globalMapStats.insert(std::make_pair(std::string(
"GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeRtabmap)*1000.0f));
705 int landmarkId = (int)
uValue(stats.
data(), rtabmap::Statistics::kLoopLandmark_detected(), 0.0f);
728 printf(
"Processed %d/%d nodes [id=%d map=%d]... %dms %s on %d [%d]\n", ++processed, totalIds, refId, refMapId,
int(iterationTime.
ticks() * 1000), stats.
loopClosureId() > 0?
"Loop":
"Prox", loopId, loopMapId);
730 else if(landmarkId != 0)
732 printf(
"Processed %d/%d nodes [id=%d map=%d]... %dms Loop on landmark %d\n", ++processed, totalIds, refId, refMapId,
int(iterationTime.
ticks() * 1000), landmarkId);
736 printf(
"Processed %d/%d nodes [id=%d map=%d]... %dms\n", ++processed, totalIds, refId, refMapId,
int(iterationTime.
ticks() * 1000));
740 if(!incrementalMemory &&
741 !lastLocalizationOdomPose.
isNull() &&
744 if(loopId>0 || landmarkId != 0)
747 lastLocalizationOdomPose = info.
odomPose;
750 if(!incrementalMemory)
752 float totalTime =
uValue(stats.
data(), rtabmap::Statistics::kTimingTotal(), 0.0f);
754 if(stats.
data().find(Statistics::kLoopOdom_correction_norm()) != stats.
data().end())
776 int skippedFrames = framesToSkip;
777 while(skippedFrames-- > 0)
782 printf(
"High variance detected, triggering a new map...\n");
783 if(!incrementalMemory && processed>0)
786 lastLocalizationOdomPose = info.
odomPose;
801 if(!incrementalMemory &&
808 if(distance < linearUpdate && angle <= angularUpdate)
815 int databasesMerged = 0;
816 if(!incrementalMemory)
824 if(databases.size()>1)
826 std::map<int, Transform> poses;
827 std::multimap<int, Link> constraints;
828 rtabmap.
getGraph(poses, constraints, 0, 1, 0,
false,
false,
false,
false,
false,
false);
829 std::set<int> mapIds;
830 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
838 databasesMerged = mapIds.size();
842 printf(
"Closing database \"%s\"...\n", outputDatabasePath.c_str());
844 printf(
"Closing database \"%s\"... done!\n", outputDatabasePath.c_str());
848 std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) +
"_map.pgm";
850 cv::Mat map = grid.
getMap(xMin, yMin);
853 cv::Mat map8U(map.rows, map.cols, CV_8U);
855 for (
int i = 0; i < map.rows; ++i)
857 for (
int j = 0; j < map.cols; ++j)
859 char v = map.at<
char>(i, j);
873 map8U.at<
unsigned char>(i, j) = gray;
876 if(cv::imwrite(outputPath, map8U))
878 printf(
"Saving occupancy grid \"%s\"... done!\n", outputPath.c_str());
882 printf(
"Saving occupancy grid \"%s\"... failed!\n", outputPath.c_str());
887 printf(
"2D map is empty! Cannot save it!\n");
892 std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) +
"_obstacles.pcd";
893 if(pcl::io::savePCDFileBinary(outputPath, *grid.
getMapObstacles()) == 0)
895 printf(
"Saving 3d obstacles \"%s\"... done!\n", outputPath.c_str());
899 printf(
"Saving 3d obstacles \"%s\"... failed!\n", outputPath.c_str());
903 outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) +
"_ground.pcd";
904 if(pcl::io::savePCDFileBinary(outputPath, *grid.
getMapGround()) == 0)
906 printf(
"Saving 3d ground \"%s\"... done!\n", outputPath.c_str());
910 printf(
"Saving 3d ground \"%s\"... failed!\n", outputPath.c_str());
915 outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) +
"_empty.pcd";
918 printf(
"Saving 3d empty cells \"%s\"... done!\n", outputPath.c_str());
922 printf(
"Saving 3d empty cells \"%s\"... failed!\n", outputPath.c_str());
926 #ifdef RTABMAP_OCTOMAP 927 if(assemble2dOctoMap)
929 std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) +
"_octomap.pgm";
930 float xMin,yMin,cellSize;
934 cv::Mat map8U(map.rows, map.cols, CV_8U);
936 for (
int i = 0; i < map.rows; ++i)
938 for (
int j = 0; j < map.cols; ++j)
940 char v = map.at<
char>(i, j);
954 map8U.at<
unsigned char>(i, j) = gray;
957 if(cv::imwrite(outputPath, map8U))
959 printf(
"Saving octomap 2D projection \"%s\"... done!\n", outputPath.c_str());
963 printf(
"Saving octomap 2D projection \"%s\"... failed!\n", outputPath.c_str());
968 printf(
"OctoMap 2D projection map is empty! Cannot save it!\n");
971 if(assemble3dOctoMap)
973 std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) +
"_octomap_occupied.pcd";
974 std::vector<int> obstacles, emptySpace, ground;
975 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap.
createCloud(0, &obstacles, &emptySpace, &ground);
976 if(pcl::io::savePCDFile(outputPath, *cloud, obstacles,
true) == 0)
978 printf(
"Saving obstacles cloud \"%s\"... done!\n", outputPath.c_str());
982 printf(
"Saving obstacles cloud \"%s\"... failed!\n", outputPath.c_str());
986 outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) +
"_octomap_ground.pcd";
987 if(pcl::io::savePCDFile(outputPath, *cloud, ground,
true) == 0)
989 printf(
"Saving empty space cloud \"%s\"... done!\n", outputPath.c_str());
993 printf(
"Saving empty space cloud \"%s\"... failed!\n", outputPath.c_str());
996 if(emptySpace.size())
998 outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) +
"_octomap_empty.pcd";
999 if(pcl::io::savePCDFile(outputPath, *cloud, emptySpace,
true) == 0)
1001 printf(
"Saving empty space cloud \"%s\"... done!\n", outputPath.c_str());
1005 printf(
"Saving empty space cloud \"%s\"... failed!\n", outputPath.c_str());
1011 return databasesMerged;
static std::string homeDir()
T uVariance(const T *v, unsigned int size, T meanV)
static bool parse(const ParametersMap ¶meters, const std::string &key, bool &value)
bool update(const std::map< int, Transform > &poses)
const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & getMapEmptyCells() const
void uncompressDataConst(cv::Mat *imageRaw, cv::Mat *depthOrRightRaw, LaserScan *laserScanRaw=0, cv::Mat *userDataRaw=0, cv::Mat *groundCellsRaw=0, cv::Mat *obstacleCellsRaw=0, cv::Mat *emptyCellsRaw=0) const
T uMean(const T *v, unsigned int size)
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
void setLaserScan(const LaserScan &laserScan, bool clearPreviousData=true)
int refImageMapId() const
static const char * showUsage()
std::vector< float > localizationAngleVariations
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
void getAllNodeIds(std::set< int > &ids, bool ignoreChildren=false, bool ignoreBadSignatures=false) const
int loopClosureMapId() const
static std::string getDir(const std::string &filePath)
int proximityDetectionId() const
std::pair< std::string, std::string > ParametersPair
const Signature & getLastSignatureData() const
static ParametersMap parseArguments(int argc, char *argv[], bool onlyParameters=false)
static int erase(const std::string &filePath)
int main(int argc, char *argv[])
std::map< std::string, std::string > ParametersMap
GLM_FUNC_DECL genType::value_type distance(genType const &p0, genType const &p1)
float gridCellSize() const
void init(const ParametersMap ¶meters, const std::string &databasePath="", bool loadDatabaseParameters=false)
Basic mathematics functions.
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
std::vector< float > odomVelocity
std::string getExtension()
static void setLevel(ULogger::Level level)
std::vector< float > localizationVariations
void getGraph(std::map< int, Transform > &poses, std::multimap< int, Link > &constraints, bool optimized, bool global, std::map< int, Signature > *signatures=0, bool withImages=false, bool withScan=false, bool withUserData=false, bool withGrid=false, bool withWords=true, bool withGlobalDescriptors=true) const
std::list< std::string > uSplit(const std::string &str, char separator= ' ')
std::vector< float > odomDistances
bool openConnection(const std::string &url, bool overwritten=false)
bool update(const std::map< int, Transform > &poses)
SensorData takeImage(CameraInfo *info=0)
Wrappers of STL for convenient functions.
const std::map< std::string, float > & data() const
void addToCache(int nodeId, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &ground, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &obstacles, const pcl::PointXYZ &viewPoint)
std::string uJoin(const std::list< std::string > &strings, const std::string &separator="")
const std::map< int, Transform > & addedNodes() const
void close(bool databaseSaved=true, const std::string &ouputDatabasePath="")
std::vector< float > previousLocalizationDistances
void closeConnection(bool save=true, const std::string &outputUrl="")
void addToCache(int nodeId, const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty)
void setScanParameters(bool fromDepth, int downsampleStep=1, float rangeMin=0.0f, float rangeMax=0.0f, float voxelSize=0.0f, int normalsK=0, int normalsRadius=0.0f, float groundNormalsUp=0.0f)
const Statistics & getStatistics() const
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
bool process(const SensorData &data, Transform odomPose, const cv::Mat &odomCovariance=cv::Mat::eye(6, 6, CV_64FC1), const std::vector< float > &odomVelocity=std::vector< float >(), const std::map< std::string, float > &externalStats=std::map< std::string, float >())
Main loop of rtabmap.
static void copy(const std::string &from, const std::string &to)
int proximityDetectionMapId() const
std::string UTILITE_EXP uReplaceChar(const std::string &str, char before, char after)
int loopClosureId() const
const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & getMapGround() const
bool RTABMAP_EXP exportPoses(const std::string &filePath, int format, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints=std::multimap< int, Link >(), const std::map< int, double > &stamps=std::map< int, double >(), const ParametersMap ¶meters=ParametersMap())
static DBDriver * create(const ParametersMap ¶meters=ParametersMap())
T uMax(const T *v, unsigned int size, unsigned int &index)
const std::map< int, Transform > & poses() const
pcl::PointCloud< pcl::PointXYZRGB >::Ptr createCloud(unsigned int treeDepth=0, std::vector< int > *obstacleIndices=0, std::vector< int > *emptyIndices=0, std::vector< int > *groundIndices=0, bool originalRefPoints=true, std::vector< int > *frontierIndices=0, std::vector< double > *cloudProb=0) const
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
SensorData & sensorData()
void postUpdate(SensorData *data, CameraInfo *info=0) const
void showLocalizationStats(const std::string &outputDatabasePath)
ParametersMap getLastParameters() const
std::vector< float > localizationTime
const Memory * getMemory() const
std::string getDatabaseVersion() const
std::map< int, Transform > odomTrajectoryPoses
static void readINI(const std::string &configFile, ParametersMap ¶meters, bool modifiedOnly=false)
std::map< int, Transform > localizationPoses
cv::Mat createProjectionMap(float &xMin, float &yMin, float &gridCellSize, float minGridSize=0.0f, unsigned int treeDepth=0)
const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & getMapObstacles() const
const Transform & mapCorrection() const
std::multimap< int, Link > odomTrajectoryLinks
std::string UTILITE_EXP uFormat(const char *fmt,...)
cv::Mat getMap(float &xMin, float &yMin) const
const std::map< int, Transform > & addedNodes() const
const cv::Point3f & gridViewPoint() const
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
void setCloudAssembling(bool enabled)
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
int getMapId(int id, bool lookInDatabase=false) const