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

CameraRealSense2: fixed wrong IR image index. DBReader: fixed imu not published. OdometryF2M: use IMU roll/pitch values when IMU is used (reducing variance on those angles before BA).

Browse Source
This commit is contained in:
matlabbe
2019-09-13 21:45:13 -04:00
parent bdc8d839ff
commit 350d3cd85f
13 changed files with 182 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Graph.cpp
+4 -4
View File
@@ -898,8 +898,8 @@ void computeMaxGraphErrors(
fabs(iter->second.transform().x() - t.x()),
fabs(iter->second.transform().y() - t.y()),
fabs(iter->second.transform().z() - t.z()));
UASSERT(iter->second.transVariance()>0.0);
float stddevLinear = sqrt(iter->second.transVariance());
UASSERT(iter->second.transVariance(false)>0.0);
float stddevLinear = sqrt(iter->second.transVariance(false));
float linearErrorRatio = linearError/stddevLinear;
if(linearErrorRatio > maxLinearErrorRatio)
{
@@ -920,8 +920,8 @@ void computeMaxGraphErrors(
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());
UASSERT(iter->second.rotVariance(false)>0.0);
float stddevAngular = sqrt(iter->second.rotVariance(false));
float angularErrorRatio = angularError/stddevAngular;
if(angularErrorRatio > maxAngularErrorRatio)
{