i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added RGBD/MaxLocalizationDistance parameter

Browse Source
This commit is contained in:
matlabbe
2019-06-12 16:03:52 -04:00
parent b5f5623af4
commit 85edc57ba5
5 changed files with 80 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/Parameters.h
View File

@@ -342,6 +342,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 3.0, uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
RTABMAP_PARAM(RGBD, MaxLocalizationDistance, float, 0.0, "Reject localizations if the distance from the map is over this distance (0=disabled). Only used in localization mode.");
RTABMAP_PARAM(RGBD, SavedLocalizationIgnored, bool, false, "Ignore last saved localization pose from previous session. If true, RTAB-Map won't assume it is restarting from the same place than where it shut down previously.");
RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m).");
RTABMAP_PARAM(RGBD, PlanStuckIterations, int, 0, "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");

1
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -230,6 +230,7 @@ private:
unsigned int _maxMemoryAllowed; // signatures count in WM
float _loopThr;
float _loopRatio;
float _localizationMaxDistance;
bool _verifyLoopClosureHypothesis;
unsigned int _maxRetrieved;
unsigned int _maxLocalRetrieved;

25
corelib/src/Rtabmap.cpp
View File

@@ -87,6 +87,7 @@ Rtabmap::Rtabmap() :
_maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
_loopThr(Parameters::defaultRtabmapLoopThr()),
_loopRatio(Parameters::defaultRtabmapLoopRatio()),
_localizationMaxDistance(Parameters::defaultRGBDMaxLocalizationDistance()),
_verifyLoopClosureHypothesis(Parameters::defaultVhEpEnabled()),
_maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
_maxLocalRetrieved(Parameters::defaultRGBDMaxLocalRetrieved()),
@@ -441,6 +442,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed);
Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr);
Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio);
Parameters::parse(parameters, Parameters::kRGBDMaxLocalizationDistance(), _localizationMaxDistance);
Parameters::parse(parameters, Parameters::kVhEpEnabled(), _verifyLoopClosureHypothesis);
Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved);
Parameters::parse(parameters, Parameters::kRGBDMaxLocalRetrieved(), _maxLocalRetrieved);
@@ -2143,6 +2145,12 @@ bool Rtabmap::process(
UWARN("Rejected loop closure %d -> %d: %s",
_loopClosureHypothesis.first, signature->id(), info.rejectedMsg.c_str());
}
else if(!_memory->isIncremental() && _localizationMaxDistance>0.0f && transform.getNorm() > _localizationMaxDistance)
{
rejectedHypothesis = true;
UWARN("Rejected localization %d -> %d because distance to map (%fm) is over %s=%fm.",
_loopClosureHypothesis.first, signature->id(), transform.getNorm(), Parameters::kRGBDMaxLocalizationDistance().c_str(), _localizationMaxDistance);
}
else
{
transform = transform.inverse();
@@ -2266,6 +2274,11 @@ bool Rtabmap::process(
}
UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths);
float proximityFilteringRadius = _proximityFilteringRadius;
if(!_memory->isIncremental() && _localizationMaxDistance>0.0f && (proximityFilteringRadius <= 0.0f || _localizationMaxDistance<proximityFilteringRadius))
{
proximityFilteringRadius = _localizationMaxDistance;
}
for(std::map<NearestPathKey, std::map<int, Transform> >::const_reverse_iterator iter=nearestPaths.rbegin();
iter!=nearestPaths.rend() &&
(_memory->isIncremental() || lastProximitySpaceClosureId == 0) &&
@@ -2294,8 +2307,8 @@ bool Rtabmap::process(
{
// nearest pose must not be linked to current location and enough close
if(!signature->hasLink(nearestId) &&
(_proximityFilteringRadius <= 0.0f ||
_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
(proximityFilteringRadius <= 0.0f ||
_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < proximityFilteringRadius*proximityFilteringRadius))
{
++localVisualPathsChecked;
RegistrationInfo info;
@@ -2304,7 +2317,7 @@ bool Rtabmap::process(
if(!transform.isNull())
{
transform = transform.inverse();
if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
if(proximityFilteringRadius <= 0 || transform.getNormSquared() <= proximityFilteringRadius*proximityFilteringRadius)
{
UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s",
signature->id(),
@@ -2331,7 +2344,7 @@ bool Rtabmap::process(
{
UWARN("Ignoring local loop closure with %d because resulting "
"transform is too large!? (%fm > %fm)",
nearestId, transform.getNorm(), _proximityFilteringRadius);
nearestId, transform.getNorm(), proximityFilteringRadius);
}
}
}
@@ -2414,10 +2427,10 @@ bool Rtabmap::process(
}
std::map<int, Transform> filteredPath;
if(optimizedLocalPath.size() > 2 && _proximityFilteringRadius > 0.0f)
if(optimizedLocalPath.size() > 2 && proximityFilteringRadius > 0.0f)
{
// path filtering
filteredPath = graph::radiusPosesFiltering(optimizedLocalPath, _proximityFilteringRadius, 0, true);
filteredPath = graph::radiusPosesFiltering(optimizedLocalPath, proximityFilteringRadius, 0, true);
// make sure the current pose is still here
filteredPath.insert(*optimizedLocalPath.find(nearestId));
}