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

Fixed graph deformation check without landmark's orientation optimized. Added parameter Marker/VarianceOrientationIgnored to be able to tune correctly GTSAM's bearing/range factor variance.

Browse Source
This commit is contained in:
matlabbe
2025-03-22 20:43:27 -07:00
parent 5f946cc26b
commit 68e3125358
10 changed files with 263 additions and 187 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Graph.cpp
+17 -5
View File
@@ -958,12 +958,24 @@ void computeMaxGraphErrors(
return;
}
Transform t = t1.inverse()*t2;
Transform t;
Transform linkT;
if(iter->second.from() < 0)
{
// For landmarks, compare from node to landmark, in case we optimized only marker's position
t = t2.inverse()*t1;
linkT = iter->second.transform().inverse();
}
else
{
t = t1.inverse()*t2;
linkT = iter->second.transform();
}
float linearError = uMax3(
fabs(iter->second.transform().x() - t.x()),
fabs(iter->second.transform().y() - t.y()),
force3DoF?0:fabs(iter->second.transform().z() - t.z()));
fabs(linkT.x() - t.x()),
fabs(linkT.y() - t.y()),
force3DoF?0:fabs(linkT.z() - t.z()));
UASSERT(iter->second.transVariance(false)>0.0);
float stddevLinear = sqrt(iter->second.transVariance(false));
float linearErrorRatio = linearError/stddevLinear;
@@ -984,7 +996,7 @@ 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);
linkT.getEulerAngles(link_roll, link_pitch, link_yaw);
float angularError = uMax3(
force3DoF?0:fabs(opt_roll - link_roll),
force3DoF?0:fabs(opt_pitch - link_pitch),