i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed angular error check when abs angle error > PI

Browse Source
This commit is contained in:
matlabbe
2018-12-10 20:20:52 -05:00
parent 73378c4d56
commit ccbc802fe6
1 changed files with 5 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
corelib/src/Graph.cpp
View File

@@ -868,14 +868,15 @@ void computeMaxGraphErrors(
} }
} }
float opt_roll,opt__pitch,opt__yaw; float opt_roll,opt_pitch,opt_yaw;
float link_roll,link_pitch,link_yaw; float link_roll,link_pitch,link_yaw;
t.getEulerAngles(opt_roll, opt__pitch, opt__yaw); t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw); iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
float angularError = uMax3( float angularError = uMax3(
fabs(opt_roll - link_roll), fabs(opt_roll - link_roll),
fabs(opt__pitch - link_pitch), fabs(opt_pitch - link_pitch),
fabs(opt__yaw - link_yaw)); fabs(opt_yaw - link_yaw));
angularError = angularError>M_PI?2*M_PI-angularError:angularError;
UASSERT(iter->second.rotVariance()>0.0); UASSERT(iter->second.rotVariance()>0.0);
float stddevAngular = sqrt(iter->second.rotVariance()); float stddevAngular = sqrt(iter->second.rotVariance());
float angularErrorRatio = angularError/stddevAngular; float angularErrorRatio = angularError/stddevAngular;