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

16
gtsam/geometry/Quaternion.h
View File

@ -82,7 +82,7 @@ struct traits<QUATERNION_TYPE> {
using std::sin;
if (H) *H = SO3::ExpmapDerivative(omega.template cast<double>());
_Scalar theta2 = omega.dot(omega);
if (theta2 > std::numeric_limits<_Scalar>::epsilon()) {
if (theta2 > 1e-8) {
_Scalar theta = std::sqrt(theta2);
_Scalar ha = _Scalar(0.5) * theta;
Vector3 vec = (sin(ha) / theta) * omega;
@ -100,8 +100,8 @@ struct traits<QUATERNION_TYPE> {
using std::sqrt;
// define these compile time constants to avoid std::abs:
static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10,
NearlyNegativeOne = -1.0 + 1e-10;
static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-8,
NearlyNegativeOne = -1.0 + 1e-8;
TangentVector omega;
@ -117,6 +117,7 @@ struct traits<QUATERNION_TYPE> {
omega = (-8. / 3. - 2. / 3. * qw) * q.vec();
} else {
// Normal, away from zero case
if (qw > 0) {
_Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
// Important: convert to [-pi,pi] to keep error continuous
if (angle > M_PI)
@ -124,6 +125,15 @@ struct traits<QUATERNION_TYPE> {
else if (angle < -M_PI)
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>());