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(
"GridGlobal/FullUpdate", std::make_pair(
false,
"")));
243 removedParameters_.insert(std::make_pair(
"Grid/FromDepth", std::make_pair(
true, Parameters::kGridSensor())));
246 removedParameters_.insert(std::make_pair(
"OdomORBSLAM2/VocPath", std::make_pair(
true, Parameters::kOdomORBSLAMVocPath())));
247 removedParameters_.insert(std::make_pair(
"OdomORBSLAM2/Bf", std::make_pair(
true, Parameters::kOdomORBSLAMBf())));
248 removedParameters_.insert(std::make_pair(
"OdomORBSLAM2/ThDepth", std::make_pair(
true, Parameters::kOdomORBSLAMThDepth())));
249 removedParameters_.insert(std::make_pair(
"OdomORBSLAM2/Fps", std::make_pair(
true, Parameters::kOdomORBSLAMFps())));
250 removedParameters_.insert(std::make_pair(
"OdomORBSLAM2/MaxFeatures", std::make_pair(
true, Parameters::kOdomORBSLAMMaxFeatures())));
251 removedParameters_.insert(std::make_pair(
"OdomORBSLAM2/MapSize", std::make_pair(
true, Parameters::kOdomORBSLAMMapSize())));
253 removedParameters_.insert(std::make_pair(
"RGBD/SavedLocalizationIgnored", std::make_pair(
true, Parameters::kRGBDStartAtOrigin())));
255 removedParameters_.insert(std::make_pair(
"Icp/PMForce4DoF", std::make_pair(
true, Parameters::kIcpForce4DoF())));
256 removedParameters_.insert(std::make_pair(
"Icp/PM", std::make_pair(
true, Parameters::kIcpStrategy())));
257 removedParameters_.insert(std::make_pair(
"Icp/PMOutlierRatio", std::make_pair(
true, Parameters::kIcpOutlierRatio())));
260 removedParameters_.insert(std::make_pair(
"SuperGlue/Path", std::make_pair(
true, Parameters::kPyMatcherPath())));
261 removedParameters_.insert(std::make_pair(
"SuperGlue/Iterations", std::make_pair(
true, Parameters::kPyMatcherIterations())));
262 removedParameters_.insert(std::make_pair(
"SuperGlue/MatchThreshold", std::make_pair(
true, Parameters::kPyMatcherThreshold())));
263 removedParameters_.insert(std::make_pair(
"SuperGlue/Cuda", std::make_pair(
true, Parameters::kPyMatcherCuda())));
264 removedParameters_.insert(std::make_pair(
"SuperGlue/Indoor", std::make_pair(
false, Parameters::kPyMatcherModel())));
266 removedParameters_.insert(std::make_pair(
"Vis/CorCrossCheck", std::make_pair(
false, Parameters::kVisCorNNType())));
267 removedParameters_.insert(std::make_pair(
"SPTorch/ModelPath", std::make_pair(
true, Parameters::kSuperPointModelPath())));
268 removedParameters_.insert(std::make_pair(
"SPTorch/Threshold", std::make_pair(
true, Parameters::kSuperPointThreshold())));
269 removedParameters_.insert(std::make_pair(
"SPTorch/NMS", std::make_pair(
true, Parameters::kSuperPointNMS())));
270 removedParameters_.insert(std::make_pair(
"SPTorch/MinDistance", std::make_pair(
true, Parameters::kSuperPointNMSRadius())));
271 removedParameters_.insert(std::make_pair(
"SPTorch/Cuda", std::make_pair(
true, Parameters::kSuperPointCuda())));
274 removedParameters_.insert(std::make_pair(
"RGBD/MaxLocalizationDistance", std::make_pair(
true, Parameters::kRGBDMaxLoopClosureDistance())));
277 removedParameters_.insert(std::make_pair(
"Aruco/Dictionary", std::make_pair(
true, Parameters::kMarkerDictionary())));
278 removedParameters_.insert(std::make_pair(
"Aruco/MarkerLength", std::make_pair(
true, Parameters::kMarkerLength())));
279 removedParameters_.insert(std::make_pair(
"Aruco/MaxDepthError", std::make_pair(
true, Parameters::kMarkerMaxDepthError())));
280 removedParameters_.insert(std::make_pair(
"Aruco/VarianceLinear", std::make_pair(
true, Parameters::kMarkerVarianceLinear())));
281 removedParameters_.insert(std::make_pair(
"Aruco/VarianceAngular", std::make_pair(
true, Parameters::kMarkerVarianceAngular())));
282 removedParameters_.insert(std::make_pair(
"Aruco/CornerRefinementMethod", std::make_pair(
true, Parameters::kMarkerCornerRefinementMethod())));
285 removedParameters_.insert(std::make_pair(
"Grid/OctoMapOccupancyThr", std::make_pair(
true, Parameters::kGridGlobalOccupancyThr())));
288 removedParameters_.insert(std::make_pair(
"Grid/Scan2dMaxFilledRange", std::make_pair(
false, Parameters::kGridRangeMax())));
291 removedParameters_.insert(std::make_pair(
"Grid/ProjRayTracing", std::make_pair(
true, Parameters::kGridRayTracing())));
292 removedParameters_.insert(std::make_pair(
"Grid/DepthMin", std::make_pair(
true, Parameters::kGridRangeMin())));
293 removedParameters_.insert(std::make_pair(
"Grid/DepthMax", std::make_pair(
true, Parameters::kGridRangeMax())));
296 removedParameters_.insert(std::make_pair(
"Reg/VarianceFromInliersCount", std::make_pair(
false,
"")));
297 removedParameters_.insert(std::make_pair(
"Reg/VarianceNormalized", std::make_pair(
false,
"")));
300 removedParameters_.insert(std::make_pair(
"Icp/PointToPlaneNormalNeighbors", std::make_pair(
true, Parameters::kIcpPointToPlaneK())));
304 removedParameters_.insert(std::make_pair(
"Rtabmap/VhStrategy", std::make_pair(
true, Parameters::kVhEpEnabled())));
307 removedParameters_.insert(std::make_pair(
"Grid/FullUpdate", std::make_pair(
false,
"")));
310 removedParameters_.insert(std::make_pair(
"Grid/3DGroundIsObstacle", std::make_pair(
true, Parameters::kGridGroundIsObstacle())));
313 removedParameters_.insert(std::make_pair(
"Optimizer/Slam2D", std::make_pair(
true, Parameters::kRegForce3DoF())));
314 removedParameters_.insert(std::make_pair(
"OdomF2M/FixedMapPath", std::make_pair(
false,
"")));
317 removedParameters_.insert(std::make_pair(
"Grid/FlatObstaclesDetected", std::make_pair(
true, Parameters::kGridFlatObstacleDetected())));
320 removedParameters_.insert(std::make_pair(
"Reg/Force2D", std::make_pair(
true, Parameters::kRegForce3DoF())));
321 removedParameters_.insert(std::make_pair(
"OdomF2M/ScanSubstractRadius", std::make_pair(
true, Parameters::kOdomF2MScanSubtractRadius())));
324 removedParameters_.insert(std::make_pair(
"RGBD/ProximityPathScansMerged", std::make_pair(
false,
"")));
327 removedParameters_.insert(std::make_pair(
"Mem/ImageDecimation", std::make_pair(
true, Parameters::kMemImagePostDecimation())));
330 removedParameters_.insert(std::make_pair(
"OdomLocalMap/HistorySize", std::make_pair(
true, Parameters::kOdomF2MMaxSize())));
331 removedParameters_.insert(std::make_pair(
"OdomLocalMap/FixedMapPath", std::make_pair(
false,
"")));
332 removedParameters_.insert(std::make_pair(
"OdomF2F/GuessMotion", std::make_pair(
true, Parameters::kOdomGuessMotion())));
333 removedParameters_.insert(std::make_pair(
"OdomF2F/KeyFrameThr", std::make_pair(
false, Parameters::kOdomKeyFrameThr())));
336 removedParameters_.insert(std::make_pair(
"OdomBow/LocalHistorySize", std::make_pair(
true, Parameters::kOdomF2MMaxSize())));
337 removedParameters_.insert(std::make_pair(
"OdomBow/FixedLocalMapPath", std::make_pair(
false,
"")));
338 removedParameters_.insert(std::make_pair(
"OdomFlow/KeyFrameThr", std::make_pair(
false, Parameters::kOdomKeyFrameThr())));
339 removedParameters_.insert(std::make_pair(
"OdomFlow/GuessMotion", std::make_pair(
true, Parameters::kOdomGuessMotion())));
341 removedParameters_.insert(std::make_pair(
"Kp/WordsPerImage", std::make_pair(
true, Parameters::kKpMaxFeatures())));
343 removedParameters_.insert(std::make_pair(
"Mem/LocalSpaceLinksKeptInWM", std::make_pair(
false,
"")));
345 removedParameters_.insert(std::make_pair(
"RGBD/PoseScanMatching", std::make_pair(
true, Parameters::kRGBDNeighborLinkRefining())));
347 removedParameters_.insert(std::make_pair(
"Odom/ParticleFiltering", std::make_pair(
false, Parameters::kOdomFilteringStrategy())));
348 removedParameters_.insert(std::make_pair(
"Odom/FeatureType", std::make_pair(
true, Parameters::kVisFeatureType())));
349 removedParameters_.insert(std::make_pair(
"Odom/EstimationType", std::make_pair(
true, Parameters::kVisEstimationType())));
350 removedParameters_.insert(std::make_pair(
"Odom/MaxFeatures", std::make_pair(
true, Parameters::kVisMaxFeatures())));
351 removedParameters_.insert(std::make_pair(
"Odom/InlierDistance", std::make_pair(
true, Parameters::kVisInlierDistance())));
352 removedParameters_.insert(std::make_pair(
"Odom/MinInliers", std::make_pair(
true, Parameters::kVisMinInliers())));
353 removedParameters_.insert(std::make_pair(
"Odom/Iterations", std::make_pair(
true, Parameters::kVisIterations())));
354 removedParameters_.insert(std::make_pair(
"Odom/RefineIterations", std::make_pair(
true, Parameters::kVisRefineIterations())));
355 removedParameters_.insert(std::make_pair(
"Odom/MaxDepth", std::make_pair(
true, Parameters::kVisMaxDepth())));
356 removedParameters_.insert(std::make_pair(
"Odom/RoiRatios", std::make_pair(
true, Parameters::kVisRoiRatios())));
357 removedParameters_.insert(std::make_pair(
"Odom/Force2D", std::make_pair(
true, Parameters::kRegForce3DoF())));
358 removedParameters_.insert(std::make_pair(
"Odom/VarianceFromInliersCount", std::make_pair(
false,
"")));
359 removedParameters_.insert(std::make_pair(
"Odom/PnPReprojError", std::make_pair(
true, Parameters::kVisPnPReprojError())));
360 removedParameters_.insert(std::make_pair(
"Odom/PnPFlags", std::make_pair(
true, Parameters::kVisPnPFlags())));
362 removedParameters_.insert(std::make_pair(
"OdomBow/NNType", std::make_pair(
true, Parameters::kVisCorNNType())));
363 removedParameters_.insert(std::make_pair(
"OdomBow/NNDR", std::make_pair(
true, Parameters::kVisCorNNDR())));
365 removedParameters_.insert(std::make_pair(
"OdomFlow/WinSize", std::make_pair(
true, Parameters::kVisCorFlowWinSize())));
366 removedParameters_.insert(std::make_pair(
"OdomFlow/Iterations", std::make_pair(
true, Parameters::kVisCorFlowIterations())));
367 removedParameters_.insert(std::make_pair(
"OdomFlow/Eps", std::make_pair(
true, Parameters::kVisCorFlowEps())));
368 removedParameters_.insert(std::make_pair(
"OdomFlow/MaxLevel", std::make_pair(
true, Parameters::kVisCorFlowMaxLevel())));
370 removedParameters_.insert(std::make_pair(
"OdomSubPix/WinSize", std::make_pair(
true, Parameters::kVisSubPixWinSize())));
371 removedParameters_.insert(std::make_pair(
"OdomSubPix/Iterations", std::make_pair(
true, Parameters::kVisSubPixIterations())));
372 removedParameters_.insert(std::make_pair(
"OdomSubPix/Eps", std::make_pair(
true, Parameters::kVisSubPixEps())));
374 removedParameters_.insert(std::make_pair(
"LccReextract/Activated", std::make_pair(
true, Parameters::kRGBDLoopClosureReextractFeatures())));
375 removedParameters_.insert(std::make_pair(
"LccReextract/FeatureType", std::make_pair(
false, Parameters::kVisFeatureType())));
376 removedParameters_.insert(std::make_pair(
"LccReextract/MaxWords", std::make_pair(
false, Parameters::kVisMaxFeatures())));
377 removedParameters_.insert(std::make_pair(
"LccReextract/MaxDepth", std::make_pair(
false, Parameters::kVisMaxDepth())));
378 removedParameters_.insert(std::make_pair(
"LccReextract/RoiRatios", std::make_pair(
false, Parameters::kVisRoiRatios())));
379 removedParameters_.insert(std::make_pair(
"LccReextract/NNType", std::make_pair(
false, Parameters::kVisCorNNType())));
380 removedParameters_.insert(std::make_pair(
"LccReextract/NNDR", std::make_pair(
false, Parameters::kVisCorNNDR())));
382 removedParameters_.insert(std::make_pair(
"LccBow/EstimationType", std::make_pair(
false, Parameters::kVisEstimationType())));
383 removedParameters_.insert(std::make_pair(
"LccBow/InlierDistance", std::make_pair(
false, Parameters::kVisInlierDistance())));
384 removedParameters_.insert(std::make_pair(
"LccBow/MinInliers", std::make_pair(
false, Parameters::kVisMinInliers())));
385 removedParameters_.insert(std::make_pair(
"LccBow/Iterations", std::make_pair(
false, Parameters::kVisIterations())));
386 removedParameters_.insert(std::make_pair(
"LccBow/RefineIterations", std::make_pair(
false, Parameters::kVisRefineIterations())));
387 removedParameters_.insert(std::make_pair(
"LccBow/Force2D", std::make_pair(
false, Parameters::kRegForce3DoF())));
388 removedParameters_.insert(std::make_pair(
"LccBow/VarianceFromInliersCount", std::make_pair(
false,
"")));
389 removedParameters_.insert(std::make_pair(
"LccBow/PnPReprojError", std::make_pair(
false, Parameters::kVisPnPReprojError())));
390 removedParameters_.insert(std::make_pair(
"LccBow/PnPFlags", std::make_pair(
false, Parameters::kVisPnPFlags())));
391 removedParameters_.insert(std::make_pair(
"LccBow/EpipolarGeometryVar", std::make_pair(
true, Parameters::kVisEpipolarGeometryVar())));
393 removedParameters_.insert(std::make_pair(
"LccIcp/Type", std::make_pair(
false, Parameters::kRegStrategy())));
395 removedParameters_.insert(std::make_pair(
"LccIcp3/Decimation", std::make_pair(
false,
"")));
396 removedParameters_.insert(std::make_pair(
"LccIcp3/MaxDepth", std::make_pair(
false,
"")));
397 removedParameters_.insert(std::make_pair(
"LccIcp3/VoxelSize", std::make_pair(
false, Parameters::kIcpVoxelSize())));
398 removedParameters_.insert(std::make_pair(
"LccIcp3/Samples", std::make_pair(
false, Parameters::kIcpDownsamplingStep())));
399 removedParameters_.insert(std::make_pair(
"LccIcp3/MaxCorrespondenceDistance", std::make_pair(
false, Parameters::kIcpMaxCorrespondenceDistance())));
400 removedParameters_.insert(std::make_pair(
"LccIcp3/Iterations", std::make_pair(
false, Parameters::kIcpIterations())));
401 removedParameters_.insert(std::make_pair(
"LccIcp3/CorrespondenceRatio", std::make_pair(
false, Parameters::kIcpCorrespondenceRatio())));
402 removedParameters_.insert(std::make_pair(
"LccIcp3/PointToPlane", std::make_pair(
true, Parameters::kIcpPointToPlane())));
403 removedParameters_.insert(std::make_pair(
"LccIcp3/PointToPlaneNormalNeighbors", std::make_pair(
true, Parameters::kIcpPointToPlaneK())));
405 removedParameters_.insert(std::make_pair(
"LccIcp2/MaxCorrespondenceDistance", std::make_pair(
true, Parameters::kIcpMaxCorrespondenceDistance())));
406 removedParameters_.insert(std::make_pair(
"LccIcp2/Iterations", std::make_pair(
true, Parameters::kIcpIterations())));
407 removedParameters_.insert(std::make_pair(
"LccIcp2/CorrespondenceRatio", std::make_pair(
true, Parameters::kIcpCorrespondenceRatio())));
408 removedParameters_.insert(std::make_pair(
"LccIcp2/VoxelSize", std::make_pair(
true, Parameters::kIcpVoxelSize())));
410 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionByTime", std::make_pair(
true, Parameters::kRGBDProximityByTime())));
411 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionBySpace", std::make_pair(
true, Parameters::kRGBDProximityBySpace())));
412 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionTime", std::make_pair(
true, Parameters::kRGBDProximityByTime())));
413 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionSpace", std::make_pair(
true, Parameters::kRGBDProximityBySpace())));
414 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionPathScansMerged", std::make_pair(
false,
"")));
415 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionMaxGraphDepth", std::make_pair(
true, Parameters::kRGBDProximityMaxGraphDepth())));
416 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionPathFilteringRadius", std::make_pair(
true, Parameters::kRGBDProximityPathFilteringRadius())));
417 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionPathRawPosesUsed", std::make_pair(
true, Parameters::kRGBDProximityPathRawPosesUsed())));
419 removedParameters_.insert(std::make_pair(
"RGBD/OptimizeStrategy", std::make_pair(
true, Parameters::kOptimizerStrategy())));
420 removedParameters_.insert(std::make_pair(
"RGBD/OptimizeEpsilon", std::make_pair(
true, Parameters::kOptimizerEpsilon())));
421 removedParameters_.insert(std::make_pair(
"RGBD/OptimizeIterations", std::make_pair(
true, Parameters::kOptimizerIterations())));
422 removedParameters_.insert(std::make_pair(
"RGBD/OptimizeRobust", std::make_pair(
true, Parameters::kOptimizerRobust())));
423 removedParameters_.insert(std::make_pair(
"RGBD/OptimizeSlam2D", std::make_pair(
true, Parameters::kRegForce3DoF())));
424 removedParameters_.insert(std::make_pair(
"RGBD/OptimizeSlam2d", std::make_pair(
true, Parameters::kRegForce3DoF())));
425 removedParameters_.insert(std::make_pair(
"RGBD/OptimizeVarianceIgnored", std::make_pair(
true, Parameters::kOptimizerVarianceIgnored())));
427 removedParameters_.insert(std::make_pair(
"Stereo/WinSize", std::make_pair(
true, Parameters::kStereoWinWidth())));
430 removedParameters_.insert(std::make_pair(
"GFTT/MaxCorners", std::make_pair(
true, Parameters::kVisMaxFeatures())));
431 removedParameters_.insert(std::make_pair(
"LccBow/MaxDepth", std::make_pair(
true, Parameters::kVisMaxDepth())));
432 removedParameters_.insert(std::make_pair(
"LccReextract/LoopClosureFeatures", std::make_pair(
true, Parameters::kRGBDLoopClosureReextractFeatures())));
433 removedParameters_.insert(std::make_pair(
"Rtabmap/DetectorStrategy", std::make_pair(
true, Parameters::kKpDetectorStrategy())));
434 removedParameters_.insert(std::make_pair(
"RGBD/ScanMatchingSize", std::make_pair(
true, Parameters::kRGBDNeighborLinkRefining())));
435 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionRadius", std::make_pair(
true, Parameters::kRGBDLocalRadius())));
436 removedParameters_.insert(std::make_pair(
"RGBD/ToroIterations", std::make_pair(
true, Parameters::kOptimizerIterations())));
437 removedParameters_.insert(std::make_pair(
"Mem/RehearsedNodesKept", std::make_pair(
true, Parameters::kMemNotLinkedNodesKept())));
438 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionMaxDiffID", std::make_pair(
true, Parameters::kRGBDProximityMaxGraphDepth())));
439 removedParameters_.insert(std::make_pair(
"RGBD/PlanVirtualLinksMaxDiffID", std::make_pair(
false,
"")));
440 removedParameters_.insert(std::make_pair(
"RGBD/LocalLoopDetectionMaxDiffID", std::make_pair(
false,
"")));
441 removedParameters_.insert(std::make_pair(
"Odom/Type", std::make_pair(
true, Parameters::kVisFeatureType())));
442 removedParameters_.insert(std::make_pair(
"Odom/MaxWords", std::make_pair(
true, Parameters::kVisMaxFeatures())));
443 removedParameters_.insert(std::make_pair(
"Odom/LocalHistory", std::make_pair(
true, Parameters::kOdomF2MMaxSize())));
444 removedParameters_.insert(std::make_pair(
"Odom/NearestNeighbor", std::make_pair(
true, Parameters::kVisCorNNType())));
445 removedParameters_.insert(std::make_pair(
"Odom/NNDR", std::make_pair(
true, Parameters::kVisCorNNDR())));
461 if(
iter->second.first)
480 UERROR(
"Parameters \"%s\" doesn't exist!", paramKey.c_str());
495 UERROR(
"Parameters \"%s\" doesn't exist!", paramKey.c_str());
502 ParametersMap::const_iterator
iter = parameters.find(
key);
503 if(
iter != parameters.end())
512 ParametersMap::const_iterator
iter = parameters.find(
key);
513 if(
iter != parameters.end())
522 ParametersMap::const_iterator
iter = parameters.find(
key);
523 if(
iter != parameters.end())
532 ParametersMap::const_iterator
iter = parameters.find(
key);
533 if(
iter != parameters.end())
542 ParametersMap::const_iterator
iter = parameters.find(
key);
543 if(
iter != parameters.end())
552 ParametersMap::const_iterator
iter = parameters.find(
key);
553 if(
iter != parameters.end())
562 for(ParametersMap::iterator
iter=parametersOut.begin();
iter!=parametersOut.end(); ++
iter)
564 ParametersMap::const_iterator jter = parameters.find(
iter->first);
565 if(jter != parameters.end())
567 iter->second = jter->second;
574 return "RTAB-Map options:\n"
575 " --help Show usage.\n"
576 " --version Show version of rtabmap and its dependencies.\n"
577 " --params Show all parameters with their default value and description. \n"
578 " If a database path is set as last argument, the parameters in the \n"
579 " database will be shown in INI format.\n"
580 " --\"parameter name\" \"value\" Overwrite a specific RTAB-Map's parameter :\n"
581 " --SURF/HessianThreshold 150\n"
582 " For parameters in table format, add ',' between values :\n"
583 " --Kp/RoiRatios 0,0,0.1,0\n"
585 " --nolog Disable logger\n"
586 " --logconsole Set logger console type\n"
587 " --logfile \"path\" Set logger file type\n"
588 " --logfilea \"path\" Set logger file type with appending mode if the file already exists\n"
589 " --udebug Set logger level to debug\n"
590 " --uinfo Set logger level to info\n"
591 " --uwarn Set logger level to warn\n"
592 " --uerror Set logger level to error\n"
593 " --logtime \"bool\" Print time when logging\n"
594 " --logwhere \"bool\" Print where when logging\n"
595 " --logthread \"bool\" Print thread id when logging\n"
603 const std::map<std::string, std::pair<bool, std::string> > & removedParams =
getRemovedParameters();
604 for(
int i=0;
i<argc;++
i)
606 bool checkParameters = onlyParameters;
609 if(strcmp(
argv[
i],
"--help") == 0)
614 else if(strcmp(
argv[
i],
"--version") == 0)
616 std::string
str =
"RTAB-Map:";
619 std::cout <<
str << std::setw(spacing -
str.size()) << RTABMAP_VERSION << std::endl;
621 std::cout <<
str << std::setw(spacing -
str.size()) << PCL_VERSION_PRETTY << std::endl;
624 std::cout <<
str << std::setw(spacing -
str.size()) << vtkVersion::GetVTKVersion() << std::endl;
626 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
629 std::cout <<
str << std::setw(spacing -
str.size()) << CV_VERSION << std::endl;
630 #if CV_MAJOR_VERSION >= 3
631 str =
"With OpenCV xfeatures2d:";
632 #ifdef HAVE_OPENCV_XFEATURES2D
633 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
635 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
638 str =
"With OpenCV nonfree:";
639 #ifdef RTABMAP_NONFREE
640 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
642 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
644 str =
"With ORB OcTree:";
645 #ifdef RTABMAP_ORB_OCTREE
646 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
648 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
650 str =
"With SuperPoint Torch:";
652 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
654 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
656 str =
"With Python3:";
657 #ifdef RTABMAP_PYTHON
658 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
660 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
662 str =
"With FastCV:";
663 #ifdef RTABMAP_FASTCV
664 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
666 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
668 str =
"With OpenGV:";
669 #ifdef RTABMAP_OPENGV
670 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
672 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
674 str =
"With Madgwick:";
675 #ifdef RTABMAP_MADGWICK
676 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
678 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
682 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
684 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
688 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
690 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
694 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
696 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
700 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
702 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
704 str =
"With Vertigo:";
705 #ifdef RTABMAP_VERTIGO
706 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
708 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
712 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
714 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
718 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
720 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
722 str =
"With OpenNI:";
723 #ifdef RTABMAP_OPENNI
724 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
726 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
728 str =
"With OpenNI2:";
729 #ifdef RTABMAP_OPENNI2
730 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
732 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
734 str =
"With Freenect:";
735 #ifdef RTABMAP_FREENECT
736 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
738 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
740 str =
"With Freenect2:";
741 #ifdef RTABMAP_FREENECT2
742 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
744 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
748 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
750 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
754 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
756 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
758 str =
"With DC1394:";
759 #ifdef RTABMAP_DC1394
760 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
762 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
764 str =
"With FlyCapture2:";
765 #ifdef RTABMAP_FLYCAPTURE2
766 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
768 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
772 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
774 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
776 str =
"With ZED Open Capture:";
778 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
780 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
782 str =
"With RealSense:";
783 #ifdef RTABMAP_REALSENSE
784 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
786 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
788 str =
"With RealSense SLAM:";
789 #ifdef RTABMAP_REALSENSE_SLAM
790 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
792 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
794 str =
"With RealSense2:";
795 #ifdef RTABMAP_REALSENSE2
796 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
798 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
800 str =
"With MYNT EYE S:";
801 #ifdef RTABMAP_MYNTEYE
802 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
804 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
806 str =
"With DepthAI:";
807 #ifdef RTABMAP_DEPTHAI
808 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
810 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
812 str =
"With XVisio SDK:";
814 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
816 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
818 str =
"With libpointmatcher:";
819 #ifdef RTABMAP_POINTMATCHER
820 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
822 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
824 str =
"With CCCoreLib:";
825 #ifdef RTABMAP_CCCORELIB
826 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
828 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
830 str =
"With Open3D:";
831 #ifdef RTABMAP_OPEN3D
832 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
834 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
836 str =
"With OctoMap:";
837 #ifdef RTABMAP_OCTOMAP
838 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
840 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
842 str =
"With GridMap:";
843 #ifdef RTABMAP_GRIDMAP
844 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
846 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
848 str =
"With cpu-tsdf:";
849 #ifdef RTABMAP_CPUTSDF
850 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
852 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
854 str =
"With open chisel:";
855 #ifdef RTABMAP_OPENCHISEL
856 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
858 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
860 str =
"With Alice Vision:";
861 #ifdef RTABMAP_ALICE_VISION
862 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
864 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
868 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
870 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
874 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
876 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
880 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
882 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
886 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
888 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
892 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
894 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
896 #if RTABMAP_ORB_SLAM == 3
897 str =
"With ORB_SLAM3:";
898 #elif RTABMAP_ORB_SLAM == 2
899 str =
"With ORB_SLAM2:";
901 str =
"With ORB_SLAM:";
903 #ifdef RTABMAP_ORB_SLAM
904 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
906 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
910 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
912 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
914 str =
"With MSCKF_VIO:";
915 #ifdef RTABMAP_MSCKF_VIO
916 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
918 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
920 str =
"With VINS-Fusion:";
922 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
924 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
926 str =
"With OpenVINS:";
927 #ifdef RTABMAP_OPENVINS
928 std::cout <<
str << std::setw(spacing -
str.size()) <<
"true" << std::endl;
930 std::cout <<
str << std::setw(spacing -
str.size()) <<
"false" << std::endl;
934 else if(strcmp(
argv[
i],
"--nolog") == 0)
938 else if(strcmp(
argv[
i],
"--logconsole") == 0)
942 else if(strcmp(
argv[
i],
"--logfile") == 0)
951 UERROR(
"\"--logfile\" argument requires following file path");
954 else if(strcmp(
argv[
i],
"--logfilea") == 0)
963 UERROR(
"\"--logfilea\" argument requires following file path");
966 else if(strcmp(
argv[
i],
"--udebug") == 0)
970 else if(strcmp(
argv[
i],
"--uinfo") == 0)
974 else if(strcmp(
argv[
i],
"--uwarn") == 0)
978 else if(strcmp(
argv[
i],
"--uerror") == 0)
982 else if(strcmp(
argv[
i],
"--ulogtime") == 0)
991 UERROR(
"\"--ulogtime\" argument requires a following boolean value");
994 else if(strcmp(
argv[
i],
"--ulogwhere") == 0)
1003 UERROR(
"\"--ulogwhere\" argument requires a following boolean value");
1006 else if(strcmp(
argv[
i],
"--ulogthread") == 0)
1015 UERROR(
"\"--ulogthread\" argument requires a following boolean value");
1020 checkParameters =
true;
1026 if(strcmp(
argv[
i],
"--params") == 0)
1031 std::string dbName =
argv[argc - 1];
1039 if (!dbParameters.empty())
1041 std::cout <<
"[Core]" << std::endl;
1042 std::cout <<
"Version = " << RTABMAP_VERSION << std::endl;
1043 for (ParametersMap::const_iterator
iter = dbParameters.begin();
iter != dbParameters.end(); ++
iter)
1045 std::string
key =
iter->first;
1048 std::string
value =
iter->second.c_str();
1051 std::cout <<
key <<
" = " <<
value << std::endl;
1065 for(rtabmap::ParametersMap::const_iterator
iter=parameters.begin();
iter!=parameters.end(); ++
iter)
1067 bool ignore =
false;
1069 std::string group =
uSplit(
iter->first,
'/').front();
1070 #ifndef RTABMAP_GTSAM
1071 if(group.compare(
"GTSAM") == 0)
1077 if(group.compare(
"g2o") == 0)
1082 #ifndef RTABMAP_FOVIS
1083 if(group.compare(
"OdomFovis") == 0)
1088 #ifndef RTABMAP_VISO2
1089 if(group.compare(
"OdomViso2") == 0)
1094 #ifndef RTABMAP_ORBSLAM2
1095 if(group.compare(
"OdomORBSLAM2") == 0)
1100 #ifndef RTABMAP_OKVIS
1101 if(group.compare(
"OdomOKVIS") == 0)
1106 #if not defined(RTABMAP_LOAM) and not defined(RTABMAP_FLOAM)
1107 if(group.compare(
"OdomLOAM") == 0)
1112 #ifndef RTABMAP_MSCKF_VIO
1113 if(group.compare(
"OdomMSCKF") == 0)
1120 std::string
str =
"Param: " +
iter->first +
" = \"" +
iter->second +
"\"";
1123 std::setw(60 -
str.size()) <<
1135 ParametersMap::const_iterator
iter = parameters.find(
key);
1136 if(
iter != parameters.end())
1142 UINFO(
"Parsed parameter \"%s\"=\"%s\"",
iter->first.c_str(),
argv[
i]);
1148 std::map<std::string, std::pair<bool, std::string> >::const_iterator jter = removedParams.find(
key);
1149 if(jter!=removedParams.end())
1151 if(jter->second.first)
1160 key = jter->second.second;
1161 UWARN(
"Parameter migration from \"%s\" to \"%s\" (value=%s).",
1162 jter->first.c_str(), jter->second.second.c_str(),
value.c_str());
1168 UERROR(
"Value missing for argument \"%s\"",
argv[
i-1]);
1171 else if(jter->second.second.empty())
1173 UERROR(
"Parameter \"%s\" doesn't exist anymore.", jter->first.c_str());
1177 UERROR(
"Parameter \"%s\" doesn't exist anymore, check this similar parameter \"%s\".", jter->first.c_str(), jter->second.second.c_str());
1192 for(CSimpleIniA::TKeyVal::const_iterator
iter=keyValMap->begin();
iter!=keyValMap->end(); ++
iter)
1194 std::string
key = (*iter).first.pItem;
1195 if(
key.compare(
"Version") == 0)
1203 if(configFilePath.find(
".rtabmap") != std::string::npos)
1205 UWARN(
"Version in the config file \"%s\" is more recent (\"%s\") than "
1206 "current RTAB-Map version used (\"%s\"). The config file will be upgraded "
1208 configFilePath.c_str(),
1214 UERROR(
"Version in the config file \"%s\" is more recent (\"%s\") than "
1215 "current RTAB-Map version used (\"%s\"). New parameters (if there are some) will "
1217 configFilePath.c_str(),
1232 if(oldIter->second.first)
1234 if(parameters.find(oldIter->second.second) == parameters.end())
1236 key = oldIter->second.second;
1237 UINFO(
"Parameter migration from \"%s\" to \"%s\" (value=%s, default=%s).",
1241 else if(oldIter->second.second.empty())
1243 UWARN(
"Parameter \"%s\" doesn't exist anymore.",
1244 oldIter->first.c_str());
1246 else if(parameters.find(oldIter->second.second) == parameters.end())
1248 UWARN(
"Parameter \"%s\" (value=%s) doesn't exist anymore, you may want to use this similar parameter \"%s (default=%s): %s\".",
1266 ULOGGER_WARN(
"Section \"Core\" in %s doesn't exist... "
1267 "Ignore this warning if the ini file does not exist yet. "
1268 "The ini file will be automatically created when rtabmap will close.", configFilePath.c_str());
1277 readINIImpl(ini, configFile, parameters, modifiedOnly);
1293 ini.
SetValue(
"Core",
"Version", RTABMAP_VERSION,
NULL,
true);
1295 for(ParametersMap::const_iterator
iter=parameters.begin();
iter!=parameters.end(); ++
iter)
1297 std::string
key =
iter->first;
1300 std::string
value =
iter->second.c_str();
1309 for(std::map<std::string, std::pair<bool, std::string> >::const_iterator
iter =
removedParameters_.begin();
1313 std::string
key =
iter->first;
1318 if(ini.
Delete(
"Core",
key.c_str(),
true))
1320 if(
iter->second.first && parameters.find(
iter->second.second) != parameters.end())
1322 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());
1326 UWARN(
"Removed deprecated parameter %s=%s from \"%s\".",
iter->first.c_str(),
value.c_str(), configFile.c_str());