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

Optimized a bit more

release/4.3a0
dellaert 2014-10-23 09:51:43 +02:00
parent a0be48ef75
commit 056254bf93
1 changed files with 14 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam/geometry/Rot3Q.cpp
View File

@ -121,24 +121,29 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Vector3 Rot3::Logmap(const Rot3& R) { Vector3 Rot3::Logmap(const Rot3& R) {
using std::acos;
using std::sqrt;
static const double twoPi = 2.0 * M_PI,
// define these compile time constants to avoid std::abs:
NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10;
const Quaternion& q = R.quaternion_; const Quaternion& q = R.quaternion_;
double qw = q.w(); const double qw = q.w();
if (std::abs(qw - 1.0) < 1e-10) { if (qw > NearlyOne) {
// Taylor expansion of (angle / s) at 1 // Taylor expansion of (angle / s) at 1
return (2 - 2 * (qw - 1) / 3) * q.vec(); return (2 - 2 * (qw - 1) / 3) * q.vec();
} else if (std::abs(qw + 1.0) < 1e-10) { } else if (qw < NearlyNegativeOne) {
// Angle is zero, return zero vector // Angle is zero, return zero vector
return Vector3::Zero(); return Vector3::Zero();
} else { } else {
// Normal, away from zero case // Normal, away from zero case
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw); double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
// TODO C++ says: acos range is [0..pi], so below cannot occur !? // Important: convert to [-pi,pi] to keep error continuous
if (angle > M_PI) // Important: use the smallest possible if (angle > M_PI)
angle -= 2.0 * M_PI; // angle, e.g. no more than PI, to keep angle -= twoPi;
if (angle < -M_PI) // error continuous. else if (angle < -M_PI)
angle += 2.0 * M_PI; angle += twoPi;
return (angle / s) * q.vec(); return (angle / s) * q.vec();
} }
} }