12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #949 from borglab/fix/quat_logmap 

Fix Quaternion on M1 (and possibly other platforms)
release/4.3a0
Fan Jiang 2021-12-03 22:35:18 -05:00 committed by GitHub
parent bb96a2cf12 578b5e6ec5
commit cbf4f9f3fd
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 17 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/geometry/Quaternion.h
View File

@ -117,13 +117,23 @@ struct traits<QUATERNION_TYPE> {
omega = (-8. / 3. - 2. / 3. * qw) * q.vec(); omega = (-8. / 3. - 2. / 3. * qw) * q.vec();
} else { } else {
// Normal, away from zero case // Normal, away from zero case
_Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw); if (qw > 0) {
// Important: convert to [-pi,pi] to keep error continuous _Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
if (angle > M_PI) // Important: convert to [-pi,pi] to keep error continuous
angle -= twoPi; if (angle > M_PI)
else if (angle < -M_PI) angle -= twoPi;
angle += twoPi; else if (angle < -M_PI)
omega = (angle / s) * q.vec(); angle += twoPi;
omega = (angle / s) * q.vec();
} else {
// Make sure that we are using a canonical quaternion with w > 0
_Scalar angle = 2 * acos(-qw), s = sqrt(1 - qw * qw);
if (angle > M_PI)
angle -= twoPi;
else if (angle < -M_PI)
angle += twoPi;
omega = (angle / s) * -q.vec();
}
} }
if(H) *H = SO3::LogmapDerivative(omega.template cast<double>()); if(H) *H = SO3::LogmapDerivative(omega.template cast<double>());