i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 09:07: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
+109 -83
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,103 +3039,112 @@ bool Rtabmap::process(
} }
_odomCacheConstraints.insert(selfLinks.begin(), selfLinks.end()); _odomCacheConstraints.insert(selfLinks.begin(), selfLinks.end());
// If there are no signatures retrieved, we don't // At least 2 localizations at 2 different time required
// need to re-optimize the graph. Just update the last if(hadAlreadyLocalizationLinks || _maxOdomCacheSize == 0)
// position if OptimizeFromGraphEnd=false or transform the
// whole graph if OptimizeFromGraphEnd=true
UINFO("Localization without map optimization");
if(_optimizeFromGraphEnd)
{ {
// update all previous nodes // If there are no signatures retrieved, we don't
// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd // need to re-optimize the graph. Just update the last
// parameters just changed state, we should put back all poses without map correction. // position if OptimizeFromGraphEnd=false or transform the
Transform oldPose = _optimizedPoses.at(localizationLinks.rbegin()->first); // whole graph if OptimizeFromGraphEnd=true
Transform mapCorrectionInv = _mapCorrection.inverse(); UINFO("Localization without map optimization");
Transform u = signature->getPose() * localizationLinks.rbegin()->second.transform(); if(_optimizeFromGraphEnd)
if(_graphOptimizer->isSlam2d())
{ {
// in case of 3d landmarks, transform constraint to 2D // update all previous nodes
u = u.to3DoF(); // Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
} // parameters just changed state, we should put back all poses without map correction.
else if(_graphOptimizer->gravitySigma() > 0) Transform oldPose = _optimizedPoses.at(localizationLinks.rbegin()->first);
{ Transform mapCorrectionInv = _mapCorrection.inverse();
// Adjust transform with gravity Transform u = signature->getPose() * localizationLinks.rbegin()->second.transform();
Transform transform = localizationLinks.rbegin()->second.transform(); if(_graphOptimizer->isSlam2d())
int loopId = localizationLinks.rbegin()->first;
if(loopId < 0)
{ {
//For landmarks, use transform against other node looking the landmark // in case of 3d landmarks, transform constraint to 2D
// (because we don't assume that landmarks are aligned with gravity) u = u.to3DoF();
int landmarkId = loopId;
UASSERT(landmarksDetected.find(landmarkId) != landmarksDetected.end() &&
!landmarksDetected.at(landmarkId).empty());
loopId = *landmarksDetected.at(landmarkId).begin();
const Signature * loopS = _memory->getSignature(loopId);
transform = transform * _optimizedPoses.at(landmarkId).inverse()*_optimizedPoses.at(loopS->id());
UASSERT(_optimizedPoses.find(loopId) != _optimizedPoses.end());
oldPose = _optimizedPoses.at(loopId);
} }
else if(_graphOptimizer->gravitySigma() > 0)
{
// Adjust transform with gravity
Transform transform = localizationLinks.rbegin()->second.transform();
int loopId = localizationLinks.rbegin()->first;
if(loopId < 0)
{
//For landmarks, use transform against other node looking the landmark
// (because we don't assume that landmarks are aligned with gravity)
int landmarkId = loopId;
UASSERT(landmarksDetected.find(landmarkId) != landmarksDetected.end() &&
!landmarksDetected.at(landmarkId).empty());
loopId = *landmarksDetected.at(landmarkId).begin();
const Signature * loopS = _memory->getSignature(loopId);
transform = transform * _optimizedPoses.at(landmarkId).inverse()*_optimizedPoses.at(loopS->id());
UASSERT(_optimizedPoses.find(loopId) != _optimizedPoses.end());
oldPose = _optimizedPoses.at(loopId);
}
const Signature * loopS = _memory->getSignature(loopId); const Signature * loopS = _memory->getSignature(loopId);
UASSERT(loopS !=0); UASSERT(loopS !=0);
std::multimap<int, Link>::const_iterator iterGravityLoop = graph::findLink(loopS->getLinks(), loopS->id(), loopS->id(), false, Link::kGravity); std::multimap<int, Link>::const_iterator iterGravityLoop = graph::findLink(loopS->getLinks(), loopS->id(), loopS->id(), false, Link::kGravity);
std::multimap<int, Link>::const_iterator iterGravitySign = graph::findLink(signature->getLinks(), signature->id(), signature->id(), false, Link::kGravity); std::multimap<int, Link>::const_iterator iterGravitySign = graph::findLink(signature->getLinks(), signature->id(), signature->id(), false, Link::kGravity);
if(iterGravityLoop!=loopS->getLinks().end() && if(iterGravityLoop!=loopS->getLinks().end() &&
iterGravitySign!=signature->getLinks().end()) iterGravitySign!=signature->getLinks().end())
{ {
float roll,pitch,yaw; float roll,pitch,yaw;
iterGravityLoop->second.transform().getEulerAngles(roll, pitch, yaw); iterGravityLoop->second.transform().getEulerAngles(roll, pitch, yaw);
Transform targetRotation = iterGravitySign->second.transform().rotation()*transform.rotation(); Transform targetRotation = iterGravitySign->second.transform().rotation()*transform.rotation();
targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta()); targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta());
Transform error = transform.rotation().inverse() * iterGravitySign->second.transform().rotation().inverse() * targetRotation; Transform error = transform.rotation().inverse() * iterGravitySign->second.transform().rotation().inverse() * targetRotation;
transform *= error; transform *= error;
u = signature->getPose() * transform; u = signature->getPose() * transform;
}
else if(iterGravityLoop!=loopS->getLinks().end() ||
iterGravitySign!=signature->getLinks().end())
{
UWARN("Gravity link not found for %d or %d, localization won't be corrected with gravity.", loopId, signature->id());
}
} }
else if(iterGravityLoop!=loopS->getLinks().end() || Transform up = u * oldPose.inverse();
iterGravitySign!=signature->getLinks().end()) for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{ {
UWARN("Gravity link not found for %d or %d, localization won't be corrected with gravity.", loopId, signature->id()); iter->second = mapCorrectionInv * up * iter->second;
} }
_optimizedPoses.at(signature->id()) = signature->getPose();
} }
Transform up = u * oldPose.inverse(); else
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{ {
iter->second = mapCorrectionInv * up * iter->second; Transform newPose = _optimizedPoses.at(localizationLinks.rbegin()->first) * localizationLinks.rbegin()->second.transform().inverse();
UDEBUG("newPose=%s", newPose.prettyPrint().c_str());
if(_graphOptimizer->isSlam2d())
{
// in case of 3d landmarks, transform constraint to 2D
newPose = newPose.to3DoF();
UDEBUG("newPose 2D=%s", newPose.prettyPrint().c_str());
}
else if(_graphOptimizer->gravitySigma() > 0)
{
// Adjust transform with gravity
std::multimap<int, Link>::const_iterator iterGravitySign = graph::findLink(signature->getLinks(), signature->id(), signature->id(), false, Link::kGravity);
if(iterGravitySign!=signature->getLinks().end())
{
float roll,pitch,yaw;
float tmp1,tmp2;
UDEBUG("Gravity link = %s", iterGravitySign->second.transform().prettyPrint().c_str());
iterGravitySign->second.transform().getEulerAngles(roll, pitch, tmp1);
newPose.getEulerAngles(tmp1, tmp2, yaw);
newPose = Transform(newPose.x(), newPose.y(), newPose.z(), roll, pitch, yaw);
UDEBUG("newPose gravity=%s", newPose.prettyPrint().c_str());
}
else if(iterGravitySign!=signature->getLinks().end())
{
UWARN("Gravity link not found for %d, localization won't be corrected with gravity.", signature->id());
}
}
_optimizedPoses.at(signature->id()) = newPose;
} }
_optimizedPoses.at(signature->id()) = signature->getPose(); localizationCovariance = localizationLinks.rbegin()->second.infMatrix().inv();
} }
else else //delayed localization (wait for more than 1 link)
{ {
Transform newPose = _optimizedPoses.at(localizationLinks.rbegin()->first) * localizationLinks.rbegin()->second.transform().inverse(); UWARN("Localization was good, but waiting for another one to be more accurate (%s>0)", Parameters::kRGBDMaxOdomCacheSize().c_str());
UDEBUG("newPose=%s", newPose.prettyPrint().c_str()); rejectLocalization = true;
if(_graphOptimizer->isSlam2d())
{
// in case of 3d landmarks, transform constraint to 2D
newPose = newPose.to3DoF();
UDEBUG("newPose 2D=%s", newPose.prettyPrint().c_str());
}
else if(_graphOptimizer->gravitySigma() > 0)
{
// Adjust transform with gravity
std::multimap<int, Link>::const_iterator iterGravitySign = graph::findLink(signature->getLinks(), signature->id(), signature->id(), false, Link::kGravity);
if(iterGravitySign!=signature->getLinks().end())
{
float roll,pitch,yaw;
float tmp1,tmp2;
UDEBUG("Gravity link = %s", iterGravitySign->second.transform().prettyPrint().c_str());
iterGravitySign->second.transform().getEulerAngles(roll, pitch, tmp1);
newPose.getEulerAngles(tmp1, tmp2, yaw);
newPose = Transform(newPose.x(), newPose.y(), newPose.z(), roll, pitch, yaw);
UDEBUG("newPose gravity=%s", newPose.prettyPrint().c_str());
}
else if(iterGravitySign!=signature->getLinks().end())
{
UWARN("Gravity link not found for %d, localization won't be corrected with gravity.", signature->id());
}
}
_optimizedPoses.at(signature->id()) = newPose;
} }
localizationCovariance = localizationLinks.rbegin()->second.infMatrix().inv();
} }
} }