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/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

47
corelib/src/Graph.cpp
View File

@@ -902,7 +902,7 @@ void computeMaxGraphErrors(
float & maxAngularError,
const Link ** maxLinearErrorLink,
const Link ** maxAngularErrorLink,
bool for3DoF)
bool force3DoF)
{
maxLinearErrorRatio = -1;
maxAngularErrorRatio = -1;
@@ -912,8 +912,8 @@ void computeMaxGraphErrors(
UDEBUG("poses=%d links=%d", (int)poses.size(), (int)links.size());
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
// ignore links with high variance, priors and landmarks
if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to() && iter->second.type() != Link::kLandmark)
// ignore priors
if(iter->second.from() != iter->second.to())
{
Transform t1 = uValue(poses, iter->second.from(), Transform());
Transform t2 = uValue(poses, iter->second.to(), Transform());
@@ -922,7 +922,7 @@ void computeMaxGraphErrors(
float linearError = uMax3(
fabs(iter->second.transform().x() - t.x()),
fabs(iter->second.transform().y() - t.y()),
for3DoF?0:fabs(iter->second.transform().z() - t.z()));
force3DoF?0:fabs(iter->second.transform().z() - t.z()));
UASSERT(iter->second.transVariance(false)>0.0);
float stddevLinear = sqrt(iter->second.transVariance(false));
float linearErrorRatio = linearError/stddevLinear;
@@ -936,25 +936,30 @@ void computeMaxGraphErrors(
}
}
float opt_roll,opt_pitch,opt_yaw;
float link_roll,link_pitch,link_yaw;
t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
float angularError = uMax3(
for3DoF?0:fabs(opt_roll - link_roll),
for3DoF?0:fabs(opt_pitch - link_pitch),
fabs(opt_yaw - link_yaw));
angularError = angularError>M_PI?2*M_PI-angularError:angularError;
UASSERT(iter->second.rotVariance(false)>0.0);
float stddevAngular = sqrt(iter->second.rotVariance(false));
float angularErrorRatio = angularError/stddevAngular;
if(angularErrorRatio > maxAngularErrorRatio)
// For landmark links, don't compute angular error if it doesn't estimate orientation
if(iter->second.type() != Link::kLandmark ||
1.0 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999.0)
{
maxAngularError = angularError;
maxAngularErrorRatio = angularErrorRatio;
if(maxAngularErrorLink)
float opt_roll,opt_pitch,opt_yaw;
float link_roll,link_pitch,link_yaw;
t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
float angularError = uMax3(
force3DoF?0:fabs(opt_roll - link_roll),
force3DoF?0:fabs(opt_pitch - link_pitch),
fabs(opt_yaw - link_yaw));
angularError = angularError>M_PI?2*M_PI-angularError:angularError;
UASSERT(iter->second.rotVariance(false)>0.0);
float stddevAngular = sqrt(iter->second.rotVariance(false));
float angularErrorRatio = angularError/stddevAngular;
if(angularErrorRatio > maxAngularErrorRatio)
{
*maxAngularErrorLink = &iter->second;
maxAngularError = angularError;
maxAngularErrorRatio = angularErrorRatio;
if(maxAngularErrorLink)
{
*maxAngularErrorLink = &iter->second;
}
}
}
}

4
corelib/src/Memory.cpp
View File

@@ -283,6 +283,10 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
-landmarkId, inserted.first->second, landmarkSize.at<float>(0,0));
}
}
else
{
UDEBUG("Caching landmark size %f for %d", landmarkSize.at<float>(0,0), -landmarkId);
}
}
std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(landmarkId);

42
corelib/src/Rtabmap.cpp
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));
}