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

Fix quaternion on M1

release/4.3a0
Fan Jiang 2021-12-03 15:14:19 -05:00
parent bb96a2cf12
commit 27579e4f34
1 changed files with 20 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
gtsam/geometry/Quaternion.h
View File

@ -82,7 +82,7 @@ struct traits<QUATERNION_TYPE> {
using std::sin; using std::sin;
if (H) *H = SO3::ExpmapDerivative(omega.template cast<double>()); if (H) *H = SO3::ExpmapDerivative(omega.template cast<double>());
_Scalar theta2 = omega.dot(omega); _Scalar theta2 = omega.dot(omega);
if (theta2 > std::numeric_limits<_Scalar>::epsilon()) { if (theta2 > 1e-8) {
_Scalar theta = std::sqrt(theta2); _Scalar theta = std::sqrt(theta2);
_Scalar ha = _Scalar(0.5) * theta; _Scalar ha = _Scalar(0.5) * theta;
Vector3 vec = (sin(ha) / theta) * omega; Vector3 vec = (sin(ha) / theta) * omega;
@ -100,8 +100,8 @@ struct traits<QUATERNION_TYPE> {
using std::sqrt; using std::sqrt;
// define these compile time constants to avoid std::abs: // define these compile time constants to avoid std::abs:
static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10, static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-8,
NearlyNegativeOne = -1.0 + 1e-10; NearlyNegativeOne = -1.0 + 1e-8;
TangentVector omega; TangentVector omega;
@ -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>());