i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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;
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);
float angularError = uMax3(
fabs(opt_roll - link_roll),
fabs(opt__pitch - link_pitch),
fabs(opt__yaw - link_yaw));
fabs(opt_pitch - link_pitch),
fabs(opt_yaw - link_yaw));
angularError = angularError>M_PI?2*M_PI-angularError:angularError;
UASSERT(iter->second.rotVariance()>0.0);
float stddevAngular = sqrt(iter->second.rotVariance());
float angularErrorRatio = angularError/stddevAngular;