41 #include <opencv2/core/version.hpp>
42 #include <pcl/pcl_config.h>
43 #include <opencv2/opencv_modules.hpp>
45 #include <vtkVersion.h>
77 UFATAL(
"Can't get the HOME variable environment!");
84 return RTABMAP_VERSION;
95 std::stringstream output;
96 for(ParametersMap::const_iterator
iter=parameters.begin();
iter!=parameters.end(); ++
iter)
98 if(
iter != parameters.begin())
105 UDEBUG(
"output=%s", output.str().c_str());
111 UDEBUG(
"parameters=%s", parameters.c_str());
113 std::list<std::string> tuplets =
uSplit(parameters,
';');
114 for(std::list<std::string>::iterator
iter=tuplets.begin();
iter!=tuplets.end(); ++
iter)
119 std::string
key =
p.front();
120 std::string
value =
p.back();
123 bool addParameter =
true;
127 addParameter = oldIter->second.first;
130 key = oldIter->second.second;
131 UWARN(
"Parameter migration from \"%s\" to \"%s\" (value=%s).",
132 oldIter->first.c_str(), oldIter->second.second.c_str(),
value.c_str());
134 else if(oldIter->second.second.empty())
136 UWARN(
"Parameter \"%s\" doesn't exist anymore.",
137 oldIter->first.c_str());
141 UWARN(
"Parameter \"%s\" doesn't exist anymore, you may want to use this similar parameter \"%s\":\"%s\".",
150 UWARN(
"Unknown parameter \"%s\"=\"%s\"! The parameter is still added to output map.",
key.c_str(),
value.c_str());
160 std::string group =
uSplit(parameter,
'/').front();
161 return group.compare(
"SURF") == 0 ||
162 group.compare(
"SIFT") == 0 ||
163 group.compare(
"ORB") == 0 ||
164 group.compare(
"FAST") == 0 ||
165 group.compare(
"FREAK") == 0 ||
166 group.compare(
"BRIEF") == 0 ||
167 group.compare(
"GFTT") == 0 ||
168 group.compare(
"BRISK") == 0 ||
169 group.compare(
"KAZE") == 0 ||
170 group.compare(
"SuperPoint") == 0 ||
171 group.compare(
"PyDetector") == 0;
178 for(rtabmap::ParametersMap::iterator
iter=defaultParameters.begin();
iter!=defaultParameters.end(); ++
iter)
180 std::string group =
uSplit(
iter->first,
'/').front();
182 (stereo && group.compare(
"Stereo") == 0) ||
183 (
icp && group.compare(
"Icp") == 0) ||
185 group.compare(
"Reg") == 0 ||
186 group.compare(
"Optimizer") == 0 ||
187 group.compare(
"g2o") == 0 ||
188 group.compare(
"GTSAM") == 0 ||
189 (vis && (group.compare(
"Vis") == 0 || group.compare(
"PyMatcher") == 0 || group.compare(
"GMS") == 0)) ||
190 iter->first.compare(kRtabmapPublishRAMUsage())==0 ||
191 iter->first.compare(kRtabmapImagesAlreadyRectified())==0 ||
192 iter->first.compare(kKpByteToFloat())==0)
194 odomParameters.insert(*
iter);
197 return odomParameters;
204 for(rtabmap::ParametersMap::const_iterator
iter=defaultParameters.begin();
iter!=defaultParameters.end(); ++
iter)
207 std::string group =
uSplit(
iter->first,
'/').front();
208 if(group.compare(groupIn) == 0)
210 parameters.insert(*
iter);
213 UASSERT_MSG(parameters.size(),
uFormat(
"No parameters found for group %s!", groupIn.c_str()).c_str());
220 for(rtabmap::ParametersMap::const_iterator
iter=parameters.begin();
iter!=parameters.end(); ++
iter)
223 std::string group =
uSplit(
iter->first,
'/').front();
224 bool sameGroup = group.compare(groupIn) == 0;
225 if((!remove && sameGroup) || (remove && !sameGroup))
227 output.insert(*
iter);
240 removedParameters_.insert(std::make_pair(
"SIFT/NFeatures", std::make_pair(
false,
"")));
243 removedParameters_.insert(std::make_pair(
"GridGlobal/FullUpdate", std::make_pair(
false,
"")));
246 removedParameters_.insert(std::make_pair(
"Grid/FromDepth", std::make_pair(
true, Parameters::kGridSensor())));
249 removedParameters_.insert(std::make_pair(
"OdomORBSLAM2/VocPath", std::make_pair(
true, Parameters::kOdomORBSLAMVocPath())));
250 removedParameters_.insert(std::make_pair(
"OdomORBSLAM2/Bf", std::make_pair(
true, Parameters::kOdomORBSLAMBf())));
251 removedParameters_.insert(std::make_pair(
"OdomORBSLAM2/ThDepth", std::make_pair(
true, Parameters::kOdomORBSLAMThDepth())));
252 removedParameters_.insert(std::make_pair(
"OdomORBSLAM2/Fps", std::make_pair(
true, Parameters::kOdomORBSLAMFps())));
253 removedParameters_.insert(std::make_pair(
"OdomORBSLAM2/MaxFeatures", std::make_pair(
true, Parameters::kOdomORBSLAMMaxFeatures())));
254 removedParameters_.insert(std::make_pair(
"OdomORBSLAM2/MapSize", std::make_pair(
true, Parameters::kOdomORBSLAMMapSize())));
256 removedParameters_.insert(std::make_pair(
"RGBD/SavedLocalizationIgnored", std::make_pair(
true, Parameters::kRGBDStartAtOrigin())));
258 removedParameters_.insert(std::make_pair(
"Icp/PMForce4DoF", std::make_pair(
true, Parameters::kIcpForce4DoF())));
259 removedParameters_.insert(std::make_pair(
"Icp/PM", std::make_pair(
true, Parameters::kIcpStrategy())));
260 removedParameters_.insert(std::make_pair(
"Icp/PMOutlierRatio", std::make_pair(
true, Parameters::kIcpOutlierRatio())));
263 removedParameters_.insert(std::make_pair(
"SuperGlue/Path", std::make_pair(
true, Parameters::kPyMatcherPath())));
264 removedParameters_.insert(std::make_pair(
"SuperGlue/Iterations", std::make_pair(
true, Parameters::kPyMatcherIterations())));
265 removedParameters_.insert(std::make_pair(
"SuperGlue/MatchThreshold", std::make_pair(
true, Parameters::kPyMatcherThreshold())));
266 removedParameters_.insert(std::make_pair(
"SuperGlue/Cuda", std::make_pair(
true, Parameters::kPyMatcherCuda())));
267 removedParameters_.insert(std::make_pair(
"SuperGlue/Indoor", std::make_pair(
false, Parameters::kPyMatcherModel())));
269 removedParameters_.insert(std::make_pair(
"Vis/CorCrossCheck", std::make_pair(
false, Parameters::kVisCorNNType())));
270 removedParameters_.insert(std::make_pair(
"SPTorch/ModelPath", std::make_pair(
true, Parameters::kSuperPointModelPath())));
271 removedParameters_.insert(std::make_pair(
"SPTorch/Threshold", std::make_pair(
true, Parameters::kSuperPointThreshold())));
272 removedParameters_.insert(std::make_pair(
"SPTorch/NMS", std::make_pair(
true, Parameters::kSuperPointNMS())));
273 removedParameters_.insert(std::make_pair(
"SPTorch/MinDistance", std::make_pair(
true, Parameters::kSuperPointNMSRadius())));
274 removedParameters_.insert(std::make_pair(
"SPTorch/Cuda", std::make_pair(
true, Parameters::kSuperPointCuda())));
277 removedParameters_.insert(std::make_pair(
"RGBD/MaxLocalizationDistance", std::make_pair(
true, Parameters::kRGBDMaxLoopClosureDistance())));
280 removedParameters_.insert(std::make_pair(
"Aruco/Dictionary", std::make_pair(
true, Parameters::kMarkerDictionary())));
281 removedParameters_.insert(std::make_pair(
"Aruco/MarkerLength", std::make_pair(
true, Parameters::kMarkerLength())));
282 removedParameters_.insert(std::make_pair(
"Aruco/MaxDepthError", std::make_pair(
true, Parameters::kMarkerMaxDepthError())));
283 removedParameters_.insert(std::make_pair(
"Aruco/VarianceLinear", std::make_pair(
true, Parameters::kMarkerVarianceLinear())));
284 removedParameters_.insert(std::make_pair(
"Aruco/VarianceAngular", std::make_pair(
true, Parameters::kMarkerVarianceAngular())));
285 removedParameters_.insert(std::make_pair(
"Aruco/CornerRefinementMethod", std::make_pair(
true, Parameters::kMarkerCornerRefinementMethod())));
288 removedParameters_.insert(std::make_pair(
"Grid/OctoMapOccupancyThr", std::make_pair(
true, Parameters::kGridGlobalOccupancyThr())));
291 removedParameters_.insert(std::make_pair(
"Grid/Scan2dMaxFilledRange", std::make_pair(
false, Parameters::kGridRangeMax())));
294 removedParameters_.insert(std::make_pair(
"Grid/ProjRayTracing", std::make_pair(
true, Parameters::kGridRayTracing())));
295 removedParameters_.insert(std::make_pair(
"Grid/DepthMin", std::make_pair(
true, Parameters::kGridRangeMin())));
296 removedParameters_.insert(std::make_pair(
"Grid/DepthMax", std::make_pair(
true, Parameters::kGridRangeMax())));
299 removedParameters_.insert(std::make_pair(
"Reg/VarianceFromInliersCount", std::make_pair(
false,
"")));
300 removedParameters_.insert(std::make_pair(
"Reg/VarianceNormalized", std::make_pair(
false,
"")));
303 removedParameters_.insert(std::make_pair(
"Icp/PointToPlaneNormalNeighbors", std::make_pair(
true, Parameters::kIcpPointToPlaneK())));
307 removedParameters_.insert(std::make_pair(
"Rtabmap/VhStrategy", std::make_pair(
true, Parameters::kVhEpEnabled())));
310 removedParameters_.insert(std::make_pair(
"Grid/FullUpdate", std::make_pair(
false,
"")));
313 removedParameters_.insert(std::make_pair(
"Grid/3DGroundIsObstacle", std::make_pair(
true, Parameters::kGridGroundIsObstacle())));
316 removedParameters_.insert(std::make_pair(
"Optimizer/Slam2D", std::make_pair(
true, Parameters::kRegForce3DoF())));
317 removedParameters_.insert(std::make_pair(
"OdomF2M/FixedMapPath", std::make_pair(
false,
"")));
320 removedParameters_.insert(std::make_pair(
"Grid/FlatObstaclesDetected", std::make_pair(
true, Parameters::kGridFlatObstacleDetected())));
323 removedParameters_.insert(std::make_pair(
"Reg/Force2D", std::make_pair(
true, Parameters::kRegForce3DoF())));
324 removedParameters_.insert(std::make_pair(
"OdomF2M/ScanSubstractRadius", std::make_pair(
true, Parameters::kOdomF2MScanSubtractRadius())));
327 removedParameters_.insert(std::make_pair(
"RGBD/ProximityPathScansMerged", std::make_pair(
false,
"")));
330 removedParameters_.insert(std::make_pair(
"Mem/ImageDecimation", std::make_pair(
true, Parameters::kMemImagePostDecimation())));
333 removedParameters_.insert(std::make_pair(
"OdomLocalMap/HistorySize", std::make_pair(
true, Parameters::kOdomF2MMaxSize())));
334 removedParameters_.insert(std::make_pair(
"OdomLocalMap/FixedMapPath", std::make_pair(
false,
"")));
335 removedParameters_.insert(std::make_pair(
"OdomF2F/GuessMotion", std::make_pair(
true, Parameters::kOdomGuessMotion())));
336 removedParameters_.insert(std::make_pair(
"OdomF2F/KeyFrameThr", std::make_pair(
false, Parameters::kOdomKeyFrameThr())));
339 removedParameters_.insert(std::make_pair(
"OdomBow/LocalHistorySize", std::make_pair(
true, Parameters::kOdomF2MMaxSize())));
340 removedParameters_.insert(std::make_pair(
"OdomBow/FixedLocalMapPath", std::make_pair(
false,
"")));
341 removedParameters_.insert(std::make_pair(
"OdomFlow/KeyFrameThr", std::make_pair(
false, Parameters::kOdomKeyFrameThr())));
342 removedParameters_.insert(std::make_pair(
"OdomFlow/GuessMotion", std::make_pair(
true, Parameters::kOdomGuessMotion())));
344 removedParameters_.insert(std::make_pair(
"Kp/WordsPerImage", std::make_pair(
true, Parameters::kKpMaxFeatures())));
346 removedParameters_.insert(std::make_pair(
"Mem/LocalSpaceLinksKeptInWM", std::make_pair(
false,
"")));
348 removedParameters_.insert(std::make_pair(
"RGBD/PoseScanMatching", std::make_pair(
true, Parameters::kRGBDNeighborLinkRefining())));
350 removedParameters_.insert(std::make_pair(
"Odom/ParticleFiltering", std::make_pair(
false, Parameters::kOdomFilteringStrategy())));
351 removedParameters_.insert(std::make_pair(
"Odom/FeatureType", std::make_pair(
true, Parameters::kVisFeatureType())));
352 removedParameters_.insert(std::make_pair(
"Odom/EstimationType", std::make_pair(
true, Parameters::kVisEstimationType())));
353 removedParameters_.insert(std::make_pair(
"Odom/MaxFeatures", std::make_pair(
true, Parameters::kVisMaxFeatures())));
354 removedParameters_.insert(std::make_pair(
"Odom/InlierDistance", std::make_pair(
true, Parameters::kVisInlierDistance())));
355 removedParameters_.insert(std::make_pair(
"Odom/MinInliers", std::make_pair(
true, Parameters::kVisMinInliers())));
356 removedParameters_.insert(std::make_pair(
"Odom/Iterations", std::make_pair(
true, Parameters::kVisIterations())));
357 removedParameters_.insert(std::make_pair(
"Odom/RefineIterations", std::make_pair(
true, Parameters::kVisRefineIterations())));
358 removedParameters_.insert(std::make_pair(
"Odom/MaxDepth", std::make_pair(
true, Parameters::kVisMaxDepth())));
359 removedParameters_.insert(std::make_pair(
"Odom/RoiRatios", std::make_pair(
true, Parameters::kVisRoiRatios())));
360 removedParameters_.insert(std::make_pair(
"Odom/Force2D", std::make_pair(
true, Parameters::kRegForce3DoF())));
361 removedParameters_.insert(std::make_pair(
"Odom/VarianceFromInliersCount", std::make_pair(
false,
"")));
362 removedParameters_.insert(std::make_pair(
"Odom/PnPReprojError", std::make_pair(
true, Parameters::kVisPnPReprojError())));
363 removedParameters_.insert(std::make_pair(
"Odom/PnPFlags", std::make_pair(
true, Parameters::kVisPnPFlags())));
365 removedParameters_.insert(std::make_pair(
"OdomBow/NNType", std::make_pair(
true, Parameters::kVisCorNNType())));
366 removedParameters_.insert(std::make_pair(
"OdomBow/NNDR", std::make_pair(
true, Parameters::kVisCorNNDR())));
368 removedParameters_.insert(std::make_pair(
"OdomFlow/WinSize", std::make_pair(
true, Parameters::kVisCorFlowWinSize())));
369 removedParameters_.insert(std::make_pair(
"OdomFlow/Iterations", std::make_pair(
true, Parameters::kVisCorFlowIterations())));
370 removedParameters_.insert(std::make_pair(
"OdomFlow/Eps", std::make_pair(
true, Parameters::kVisCorFlowEps())));
371 removedParameters_.insert(std::make_pair(
"OdomFlow/MaxLevel", std::make_pair(
true, Parameters::kVisCorFlowMaxLevel())));
373 removedParameters_.insert(std::make_pair(
"OdomSubPix/WinSize", std::make_pair(
true, Parameters::kVisSubPixWinSize())));
374 removedParameters_.insert(std::make_pair(
"OdomSubPix/Iterations", std::make_pair(
true, Parameters::kVisSubPixIterations())));
375 removedParameters_.insert(std::make_pair(
"OdomSubPix/Eps", std::make_pair(
true, Parameters::kVisSubPixEps())));
377 removedParameters_.insert(std::make_pair(
"LccReextract/Activated", std::make_pair(
true, Parameters::kRGBDLoopClosureReextractFeatures())));
378 removedParameters_.insert(std::make_pair(
"LccReextract/FeatureType", std::make_pair(
false, Parameters::kVisFeatureType())));
379 removedParameters_.insert(std::make_pair(
"LccReextract/MaxWords", std::make_pair(
false, Parameters::kVisMaxFeatures())));
380 removedParameters_.insert(std::make_pair(
"LccReextract/MaxDepth", std::make_pair(
false, Parameters::kVisMaxDepth())));
381 removedParameters_.insert(std::make_pair(
"LccReextract/RoiRatios", std::make_pair(
false, Parameters::kVisRoiRatios())));
382 removedParameters_.insert(std::make_pair(
"LccReextract/NNType", std::make_pair(
false, Parameters::kVisCorNNType())));
383 removedParameters_.insert(std::make_pair(
"LccReextract/NNDR", std::make_pair(
false, Parameters::kVisCorNNDR())));
385 removedParameters_.insert(std::make_pair(
"LccBow/EstimationType", std::make_pair(
false, Parameters::kVisEstimationType())));
386 removedParameters_.insert(std::make_pair(
"LccBow/InlierDistance", std::make_pair(
false, Parameters::kVisInlierDistance())));
387 removedParameters_.insert(std::make_pair(
"LccBow/MinInliers", std::make_pair(
false, Parameters::kVisMinInliers())));
388 removedParameters_.insert(std::make_pair(
"LccBow/Iterations", std::make_pair(
false, Parameters::kVisIterations())));
389 removedParameters_.insert(std::make_pair(
"LccBow/RefineIterations", std::make_pair(
false, Parameters::kVisRefineIterations())));
390 removedParameters_.insert(std::make_pair(
"LccBow/Force2D", std::make_pair(
false, Parameters::kRegForce3DoF())));
391 removedParameters_.insert(std::make_pair(
"LccBow/VarianceFromInliersCount", std::make_pair(
false,
"")));
392 removedParameters_.insert(std::make_pair(
"LccBow/PnPReprojError", std::make_pair(
false, Parameters::kVisPnPReprojError())));
393 removedParameters_.insert(std::make_pair(
"LccBow/PnPFlags", std::make_pair(
false, Parameters::kVisPnPFlags())));
394 removedParameters_.insert(std::make_pair(
"LccBow/EpipolarGeometryVar", std::make_pair(
true, Parameters::kVisEpipolarGeometryVar())));
396 removedParameters_.insert(std::make_pair(
"LccIcp/Type", std::make_pair(
false, Parameters::kRegStrategy())));
398 removedParameters_.insert(std::make_pair(
"LccIcp3/Decimation", std::make_pair(
false,
"")));
399 removedParameters_.insert(std::make_pair(
"LccIcp3/MaxDepth", std::make_pair(
false,
"")));
400 removedParameters_.insert(std::make_pair(
"LccIcp3/VoxelSize", std::make_pair(
false, Parameters::kIcpVoxelSize())));
401 removedParameters_.insert(std::make_pair(
"LccIcp3/Samples", std::make_pair(
false, Parameters::kIcpDownsamplingStep())));
402 removedParameters_.insert(std::make_pair(
"LccIcp3/MaxCorrespondenceDistance", std::make_pair(
false, Parameters::kIcpMaxCorrespondenceDistance())));
403 removedParameters_.insert(std::make_pair(
"LccIcp3/Iterations", std::make_pair(
false, Parameters::kIcpIterations())));
404 removedParameters_.insert(std::make_pair(
"LccIcp3/CorrespondenceRatio", std::make_pair(
false, Parameters::kIcpCorrespondenceRatio())));
405 removedParameters_.insert(std::make_pair(
"LccIcp3/PointToPlane", std::make_pair(
true, Parameters::kIcpPointToPlane())));
406 removedParameters_.insert(std::make_pair(
"LccIcp3/PointToPlaneNormalNeighbors", std::make_pair(
true, Parameters::kIcpPointToPlaneK())));
408 removedParameters_.insert(std::make_pair(
"LccIcp2/MaxCorrespondenceDistance", std::make_pair(
true, Parameters::kIcpMaxCorrespondenceDistance())));
409 removedParameters_.insert(std::make_pair(
"LccIcp2/Iterations", std::make_pair(
true, Parameters::kIcpIterations())));
410 removedParameters_.insert(std::make_pair(
"LccIcp2/CorrespondenceRatio", std::make_pair(
true, Parameters::kIcpCorrespondenceRatio())));
411 removedParameters_.insert(std::make_pair(
"LccIcp2/VoxelSize", std::make_pair(
true, Parameters::kIcpVoxelSize())));
413 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionByTime", std::make_pair(
true, Parameters::kRGBDProximityByTime())));
414 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionBySpace", std::make_pair(
true, Parameters::kRGBDProximityBySpace())));
415 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionTime", std::make_pair(
true, Parameters::kRGBDProximityByTime())));
416 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionSpace", std::make_pair(
true, Parameters::kRGBDProximityBySpace())));
417 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionPathScansMerged", std::make_pair(
false,
"")));
418 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionMaxGraphDepth", std::make_pair(
true, Parameters::kRGBDProximityMaxGraphDepth())));
419 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionPathFilteringRadius", std::make_pair(
true, Parameters::kRGBDProximityPathFilteringRadius())));
420 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionPathRawPosesUsed", std::make_pair(
true, Parameters::kRGBDProximityPathRawPosesUsed())));
422 removedParameters_.insert(std::make_pair(
"RGBD/OptimizeStrategy", std::make_pair(
true, Parameters::kOptimizerStrategy())));
423 removedParameters_.insert(std::make_pair(
"RGBD/OptimizeEpsilon", std::make_pair(
true, Parameters::kOptimizerEpsilon())));
424 removedParameters_.insert(std::make_pair(
"RGBD/OptimizeIterations", std::make_pair(
true, Parameters::kOptimizerIterations())));
425 removedParameters_.insert(std::make_pair(
"RGBD/OptimizeRobust", std::make_pair(
true, Parameters::kOptimizerRobust())));
426 removedParameters_.insert(std::make_pair(
"RGBD/OptimizeSlam2D", std::make_pair(
true, Parameters::kRegForce3DoF())));
427 removedParameters_.insert(std::make_pair(
"RGBD/OptimizeSlam2d", std::make_pair(
true, Parameters::kRegForce3DoF())));
428 removedParameters_.insert(std::make_pair(
"RGBD/OptimizeVarianceIgnored", std::make_pair(
true, Parameters::kOptimizerVarianceIgnored())));
430 removedParameters_.insert(std::make_pair(
"Stereo/WinSize", std::make_pair(
true, Parameters::kStereoWinWidth())));
433 removedParameters_.insert(std::make_pair(
"GFTT/MaxCorners", std::make_pair(
true, Parameters::kVisMaxFeatures())));
434 removedParameters_.insert(std::make_pair(
"LccBow/MaxDepth", std::make_pair(
true, Parameters::kVisMaxDepth())));
435 removedParameters_.insert(std::make_pair(
"LccReextract/LoopClosureFeatures", std::make_pair(
true, Parameters::kRGBDLoopClosureReextractFeatures())));
436 removedParameters_.insert(std::make_pair(
"Rtabmap/DetectorStrategy", std::make_pair(
true, Parameters::kKpDetectorStrategy())));
437 removedParameters_.insert(std::make_pair(
"RGBD/ScanMatchingSize", std::make_pair(
true, Parameters::kRGBDNeighborLinkRefining())));
438 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionRadius", std::make_pair(
true, Parameters::kRGBDLocalRadius())));
439 removedParameters_.insert(std::make_pair(
"RGBD/ToroIterations", std::make_pair(
true, Parameters::kOptimizerIterations())));
440 removedParameters_.insert(std::make_pair(
"Mem/RehearsedNodesKept", std::make_pair(
true, Parameters::kMemNotLinkedNodesKept())));
441 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionMaxDiffID", std::make_pair(
true, Parameters::kRGBDProximityMaxGraphDepth())));
442 removedParameters_.insert(std::make_pair(
"RGBD/PlanVirtualLinksMaxDiffID", std::make_pair(
false,
"")));
443 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionMaxDiffID", std::make_pair(
false,
"")));
444 removedParameters_.insert(std::make_pair(
"Odom/Type", std::make_pair(
true, Parameters::kVisFeatureType())));
445 removedParameters_.insert(std::make_pair(
"Odom/MaxWords", std::make_pair(
true, Parameters::kVisMaxFeatures())));
446 removedParameters_.insert(std::make_pair(
"Odom/LocalHistory", std::make_pair(
true, Parameters::kOdomF2MMaxSize())));
447 removedParameters_.insert(std::make_pair(
"Odom/NearestNeighbor", std::make_pair(
true, Parameters::kVisCorNNType())));
448 removedParameters_.insert(std::make_pair(
"Odom/NNDR", std::make_pair(
true, Parameters::kVisCorNNDR())));
464 if(
iter->second.first)
483 UERROR(
"Parameters \"%s\" doesn't exist!", paramKey.c_str());
498 UERROR(
"Parameters \"%s\" doesn't exist!", paramKey.c_str());
505 ParametersMap::const_iterator
iter = parameters.find(
key);
506 if(
iter != parameters.end())
515 ParametersMap::const_iterator
iter = parameters.find(
key);
516 if(
iter != parameters.end())
525 ParametersMap::const_iterator
iter = parameters.find(
key);
526 if(
iter != parameters.end())
535 ParametersMap::const_iterator
iter = parameters.find(
key);
536 if(
iter != parameters.end())
545 ParametersMap::const_iterator
iter = parameters.find(
key);
546 if(
iter != parameters.end())
555 ParametersMap::const_iterator
iter = parameters.find(
key);
556 if(
iter != parameters.end())
565 for(ParametersMap::iterator
iter=parametersOut.begin();
iter!=parametersOut.end(); ++
iter)
567 ParametersMap::const_iterator jter = parameters.find(
iter->first);
568 if(jter != parameters.end())
570 iter->second = jter->second;
577 return "RTAB-Map options:\n"
578 " --help Show usage.\n"
579 " --version Show version of rtabmap and its dependencies.\n"
580 " --params Show all parameters with their default value and description. \n"
581 " If a database path is set as last argument, the parameters in the \n"
582 " database will be shown in INI format.\n"
583 " --\"parameter name\" \"value\" Overwrite a specific RTAB-Map's parameter :\n"
584 " --SURF/HessianThreshold 150\n"
585 " For parameters in table format, add ',' between values :\n"
586 " --Kp/RoiRatios 0,0,0.1,0\n"
588 " --nolog Disable logger\n"
589 " --logconsole Set logger console type\n"
590 " --logfile \"path\" Set logger file type\n"
591 " --logfilea \"path\" Set logger file type with appending mode if the file already exists\n"
592 " --udebug Set logger level to debug\n"
593 " --uinfo Set logger level to info\n"
594 " --uwarn Set logger level to warn\n"
595 " --uerror Set logger level to error\n"
596 " --logtime \"bool\" Print time when logging\n"
597 " --logwhere \"bool\" Print where when logging\n"
598 " --logthread \"bool\" Print thread id when logging\n"
606 const std::map<std::string, std::pair<bool, std::string> > & removedParams =
getRemovedParameters();
607 for(
int i=0;
i<argc;++
i)
609 bool checkParameters = onlyParameters;
612 if(strcmp(
argv[
i],
"--help") == 0)
617 else if(strcmp(
argv[
i],
"--version") == 0)
619 std::string
str =
"RTAB-Map:";
622 std::cout <<
str << std::setw(spacing -
str.size()) << RTABMAP_VERSION << std::endl;
624 std::cout <<
str << std::setw(spacing -
str.size()) << PCL_VERSION_PRETTY << std::endl;
627 std::cout <<
str << std::setw(spacing -
str.size()) << vtkVersion::GetVTKVersion() << std::endl;
629 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
632 std::cout <<
str << std::setw(spacing -
str.size()) << CV_VERSION << std::endl;
633 #if CV_MAJOR_VERSION >= 3
634 str =
"With OpenCV xfeatures2d:";
635 #ifdef HAVE_OPENCV_XFEATURES2D
636 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
638 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
641 str =
"With OpenCV nonfree:";
642 #ifdef RTABMAP_NONFREE
643 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
645 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
647 str =
"With ORB OcTree:";
648 #ifdef RTABMAP_ORB_OCTREE
649 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
651 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
653 str =
"With SuperPoint Torch:";
655 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
657 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
659 str =
"With Python3:";
660 #ifdef RTABMAP_PYTHON
661 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
663 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
665 str =
"With FastCV:";
666 #ifdef RTABMAP_FASTCV
667 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
669 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
671 str =
"With OpenGV:";
672 #ifdef RTABMAP_OPENGV
673 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
675 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
677 str =
"With Madgwick:";
678 #ifdef RTABMAP_MADGWICK
679 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
681 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
685 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
687 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
689 str =
"With libLAS:";
690 #ifdef RTABMAP_LIBLAS
691 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
693 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
695 str =
"With CudaSift:";
696 #ifdef RTABMAP_CUDASIFT
697 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
699 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
703 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
705 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
709 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
711 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
715 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
717 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
719 str =
"With Vertigo:";
720 #ifdef RTABMAP_VERTIGO
721 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
723 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
727 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
729 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
733 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
735 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
737 str =
"With OpenNI:";
738 #ifdef RTABMAP_OPENNI
739 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
741 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
743 str =
"With OpenNI2:";
744 #ifdef RTABMAP_OPENNI2
745 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
747 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
749 str =
"With Freenect:";
750 #ifdef RTABMAP_FREENECT
751 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
753 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
755 str =
"With Freenect2:";
756 #ifdef RTABMAP_FREENECT2
757 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
759 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
763 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
765 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
769 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
771 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
773 str =
"With DC1394:";
774 #ifdef RTABMAP_DC1394
775 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
777 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
779 str =
"With FlyCapture2:";
780 #ifdef RTABMAP_FLYCAPTURE2
781 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
783 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
787 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
789 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
791 str =
"With ZED Open Capture:";
793 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
795 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
797 str =
"With RealSense:";
798 #ifdef RTABMAP_REALSENSE
799 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
801 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
803 str =
"With RealSense SLAM:";
804 #ifdef RTABMAP_REALSENSE_SLAM
805 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
807 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
809 str =
"With RealSense2:";
810 #ifdef RTABMAP_REALSENSE2
811 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
813 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
815 str =
"With MYNT EYE S:";
816 #ifdef RTABMAP_MYNTEYE
817 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
819 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
821 str =
"With DepthAI:";
822 #ifdef RTABMAP_DEPTHAI
823 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
825 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
827 str =
"With XVisio SDK:";
829 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
831 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
833 str =
"With libpointmatcher:";
834 #ifdef RTABMAP_POINTMATCHER
835 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
837 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
839 str =
"With CCCoreLib:";
840 #ifdef RTABMAP_CCCORELIB
841 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
843 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
845 str =
"With Open3D:";
846 #ifdef RTABMAP_OPEN3D
847 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
849 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
851 str =
"With OctoMap:";
852 #ifdef RTABMAP_OCTOMAP
853 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
855 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
857 str =
"With GridMap:";
858 #ifdef RTABMAP_GRIDMAP
859 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
861 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
863 str =
"With cpu-tsdf:";
864 #ifdef RTABMAP_CPUTSDF
865 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
867 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
869 str =
"With open chisel:";
870 #ifdef RTABMAP_OPENCHISEL
871 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
873 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
875 str =
"With Alice Vision:";
876 #ifdef RTABMAP_ALICE_VISION
877 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
879 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
883 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
885 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
889 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
891 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
895 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
897 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
901 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
903 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
907 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
909 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
911 #if RTABMAP_ORB_SLAM == 3
912 str =
"With ORB_SLAM3:";
913 #elif RTABMAP_ORB_SLAM == 2
914 str =
"With ORB_SLAM2:";
916 str =
"With ORB_SLAM:";
918 #ifdef RTABMAP_ORB_SLAM
919 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
921 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
925 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
927 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
929 str =
"With MSCKF_VIO:";
930 #ifdef RTABMAP_MSCKF_VIO
931 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
933 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
935 str =
"With VINS-Fusion:";
937 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
939 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
941 str =
"With OpenVINS:";
942 #ifdef RTABMAP_OPENVINS
943 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
945 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
949 else if(strcmp(
argv[
i],
"--nolog") == 0)
953 else if(strcmp(
argv[
i],
"--logconsole") == 0)
957 else if(strcmp(
argv[
i],
"--logfile") == 0)
966 UERROR(
"\"--logfile\" argument requires following file path");
969 else if(strcmp(
argv[
i],
"--logfilea") == 0)
978 UERROR(
"\"--logfilea\" argument requires following file path");
981 else if(strcmp(
argv[
i],
"--udebug") == 0)
985 else if(strcmp(
argv[
i],
"--uinfo") == 0)
989 else if(strcmp(
argv[
i],
"--uwarn") == 0)
993 else if(strcmp(
argv[
i],
"--uerror") == 0)
997 else if(strcmp(
argv[
i],
"--ulogtime") == 0)
1006 UERROR(
"\"--ulogtime\" argument requires a following boolean value");
1009 else if(strcmp(
argv[
i],
"--ulogwhere") == 0)
1018 UERROR(
"\"--ulogwhere\" argument requires a following boolean value");
1021 else if(strcmp(
argv[
i],
"--ulogthread") == 0)
1030 UERROR(
"\"--ulogthread\" argument requires a following boolean value");
1035 checkParameters =
true;
1041 if(strcmp(
argv[
i],
"--params") == 0)
1046 std::string dbName =
argv[argc - 1];
1054 if (!dbParameters.empty())
1056 std::cout <<
"[Core]" << std::endl;
1057 std::cout <<
"Version = " << RTABMAP_VERSION << std::endl;
1058 for (ParametersMap::const_iterator
iter = dbParameters.begin();
iter != dbParameters.end(); ++
iter)
1060 std::string
key =
iter->first;
1063 std::string
value =
iter->second.c_str();
1066 std::cout <<
key <<
" = " <<
value << std::endl;
1080 for(rtabmap::ParametersMap::const_iterator
iter=parameters.begin();
iter!=parameters.end(); ++
iter)
1082 bool ignore =
false;
1084 std::string group =
uSplit(
iter->first,
'/').front();
1085 #ifndef RTABMAP_GTSAM
1086 if(group.compare(
"GTSAM") == 0)
1092 if(group.compare(
"g2o") == 0)
1097 #ifndef RTABMAP_FOVIS
1098 if(group.compare(
"OdomFovis") == 0)
1103 #ifndef RTABMAP_VISO2
1104 if(group.compare(
"OdomViso2") == 0)
1109 #ifndef RTABMAP_ORBSLAM2
1110 if(group.compare(
"OdomORBSLAM2") == 0)
1115 #ifndef RTABMAP_OKVIS
1116 if(group.compare(
"OdomOKVIS") == 0)
1121 #if not defined(RTABMAP_LOAM) and not defined(RTABMAP_FLOAM)
1122 if(group.compare(
"OdomLOAM") == 0)
1127 #ifndef RTABMAP_MSCKF_VIO
1128 if(group.compare(
"OdomMSCKF") == 0)
1135 std::string
str =
"Param: " +
iter->first +
" = \"" +
iter->second +
"\"";
1138 std::setw(60 -
str.size()) <<
1150 ParametersMap::const_iterator
iter = parameters.find(
key);
1151 if(
iter != parameters.end())
1157 UINFO(
"Parsed parameter \"%s\"=\"%s\"",
iter->first.c_str(),
argv[
i]);
1163 std::map<std::string, std::pair<bool, std::string> >::const_iterator jter = removedParams.find(
key);
1164 if(jter!=removedParams.end())
1166 if(jter->second.first)
1175 key = jter->second.second;
1176 UWARN(
"Parameter migration from \"%s\" to \"%s\" (value=%s).",
1177 jter->first.c_str(), jter->second.second.c_str(),
value.c_str());
1183 UERROR(
"Value missing for argument \"%s\"",
argv[
i-1]);
1186 else if(jter->second.second.empty())
1188 UERROR(
"Parameter \"%s\" doesn't exist anymore.", jter->first.c_str());
1192 UERROR(
"Parameter \"%s\" doesn't exist anymore, check this similar parameter \"%s\".", jter->first.c_str(), jter->second.second.c_str());
1207 for(CSimpleIniA::TKeyVal::const_iterator
iter=keyValMap->begin();
iter!=keyValMap->end(); ++
iter)
1209 std::string
key = (*iter).first.pItem;
1210 if(
key.compare(
"Version") == 0)
1218 if(configFilePath.find(
".rtabmap") != std::string::npos)
1220 UWARN(
"Version in the config file \"%s\" is more recent (\"%s\") than "
1221 "current RTAB-Map version used (\"%s\"). The config file will be upgraded "
1223 configFilePath.c_str(),
1229 UERROR(
"Version in the config file \"%s\" is more recent (\"%s\") than "
1230 "current RTAB-Map version used (\"%s\"). New parameters (if there are some) will "
1232 configFilePath.c_str(),
1247 if(oldIter->second.first)
1249 if(parameters.find(oldIter->second.second) == parameters.end())
1251 key = oldIter->second.second;
1252 UINFO(
"Parameter migration from \"%s\" to \"%s\" (value=%s, default=%s).",
1256 else if(oldIter->second.second.empty())
1258 UWARN(
"Parameter \"%s\" doesn't exist anymore.",
1259 oldIter->first.c_str());
1261 else if(parameters.find(oldIter->second.second) == parameters.end())
1263 UWARN(
"Parameter \"%s\" (value=%s) doesn't exist anymore, you may want to use this similar parameter \"%s (default=%s): %s\".",
1281 ULOGGER_WARN(
"Section \"Core\" in %s doesn't exist... "
1282 "Ignore this warning if the ini file does not exist yet. "
1283 "The ini file will be automatically created when rtabmap will close.", configFilePath.c_str());
1292 readINIImpl(ini, configFile, parameters, modifiedOnly);
1308 ini.
SetValue(
"Core",
"Version", RTABMAP_VERSION,
NULL,
true);
1310 for(ParametersMap::const_iterator
iter=parameters.begin();
iter!=parameters.end(); ++
iter)
1312 std::string
key =
iter->first;
1315 std::string
value =
iter->second.c_str();
1324 for(std::map<std::string, std::pair<bool, std::string> >::const_iterator
iter =
removedParameters_.begin();
1328 std::string
key =
iter->first;
1333 if(ini.
Delete(
"Core",
key.c_str(),
true))
1335 if(
iter->second.first && parameters.find(
iter->second.second) != parameters.end())
1337 UWARN(
"Removed deprecated parameter %s=%s (replaced by %s=%s) from \"%s\".",
iter->first.c_str(),
value.c_str(),
iter->second.second.c_str(), parameters.at(
iter->second.second).c_str(), configFile.c_str());
1341 UWARN(
"Removed deprecated parameter %s=%s from \"%s\".",
iter->first.c_str(),
value.c_str(), configFile.c_str());