i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-05 17:47:49 +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);
// Update landmark index