mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-03 01:50:24 +08:00
Added parameter "RGBD/OptimizeMaxError" (default: 1 m) to reject loop closures after too large optimization errors
This commit is contained in:
@@ -284,18 +284,19 @@ class RTABMAP_EXP Parameters
|
|||||||
RTABMAP_PARAM(VhEp, RansacParam2, float, 0.99, "Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC.");
|
RTABMAP_PARAM(VhEp, RansacParam2, float, 0.99, "Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC.");
|
||||||
|
|
||||||
// RGB-D SLAM
|
// RGB-D SLAM
|
||||||
RTABMAP_PARAM(RGBD, Enabled, bool, true, "");
|
RTABMAP_PARAM(RGBD, Enabled, bool, true, "");
|
||||||
RTABMAP_PARAM(RGBD, PoseScanMatching, bool, false, "Laser scan matching for odometry pose correction (laser scans are required).");
|
RTABMAP_PARAM(RGBD, PoseScanMatching, bool, false, "Laser scan matching for odometry pose correction (laser scans are required).");
|
||||||
RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.0, "Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
|
RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.0, "Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
|
||||||
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
|
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
|
||||||
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, 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 mode 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, 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 mode 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, GoalReachedRadius, float, 0.5, "Goal reached radius (m).");
|
RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 1.0, "Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph.");
|
||||||
RTABMAP_PARAM(RGBD, PlanVirtualLinks, bool, true, "Before planning in the graph, close nodes are linked together. Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
|
RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m).");
|
||||||
|
RTABMAP_PARAM(RGBD, PlanVirtualLinks, bool, true, "Before planning in the graph, close nodes are linked together. Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
|
||||||
RTABMAP_PARAM(RGBD, GoalsSavedInUserData, bool, false, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
|
RTABMAP_PARAM(RGBD, GoalsSavedInUserData, bool, false, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
|
||||||
RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
|
RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
|
||||||
RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
|
RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
|
||||||
RTABMAP_PARAM(RGBD, LocalImmunizationRatio, float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer.");
|
RTABMAP_PARAM(RGBD, LocalImmunizationRatio, float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer.");
|
||||||
|
|
||||||
// Local loop closure detection
|
// Local loop closure detection
|
||||||
RTABMAP_PARAM(RGBD, LocalLoopDetectionTime, bool, false, "Detection over all locations in STM.");
|
RTABMAP_PARAM(RGBD, LocalLoopDetectionTime, bool, false, "Detection over all locations in STM.");
|
||||||
|
|||||||
@@ -191,6 +191,7 @@ private:
|
|||||||
bool _localPathOdomPosesUsed;
|
bool _localPathOdomPosesUsed;
|
||||||
std::string _databasePath;
|
std::string _databasePath;
|
||||||
bool _optimizeFromGraphEnd;
|
bool _optimizeFromGraphEnd;
|
||||||
|
float _optimizationMaxLinearError;
|
||||||
bool _reextractLoopClosureFeatures;
|
bool _reextractLoopClosureFeatures;
|
||||||
int _reextractNNType;
|
int _reextractNNType;
|
||||||
float _reextractNNDR;
|
float _reextractNNDR;
|
||||||
@@ -205,6 +206,7 @@ private:
|
|||||||
std::pair<int, float> _loopClosureHypothesis;
|
std::pair<int, float> _loopClosureHypothesis;
|
||||||
std::pair<int, float> _highestHypothesis;
|
std::pair<int, float> _highestHypothesis;
|
||||||
double _lastProcessTime;
|
double _lastProcessTime;
|
||||||
|
bool _someNodesHaveBeenTransferred;
|
||||||
|
|
||||||
// Abstract classes containing all loop closure
|
// Abstract classes containing all loop closure
|
||||||
// strategies for a type of signature or configuration.
|
// strategies for a type of signature or configuration.
|
||||||
|
|||||||
@@ -57,11 +57,12 @@ class RTABMAP_EXP Statistics
|
|||||||
RTABMAP_STATS(Loop, Highest_hypothesis_id,);
|
RTABMAP_STATS(Loop, Highest_hypothesis_id,);
|
||||||
RTABMAP_STATS(Loop, Highest_hypothesis_value,);
|
RTABMAP_STATS(Loop, Highest_hypothesis_value,);
|
||||||
RTABMAP_STATS(Loop, Vp_hypothesis,);
|
RTABMAP_STATS(Loop, Vp_hypothesis,);
|
||||||
RTABMAP_STATS(Loop, ReactivateId,);
|
RTABMAP_STATS(Loop, Reactivate_id,);
|
||||||
RTABMAP_STATS(Loop, Hypothesis_ratio,);
|
RTABMAP_STATS(Loop, Hypothesis_ratio,);
|
||||||
RTABMAP_STATS(Loop, Hypothesis_reactivated,);
|
RTABMAP_STATS(Loop, Hypothesis_reactivated,);
|
||||||
RTABMAP_STATS(Loop, VisualInliers,);
|
RTABMAP_STATS(Loop, Visual_inliers,);
|
||||||
RTABMAP_STATS(Loop, Last_id,);
|
RTABMAP_STATS(Loop, Last_id,);
|
||||||
|
RTABMAP_STATS(Loop, Optimization_max_error, m);
|
||||||
|
|
||||||
RTABMAP_STATS(LocalLoop, Time_closures,);
|
RTABMAP_STATS(LocalLoop, Time_closures,);
|
||||||
RTABMAP_STATS(LocalLoop, Space_last_closure_id,);
|
RTABMAP_STATS(LocalLoop, Space_last_closure_id,);
|
||||||
|
|||||||
@@ -100,6 +100,7 @@ Rtabmap::Rtabmap() :
|
|||||||
_localPathOdomPosesUsed(Parameters::defaultRGBDLocalLoopDetectionPathOdomPosesUsed()),
|
_localPathOdomPosesUsed(Parameters::defaultRGBDLocalLoopDetectionPathOdomPosesUsed()),
|
||||||
_databasePath(""),
|
_databasePath(""),
|
||||||
_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
|
_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
|
||||||
|
_optimizationMaxLinearError(Parameters::defaultRGBDOptimizeMaxError()),
|
||||||
_reextractLoopClosureFeatures(Parameters::defaultLccReextractActivated()),
|
_reextractLoopClosureFeatures(Parameters::defaultLccReextractActivated()),
|
||||||
_reextractNNType(Parameters::defaultLccReextractNNType()),
|
_reextractNNType(Parameters::defaultLccReextractNNType()),
|
||||||
_reextractNNDR(Parameters::defaultLccReextractNNDR()),
|
_reextractNNDR(Parameters::defaultLccReextractNNDR()),
|
||||||
@@ -113,6 +114,7 @@ Rtabmap::Rtabmap() :
|
|||||||
_loopClosureHypothesis(0,0.0f),
|
_loopClosureHypothesis(0,0.0f),
|
||||||
_highestHypothesis(0,0.0f),
|
_highestHypothesis(0,0.0f),
|
||||||
_lastProcessTime(0.0),
|
_lastProcessTime(0.0),
|
||||||
|
_someNodesHaveBeenTransferred(false),
|
||||||
_epipolarGeometry(0),
|
_epipolarGeometry(0),
|
||||||
_bayesFilter(0),
|
_bayesFilter(0),
|
||||||
_graphOptimizer(0),
|
_graphOptimizer(0),
|
||||||
@@ -319,6 +321,7 @@ void Rtabmap::close()
|
|||||||
_highestHypothesis = std::make_pair(0,0.0f);
|
_highestHypothesis = std::make_pair(0,0.0f);
|
||||||
_loopClosureHypothesis = std::make_pair(0,0.0f);
|
_loopClosureHypothesis = std::make_pair(0,0.0f);
|
||||||
_lastProcessTime = 0.0;
|
_lastProcessTime = 0.0;
|
||||||
|
_someNodesHaveBeenTransferred = false;
|
||||||
_optimizedPoses.clear();
|
_optimizedPoses.clear();
|
||||||
_constraints.clear();
|
_constraints.clear();
|
||||||
_mapCorrection.setIdentity();
|
_mapCorrection.setIdentity();
|
||||||
@@ -398,6 +401,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
|
|||||||
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathFilteringRadius(), _localPathFilteringRadius);
|
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathFilteringRadius(), _localPathFilteringRadius);
|
||||||
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed(), _localPathOdomPosesUsed);
|
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed(), _localPathOdomPosesUsed);
|
||||||
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
|
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
|
||||||
|
Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxLinearError);
|
||||||
Parameters::parse(parameters, Parameters::kLccReextractActivated(), _reextractLoopClosureFeatures);
|
Parameters::parse(parameters, Parameters::kLccReextractActivated(), _reextractLoopClosureFeatures);
|
||||||
Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType);
|
Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType);
|
||||||
Parameters::parse(parameters, Parameters::kLccReextractNNDR(), _reextractNNDR);
|
Parameters::parse(parameters, Parameters::kLccReextractNNDR(), _reextractNNDR);
|
||||||
@@ -829,6 +833,7 @@ void Rtabmap::resetMemory()
|
|||||||
_highestHypothesis = std::make_pair(0,0.0f);
|
_highestHypothesis = std::make_pair(0,0.0f);
|
||||||
_loopClosureHypothesis = std::make_pair(0,0.0f);
|
_loopClosureHypothesis = std::make_pair(0,0.0f);
|
||||||
_lastProcessTime = 0.0;
|
_lastProcessTime = 0.0;
|
||||||
|
_someNodesHaveBeenTransferred = false;
|
||||||
_optimizedPoses.clear();
|
_optimizedPoses.clear();
|
||||||
_constraints.clear();
|
_constraints.clear();
|
||||||
_mapCorrection.setIdentity();
|
_mapCorrection.setIdentity();
|
||||||
@@ -902,7 +907,6 @@ bool Rtabmap::process(
|
|||||||
std::map<int, int> childCount;
|
std::map<int, int> childCount;
|
||||||
std::set<int> signaturesRetrieved;
|
std::set<int> signaturesRetrieved;
|
||||||
int localLoopClosuresInTimeFound = 0;
|
int localLoopClosuresInTimeFound = 0;
|
||||||
bool scanMatchingSuccess = false;
|
|
||||||
|
|
||||||
const Signature * signature = 0;
|
const Signature * signature = 0;
|
||||||
const Signature * sLoop = 0;
|
const Signature * sLoop = 0;
|
||||||
@@ -1044,49 +1048,77 @@ bool Rtabmap::process(
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
Transform newPose = _mapCorrection * signature->getPose();
|
// Update optimizedPoses with the newly added node
|
||||||
_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
|
Transform newPose;
|
||||||
_lastLocalizationPose = newPose; // used in localization mode only (path planning)
|
if(signature->getLinks().size() == 1)
|
||||||
|
|
||||||
//============================================================
|
|
||||||
// Scan matching
|
|
||||||
//============================================================
|
|
||||||
if(_poseScanMatching &&
|
|
||||||
signature->getLinks().size() == 1 &&
|
|
||||||
!signature->sensorData().laserScanCompressed().empty() &&
|
|
||||||
rehearsedId == 0) // don't do it if rehearsal happened
|
|
||||||
{
|
{
|
||||||
UINFO("Odometry correction by scan matching");
|
|
||||||
int oldId = signature->getLinks().begin()->first;
|
int oldId = signature->getLinks().begin()->first;
|
||||||
const Signature * oldS = _memory->getSignature(oldId);
|
const Signature * oldS = _memory->getSignature(oldId);
|
||||||
UASSERT(oldS != 0);
|
UASSERT(oldS != 0);
|
||||||
std::string rejectedMsg;
|
|
||||||
Transform guess = signature->getLinks().begin()->second.transform();
|
//============================================================
|
||||||
double variance = 1.0;
|
// Scan matching
|
||||||
int inliers = 0;
|
//============================================================
|
||||||
float inliersRatio = 0;
|
if(_poseScanMatching &&
|
||||||
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio);
|
!signature->sensorData().laserScanCompressed().empty() &&
|
||||||
if(!t.isNull())
|
rehearsedId == 0) // don't do it if rehearsal happened
|
||||||
{
|
{
|
||||||
scanMatchingSuccess = true;
|
UINFO("Odometry correction by scan matching");
|
||||||
UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s",
|
Transform guess = signature->getLinks().begin()->second.transform();
|
||||||
signature->id(),
|
double variance = 1.0;
|
||||||
oldId,
|
int inliers = 0;
|
||||||
signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
|
float inliersRatio = 0;
|
||||||
t.prettyPrint().c_str());
|
std::string rejectedMsg;
|
||||||
_memory->updateLink(signature->id(), oldId, t, variance, variance);
|
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio);
|
||||||
|
if(!t.isNull())
|
||||||
|
{
|
||||||
|
UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s",
|
||||||
|
signature->id(),
|
||||||
|
oldId,
|
||||||
|
signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
|
||||||
|
t.prettyPrint().c_str());
|
||||||
|
_memory->updateLink(signature->id(), oldId, t, variance, variance);
|
||||||
|
|
||||||
|
if(_optimizeFromGraphEnd)
|
||||||
|
{
|
||||||
|
// update all previous nodes
|
||||||
|
// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
|
||||||
|
// parameters just changed state, we should put back all poses without map correction.
|
||||||
|
Transform u = guess.inverse() * t;
|
||||||
|
Transform mapCorrectionInv = _mapCorrection.inverse();
|
||||||
|
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
|
||||||
|
{
|
||||||
|
iter->second = mapCorrectionInv * iter->second * u;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
UINFO("Scan matching rejected: %s", rejectedMsg.c_str());
|
||||||
|
}
|
||||||
|
statistics_.addStatistic(Statistics::kOdomCorrectionAccepted(), !t.isNull()?1.0f:0);
|
||||||
|
statistics_.addStatistic(Statistics::kOdomCorrectionInliers(), inliers);
|
||||||
|
statistics_.addStatistic(Statistics::kOdomCorrectionInliers_ratio(), inliersRatio);
|
||||||
|
statistics_.addStatistic(Statistics::kOdomCorrectionVariance(), variance);
|
||||||
}
|
}
|
||||||
else
|
timeScanMatching = timer.ticks();
|
||||||
|
ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching);
|
||||||
|
|
||||||
|
UASSERT(oldS->hasLink(signature->id()));
|
||||||
|
UASSERT(uContains(_optimizedPoses, oldId));
|
||||||
|
newPose = _optimizedPoses.at(oldId) * oldS->getLinks().at(signature->id()).transform();
|
||||||
|
_mapCorrection = newPose * signature->getPose().inverse();
|
||||||
|
if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
|
||||||
{
|
{
|
||||||
UINFO("Scan matching rejected: %s", rejectedMsg.c_str());
|
UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str());
|
||||||
}
|
}
|
||||||
statistics_.addStatistic(Statistics::kOdomCorrectionAccepted(), scanMatchingSuccess?1.0f:0);
|
|
||||||
statistics_.addStatistic(Statistics::kOdomCorrectionInliers(), inliers);
|
|
||||||
statistics_.addStatistic(Statistics::kOdomCorrectionInliers_ratio(), inliersRatio);
|
|
||||||
statistics_.addStatistic(Statistics::kOdomCorrectionVariance(), variance);
|
|
||||||
}
|
}
|
||||||
timeScanMatching = timer.ticks();
|
else
|
||||||
ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching);
|
{
|
||||||
|
newPose = _mapCorrection * signature->getPose();
|
||||||
|
}
|
||||||
|
_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
|
||||||
|
_lastLocalizationPose = newPose; // used in localization mode only (path planning)
|
||||||
|
|
||||||
if(signature->getLinks().size() == 1)
|
if(signature->getLinks().size() == 1)
|
||||||
{
|
{
|
||||||
@@ -1543,7 +1575,7 @@ bool Rtabmap::process(
|
|||||||
if(immunizedLocally >= maxLocalLocationsImmunized)
|
if(immunizedLocally >= maxLocalLocationsImmunized)
|
||||||
{
|
{
|
||||||
// set 20 to avoid this warning when starting mapping
|
// set 20 to avoid this warning when starting mapping
|
||||||
if(maxLocalLocationsImmunized > 20)
|
if(maxLocalLocationsImmunized > 20 && _someNodesHaveBeenTransferred)
|
||||||
{
|
{
|
||||||
UWARN("Could not immunize the whole local path (%d) between "
|
UWARN("Could not immunize the whole local path (%d) between "
|
||||||
"%d and %d (max location immunized=%d). You may want "
|
"%d and %d (max location immunized=%d). You may want "
|
||||||
@@ -1675,6 +1707,7 @@ bool Rtabmap::process(
|
|||||||
// Update loop closure links
|
// Update loop closure links
|
||||||
// (updated: place this after retrieval to be sure that neighbors of the loop closure are in RAM)
|
// (updated: place this after retrieval to be sure that neighbors of the loop closure are in RAM)
|
||||||
//=============================================================
|
//=============================================================
|
||||||
|
std::list<std::pair<int, int> > loopClosureLinksAdded;
|
||||||
int loopClosureVisualInliers = 0; // for statistics
|
int loopClosureVisualInliers = 0; // for statistics
|
||||||
if(_loopClosureHypothesis.first>0)
|
if(_loopClosureHypothesis.first>0)
|
||||||
{
|
{
|
||||||
@@ -1758,6 +1791,10 @@ bool Rtabmap::process(
|
|||||||
{
|
{
|
||||||
// Make the new one the parent of the old one
|
// Make the new one the parent of the old one
|
||||||
rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, variance, variance));
|
rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, variance, variance));
|
||||||
|
if(!rejectedHypothesis)
|
||||||
|
{
|
||||||
|
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if(rejectedHypothesis)
|
if(rejectedHypothesis)
|
||||||
@@ -1911,6 +1948,7 @@ bool Rtabmap::process(
|
|||||||
nearestId,
|
nearestId,
|
||||||
transform.prettyPrint().c_str());
|
transform.prettyPrint().c_str());
|
||||||
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance, variance));
|
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance, variance));
|
||||||
|
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
|
||||||
|
|
||||||
if(_loopClosureHypothesis.first == 0)
|
if(_loopClosureHypothesis.first == 0)
|
||||||
{
|
{
|
||||||
@@ -2000,6 +2038,7 @@ bool Rtabmap::process(
|
|||||||
transform.prettyPrint().c_str());
|
transform.prettyPrint().c_str());
|
||||||
// set Identify covariance for laser scan matching only
|
// set Identify covariance for laser scan matching only
|
||||||
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, 1, 1));
|
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, 1, 1));
|
||||||
|
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
|
||||||
|
|
||||||
++localSpaceClosuresAddedByICPOnly;
|
++localSpaceClosuresAddedByICPOnly;
|
||||||
|
|
||||||
@@ -2027,10 +2066,10 @@ bool Rtabmap::process(
|
|||||||
//============================================================
|
//============================================================
|
||||||
// Optimize map graph
|
// Optimize map graph
|
||||||
//============================================================
|
//============================================================
|
||||||
|
float maxLinearError = 0.0f;
|
||||||
if(_rgbdSlamMode &&
|
if(_rgbdSlamMode &&
|
||||||
(_loopClosureHypothesis.first>0 || // can be different map of the current one
|
(_loopClosureHypothesis.first>0 || // can be different map of the current one
|
||||||
localLoopClosuresInTimeFound>0 || // only same map of the current one
|
localLoopClosuresInTimeFound>0 || // only same map of the current one
|
||||||
scanMatchingSuccess || // only same map of the current one
|
|
||||||
lastLocalSpaceClosureId>0 || // can be different map of the current one
|
lastLocalSpaceClosureId>0 || // can be different map of the current one
|
||||||
signaturesRetrieved.size())) // can be different map of the current one
|
signaturesRetrieved.size())) // can be different map of the current one
|
||||||
{
|
{
|
||||||
@@ -2038,8 +2077,61 @@ bool Rtabmap::process(
|
|||||||
{
|
{
|
||||||
UINFO("Update map correction: SLAM mode");
|
UINFO("Update map correction: SLAM mode");
|
||||||
// SLAM mode!
|
// SLAM mode!
|
||||||
optimizeCurrentMap(signature->id(), false, _optimizedPoses, &_constraints);
|
std::map<int, Transform> poses = _optimizedPoses;
|
||||||
UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
|
std::multimap<int, Link> constraints;
|
||||||
|
optimizeCurrentMap(signature->id(), false, poses, &constraints);
|
||||||
|
UASSERT(poses.find(signature->id()) != poses.end());
|
||||||
|
|
||||||
|
// Check added loop closures have broken the graph
|
||||||
|
// (in case of wrong loop closures).
|
||||||
|
bool updateConstraints = true;
|
||||||
|
if(_optimizationMaxLinearError > 0.0f && loopClosureLinksAdded.size())
|
||||||
|
{
|
||||||
|
const Link * maxLinearLink = 0;
|
||||||
|
for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
|
||||||
|
{
|
||||||
|
Transform t1 = uValue(poses, iter->second.from(), Transform());
|
||||||
|
Transform t2 = uValue(poses, iter->second.to(), Transform());
|
||||||
|
Transform t = t1.inverse()*t2;
|
||||||
|
float linearError = uMax3(
|
||||||
|
fabs(iter->second.transform().x() - t.x()),
|
||||||
|
fabs(iter->second.transform().y() - t.y()),
|
||||||
|
fabs(iter->second.transform().z() - t.z()));
|
||||||
|
if(linearError > maxLinearError)
|
||||||
|
{
|
||||||
|
maxLinearError = linearError;
|
||||||
|
maxLinearLink = &iter->second;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if(maxLinearError > _optimizationMaxLinearError)
|
||||||
|
{
|
||||||
|
UWARN("Rejecting all added loop closures (%d) in this "
|
||||||
|
"iteration because a wrong loop closure has been "
|
||||||
|
"detected after graph optimization, resulting in "
|
||||||
|
"a maximum graph error of %f m (edge %d->%d). The "
|
||||||
|
"maximum error parameter is %f m.",
|
||||||
|
(int)loopClosureLinksAdded.size(),
|
||||||
|
maxLinearError,
|
||||||
|
maxLinearLink->from(),
|
||||||
|
maxLinearLink->to(),
|
||||||
|
_optimizationMaxLinearError);
|
||||||
|
for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
|
||||||
|
{
|
||||||
|
_memory->removeLink(iter->first, iter->second);
|
||||||
|
UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
|
||||||
|
}
|
||||||
|
updateConstraints = false;
|
||||||
|
_loopClosureHypothesis.first = 0;
|
||||||
|
lastLocalSpaceClosureId = 0;
|
||||||
|
rejectedHypothesis = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if(updateConstraints)
|
||||||
|
{
|
||||||
|
_optimizedPoses = poses;
|
||||||
|
_constraints = constraints;
|
||||||
|
}
|
||||||
|
|
||||||
// Update map correction, it should be identify when optimizing from the last node
|
// Update map correction, it should be identify when optimizing from the last node
|
||||||
_mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
|
_mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
|
||||||
@@ -2138,10 +2230,11 @@ bool Rtabmap::process(
|
|||||||
statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_value(), _highestHypothesis.second);
|
statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_value(), _highestHypothesis.second);
|
||||||
statistics_.addStatistic(Statistics::kLoopHypothesis_reactivated(), lcHypothesisReactivated);
|
statistics_.addStatistic(Statistics::kLoopHypothesis_reactivated(), lcHypothesisReactivated);
|
||||||
statistics_.addStatistic(Statistics::kLoopVp_hypothesis(), vpHypothesis);
|
statistics_.addStatistic(Statistics::kLoopVp_hypothesis(), vpHypothesis);
|
||||||
statistics_.addStatistic(Statistics::kLoopReactivateId(), retrievalId);
|
statistics_.addStatistic(Statistics::kLoopReactivate_id(), retrievalId);
|
||||||
statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
|
statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
|
||||||
statistics_.addStatistic(Statistics::kLoopVisualInliers(), loopClosureVisualInliers);
|
statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers);
|
||||||
statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
|
statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
|
||||||
|
statistics_.addStatistic(Statistics::kLoopOptimization_max_error(), maxLinearError);
|
||||||
|
|
||||||
statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);
|
statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);
|
||||||
statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added_visually(), localSpaceClosuresAddedVisually);
|
statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added_visually(), localSpaceClosuresAddedVisually);
|
||||||
@@ -2273,6 +2366,10 @@ bool Rtabmap::process(
|
|||||||
ULOGGER_INFO("Removing old signatures because time limit is reached %f>%f or memory is reached %d>%d...", totalTime*1000, _maxTimeAllowed, _memory->getWorkingMem().size(), _maxMemoryAllowed);
|
ULOGGER_INFO("Removing old signatures because time limit is reached %f>%f or memory is reached %d>%d...", totalTime*1000, _maxTimeAllowed, _memory->getWorkingMem().size(), _maxMemoryAllowed);
|
||||||
std::list<int> transferred = _memory->forget(immunizedLocations);
|
std::list<int> transferred = _memory->forget(immunizedLocations);
|
||||||
signaturesRemoved.insert(signaturesRemoved.end(), transferred.begin(), transferred.end());
|
signaturesRemoved.insert(signaturesRemoved.end(), transferred.begin(), transferred.end());
|
||||||
|
if(!_someNodesHaveBeenTransferred && transferred.size())
|
||||||
|
{
|
||||||
|
_someNodesHaveBeenTransferred = true; // only used to hide a warning on close ndoes immunization
|
||||||
|
}
|
||||||
}
|
}
|
||||||
_lastProcessTime = totalTime;
|
_lastProcessTime = totalTime;
|
||||||
|
|
||||||
|
|||||||
@@ -578,6 +578,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
|
|||||||
_ui->graphOptimization_iterations->setObjectName(Parameters::kRGBDOptimizeIterations().c_str());
|
_ui->graphOptimization_iterations->setObjectName(Parameters::kRGBDOptimizeIterations().c_str());
|
||||||
_ui->graphOptimization_covarianceIgnored->setObjectName(Parameters::kRGBDOptimizeVarianceIgnored().c_str());
|
_ui->graphOptimization_covarianceIgnored->setObjectName(Parameters::kRGBDOptimizeVarianceIgnored().c_str());
|
||||||
_ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
|
_ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
|
||||||
|
_ui->graphOptimization_maxError->setObjectName(Parameters::kRGBDOptimizeMaxError().c_str());
|
||||||
_ui->graphOptimization_stopEpsilon->setObjectName(Parameters::kRGBDOptimizeEpsilon().c_str());
|
_ui->graphOptimization_stopEpsilon->setObjectName(Parameters::kRGBDOptimizeEpsilon().c_str());
|
||||||
_ui->graphOptimization_robust->setObjectName(Parameters::kRGBDOptimizeRobust().c_str());
|
_ui->graphOptimization_robust->setObjectName(Parameters::kRGBDOptimizeRobust().c_str());
|
||||||
|
|
||||||
|
|||||||
@@ -63,9 +63,9 @@
|
|||||||
<property name="geometry">
|
<property name="geometry">
|
||||||
<rect>
|
<rect>
|
||||||
<x>0</x>
|
<x>0</x>
|
||||||
<y>-392</y>
|
<y>-509</y>
|
||||||
<width>760</width>
|
<width>760</width>
|
||||||
<height>1570</height>
|
<height>1598</height>
|
||||||
</rect>
|
</rect>
|
||||||
</property>
|
</property>
|
||||||
<layout class="QVBoxLayout" name="verticalLayout_16">
|
<layout class="QVBoxLayout" name="verticalLayout_16">
|
||||||
@@ -6735,6 +6735,41 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
|
|||||||
</property>
|
</property>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
|
<item row="10" column="1">
|
||||||
|
<widget class="QLabel" name="label_259">
|
||||||
|
<property name="text">
|
||||||
|
<string>Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph.</string>
|
||||||
|
</property>
|
||||||
|
<property name="wordWrap">
|
||||||
|
<bool>true</bool>
|
||||||
|
</property>
|
||||||
|
<property name="textInteractionFlags">
|
||||||
|
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="10" column="0">
|
||||||
|
<widget class="QDoubleSpinBox" name="graphOptimization_maxError">
|
||||||
|
<property name="suffix">
|
||||||
|
<string> m</string>
|
||||||
|
</property>
|
||||||
|
<property name="decimals">
|
||||||
|
<number>2</number>
|
||||||
|
</property>
|
||||||
|
<property name="minimum">
|
||||||
|
<double>0.000000000000000</double>
|
||||||
|
</property>
|
||||||
|
<property name="maximum">
|
||||||
|
<double>100.000000000000000</double>
|
||||||
|
</property>
|
||||||
|
<property name="singleStep">
|
||||||
|
<double>1.000000000000000</double>
|
||||||
|
</property>
|
||||||
|
<property name="value">
|
||||||
|
<double>1.000000000000000</double>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
</layout>
|
</layout>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
|
|||||||
Reference in New Issue
Block a user