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

Tuning localization priors (added RGBD/LocalizationPriorError parameter) (#1156)

Browse Source 
* tuning localization priors (for https://github.com/introlab/rtabmap_ros/issues/1057)

* Set prior error value to same than old default value
This commit is contained in:
matlabbe
2023-11-05 15:14:11 -08:00
committed by GitHub
parent 45a3b59a23
commit 56552afa6c
5 changed files with 358 additions and 294 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
corelib/src/Rtabmap.cpp
View File

@@ -148,6 +148,7 @@ Rtabmap::Rtabmap() :
_loopGPS(Parameters::defaultRtabmapLoopGPS()),
_maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()),
_localizationSmoothing(Parameters::defaultRGBDLocalizationSmoothing()),
_localizationPriorInf(1.0/(Parameters::defaultRGBDLocalizationPriorError()*Parameters::defaultRGBDLocalizationPriorError())),
_createGlobalScanMap(Parameters::defaultRGBDProximityGlobalScanMap()),
_markerPriorsLinearVariance(Parameters::defaultMarkerPriorsVarianceLinear()),
_markerPriorsAngularVariance(Parameters::defaultMarkerPriorsVarianceAngular()),
@@ -620,6 +621,10 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS);
Parameters::parse(parameters, Parameters::kRGBDMaxOdomCacheSize(), _maxOdomCacheSize);
Parameters::parse(parameters, Parameters::kRGBDLocalizationSmoothing(), _localizationSmoothing);
double localizationPriorError = Parameters::defaultRGBDLocalizationPriorError();
Parameters::parse(parameters, Parameters::kRGBDLocalizationPriorError(), localizationPriorError);
UASSERT(localizationPriorError>0.0);
_localizationPriorInf = 1.0/(localizationPriorError*localizationPriorError);
Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap);
Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), _markerPriorsLinearVariance);
@@ -3137,15 +3142,19 @@ bool Rtabmap::process(
{
constraints.insert(std::make_pair(iter->second.from(), iter->second));
}
cv::Mat priorInfMat = cv::Mat::eye(6,6, CV_64FC1)*_localizationPriorInf;
for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
{
std::map<int, Transform>::iterator iterPose = _optimizedPoses.find(iter->second.to());
if(iterPose != _optimizedPoses.end() && poses.find(iterPose->first) == poses.end())
{
poses.insert(*iterPose);
// make the poses in the map fixed
constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, cv::Mat::eye(6,6, CV_64FC1)*1000000)));
UDEBUG("Constraint %d->%d (type=%s)", iterPose->first, iterPose->first, Link::typeName(Link::kPosePrior).c_str());
if(iterPose->first > 0)
{
// make the poses in the map fixed
constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat)));
UDEBUG("Constraint %d->%d (type=%s)", iterPose->first, iterPose->first, Link::typeName(Link::kPosePrior).c_str());
}
}
UDEBUG("Constraint %d->%d (type=%s, var = %f %f)", iter->second.from(), iter->second.to(), iter->second.typeName().c_str(), iter->second.transVariance(), iter->second.rotVariance());
}
@@ -3163,7 +3172,17 @@ bool Rtabmap::process(
// If slam2d: get connected graph while keeping original roll,pitch,z values.
_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut, !_graphOptimizer->isSlam2d());
cv::Mat locOptCovariance;
std::map<int, Transform> optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
std::map<int, Transform> optPoses;
if(poses.lower_bound(1) != poses.end() &&
_odomCachePoses.lower_bound(1) != poses.end() &&
poses.lower_bound(1)->first < _odomCachePoses.lower_bound(1)->first)
{
optPoses = _graphOptimizer->optimize(poses.lower_bound(1)->first, posesOut, edgeConstraintsOut, locOptCovariance);
}
else
{
UERROR("Invalid localization constraints");
}
_graphOptimizer->setPriorsIgnored(priorsIgnored); // set back
for(std::map<int, Transform>::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter)
{
@@ -3303,7 +3322,17 @@ bool Rtabmap::process(
_graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
// If slam2d: get connected graph while keeping original roll,pitch,z values.
_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut, !_graphOptimizer->isSlam2d());
optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
optPoses.clear();
if(poses.lower_bound(1) != poses.end() &&
_odomCachePoses.lower_bound(1) != poses.end() &&
poses.lower_bound(1)->first < _odomCachePoses.lower_bound(1)->first)
{
optPoses = _graphOptimizer->optimize(poses.lower_bound(1)->first, posesOut, edgeConstraintsOut, locOptCovariance);
}
else
{
UERROR("Invalid localization constraints");
}
_graphOptimizer->setPriorsIgnored(priorsIgnored); // set back
for(std::map<int, Transform>::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter)
{