i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-05 01:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added RGBD/LocalizationSmoothing parameter and fixed related issues (#1032)

Browse Source 
* removed code

* added landmark in graph optimization checks

* reverted smoothing, only if there is already a previous localization link

* Added RGBDLocalizationSmoothing parameter

* Update .appveyor.yml

* Update .appveyor.yml

* Update .appveyor.yml

* Update .appveyor.yml

* Update .appveyor.yml

* Update .appveyor.yml

* Update .appveyor.yml

* Update .appveyor.yml

* Update .appveyor.yml
This commit is contained in:
matlabbe
2023-05-14 13:03:52 -07:00
committed by GitHub
parent 2da448f4ee
commit ba33c080bc
8 changed files with 320 additions and 273 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+27 -15
View File
@@ -147,6 +147,7 @@ Rtabmap::Rtabmap() :
_loopCovLimited(Parameters::defaultRGBDLoopCovLimited()),
_loopGPS(Parameters::defaultRtabmapLoopGPS()),
_maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()),
_localizationSmoothing(Parameters::defaultRGBDLocalizationSmoothing()),
_createGlobalScanMap(Parameters::defaultRGBDProximityGlobalScanMap()),
_markerPriorsLinearVariance(Parameters::defaultMarkerPriorsVarianceLinear()),
_markerPriorsAngularVariance(Parameters::defaultMarkerPriorsVarianceAngular()),
@@ -618,6 +619,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited);
Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS);
Parameters::parse(parameters, Parameters::kRGBDMaxOdomCacheSize(), _maxOdomCacheSize);
Parameters::parse(parameters, Parameters::kRGBDLocalizationSmoothing(), _localizationSmoothing);
Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap);
Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), _markerPriorsLinearVariance);
@@ -3173,10 +3175,10 @@ bool Rtabmap::process(
&maxLinearLink,
&maxAngularLink,
_graphOptimizer->isSlam2d());
if(maxLinearLink == 0 && maxAngularLink==0 && _maxOdomCacheSize>0)
if(maxLinearLink == 0 && maxAngularLink==0)
{
UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
optPoses = posesOut;
UWARN("Could not compute graph errors! Rejecting localization!");
rejectLocalization = true;
}
if(maxLinearLink)
@@ -3287,10 +3289,10 @@ bool Rtabmap::process(
&maxLinearLink,
&maxAngularLink,
_graphOptimizer->isSlam2d());
if(maxLinearLink == 0 && maxAngularLink==0 && _maxOdomCacheSize>0)
if(maxLinearLink == 0 && maxAngularLink==0)
{
UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
optPoses = posesOut;
UWARN("Could not compute graph errors! Rejecting localization!");
rejectLocalization = true;
}
if(maxLinearLink)
@@ -3395,16 +3397,26 @@ bool Rtabmap::process(
Transform newOptPoseInv = optPoses.at(signature->id()).inverse();
for(std::multimap<int, Link>::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter)
{
Transform newT = newOptPoseInv * optPoses.at(iter->first);
UDEBUG("Adjusted localization link %d->%d after optimization", iter->second.from(), iter->second.to());
UDEBUG("from %s", iter->second.transform().prettyPrint().c_str());
UDEBUG(" to %s", newT.prettyPrint().c_str());
iter->second.setTransform(newT);
// Update link in the referred signatures
if(iter->first > 0)
_memory->updateLink(iter->second, false);
if(!_localizationSmoothing)
{
// Add original link without optimization
UDEBUG("Adding new odom cache constraint %d->%d (%s)",
iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str());
}
else
{
// Adjust with optimized poses, this will smooth the localization
Transform newT = newOptPoseInv * optPoses.at(iter->first);
UDEBUG("Adjusted localization link %d->%d after optimization", iter->second.from(), iter->second.to());
UDEBUG("from %s", iter->second.transform().prettyPrint().c_str());
UDEBUG(" to %s", newT.prettyPrint().c_str());
iter->second.setTransform(newT);
// Update link in the referred signatures
if(iter->first > 0)
_memory->updateLink(iter->second, false);
}
_odomCacheConstraints.insert(std::make_pair(signature->id(), iter->second));
}