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

Localization 2d Slam: automatically rotate landmark links to have z-axis up for correct 3DoF optimization. When RGBD/MaxOdomCacheSize is used, wait for at least 2 temporal localizations before adjusting the pose (to avoid big jumps when only one constraint is used).

Browse Source
This commit is contained in:
matlabbe
2021-12-17 11:00:37 -05:00
parent 6eb81cbb72
commit bca8b30832
2 changed files with 127 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Memory.cpp
+18 -1
View File
@@ -5787,7 +5787,24 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
} }
} }
Link landmark(s->id(), landmarkId, Link::kLandmark, iter->second.pose(), iter->second.covariance().inv(), landmarkSize);
Transform landmarkPose = iter->second.pose();
if(_registrationPipeline->force3DoF())
{
// For 2D slam, make sure the landmark z axis is up
rtabmap::Transform tx = landmarkPose.rotation() * rtabmap::Transform(1,0,0,0,0,0);
rtabmap::Transform ty = landmarkPose.rotation() * rtabmap::Transform(0,1,0,0,0,0);
if(fabs(tx.z()) > 0.9)
{
landmarkPose*=rtabmap::Transform(0,0,0,0,(tx.z()>0?1:-1)*M_PI/2,0);
}
else if(fabs(ty.z()) > 0.9)
{
landmarkPose*=rtabmap::Transform(0,0,0,(ty.z()>0?-1:1)*M_PI/2,0,0);
}
}
Link landmark(s->id(), landmarkId, Link::kLandmark, landmarkPose, iter->second.covariance().inv(), landmarkSize);
s->addLandmark(landmark); s->addLandmark(landmark);
// Update landmark index // Update landmark index
corelib/src/Rtabmap.cpp
+26
View File
@@ -3008,6 +3008,23 @@ bool Rtabmap::process(
if(!rejectLocalization) if(!rejectLocalization)
{ {
// Count how many localization links are in the constraints
bool hadAlreadyLocalizationLinks = false;
for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin();
iter!=_odomCacheConstraints.end(); ++iter)
{
if(iter->second.type() == Link::kGlobalClosure ||
iter->second.type() == Link::kLocalSpaceClosure ||
iter->second.type() == Link::kLocalTimeClosure ||
iter->second.type() == Link::kUserClosure ||
iter->second.type() == Link::kNeighborMerged ||
iter->second.type() == Link::kLandmark)
{
hadAlreadyLocalizationLinks = true;
break;
}
}
// update localization links // update localization links
Transform newOptPoseInv = optPoses.at(signature->id()).inverse(); Transform newOptPoseInv = optPoses.at(signature->id()).inverse();
for(std::multimap<int, Link>::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter) for(std::multimap<int, Link>::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter)
@@ -3022,6 +3039,9 @@ bool Rtabmap::process(
} }
_odomCacheConstraints.insert(selfLinks.begin(), selfLinks.end()); _odomCacheConstraints.insert(selfLinks.begin(), selfLinks.end());
// At least 2 localizations at 2 different time required
if(hadAlreadyLocalizationLinks || _maxOdomCacheSize == 0)
{
// If there are no signatures retrieved, we don't // If there are no signatures retrieved, we don't
// need to re-optimize the graph. Just update the last // need to re-optimize the graph. Just update the last
// position if OptimizeFromGraphEnd=false or transform the // position if OptimizeFromGraphEnd=false or transform the
@@ -3120,6 +3140,12 @@ bool Rtabmap::process(
} }
localizationCovariance = localizationLinks.rbegin()->second.infMatrix().inv(); localizationCovariance = localizationLinks.rbegin()->second.infMatrix().inv();
} }
else //delayed localization (wait for more than 1 link)
{
UWARN("Localization was good, but waiting for another one to be more accurate (%s>0)", Parameters::kRGBDMaxOdomCacheSize().c_str());
rejectLocalization = true;
}
}
} }
if(rejectLocalization) if(rejectLocalization)