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

Fixed sign bug

release/4.3a0
dellaert 2015-02-11 15:00:50 +01:00
parent 9cf6bb3f0f
commit 559da158fe
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/geometry/Quaternion.h
View File

@ -96,14 +96,15 @@ struct traits<QUATERNION_TYPE> {
Vector3 omega;
const double qw = q.w();
// See Quaternion-Logmap.nb in doc for Taylor expansions
if (qw > NearlyOne) {
// Taylor expansion of (angle / s) at 1
//return (2 + 2 * (1-qw) / 3) * q.vec();
// (2 + 2 * (1-qw) / 3) * q.vec();
omega = ( 8. / 3. - 2. / 3. * qw) * q.vec();
} else if (qw < NearlyNegativeOne) {
// Taylor expansion of (angle / s) at -1
//return (-2 - 2 * (1 + qw) / 3) * q.vec();
omega = (-8. / 3 + 2. / 3 * qw) * q.vec();
// (-2 - 2 * (1 + qw) / 3) * q.vec();
omega = (-8. / 3. - 2. / 3. * qw) * q.vec();
} else {
// Normal, away from zero case
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);