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

Now Rot3 uses QuaternionChart Expmap/Logmap in quaternion mode

release/4.3a0
dellaert 2014-12-08 12:37:00 +01:00
parent 8cbab779b3
commit 9dc3d28bf2
2 changed files with 7 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/geometry/Rot3.h
View File

@ -23,6 +23,10 @@
#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro #include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
#ifdef GTSAM_USE_QUATERNIONS
#include <gtsam/geometry/Quaternion.h>
#endif
// You can override the default coordinate mode using this flag // You can override the default coordinate mode using this flag
#ifndef ROT3_DEFAULT_COORDINATES_MODE #ifndef ROT3_DEFAULT_COORDINATES_MODE
#ifdef GTSAM_USE_QUATERNIONS #ifdef GTSAM_USE_QUATERNIONS
@ -46,11 +50,6 @@
namespace gtsam { namespace gtsam {
/// Typedef to an Eigen Quaternion<double>, we disable alignment because
/// geometry objects are stored in boost pool allocators, in Values
/// containers, and and these pool allocators do not support alignment.
typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;
/** /**
* @brief A 3D rotation represented as a rotation matrix if the preprocessor * @brief A 3D rotation represented as a rotation matrix if the preprocessor
* symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it * symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it

28
gtsam/geometry/Rot3Q.cpp
View File

@ -85,7 +85,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w, double theta) { Rot3 Rot3::rodriguez(const Vector& w, double theta) {
return Quaternion(Eigen::AngleAxisd(theta, w)); } return QuaternionChart::Expmap(theta,w);
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2, Rot3 Rot3::compose(const Rot3& R2,
@ -134,30 +135,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 Rot3::Logmap(const Rot3& R) { Vector3 Rot3::Logmap(const Rot3& R) {
using std::acos; return QuaternionChart::Logmap(R.quaternion_);
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 double qw = q.w();
if (qw > NearlyOne) {
// Taylor expansion of (angle / s) at 1
return (2 - 2 * (qw - 1) / 3) * q.vec();
} else if (qw < NearlyNegativeOne) {
// Angle is zero, return zero vector
return Vector3::Zero();
} else {
// Normal, away from zero case
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
// Important: convert to [-pi,pi] to keep error continuous
if (angle > M_PI)
angle -= twoPi;
else if (angle < -M_PI)
angle += twoPi;
return (angle / s) * q.vec();
}
} }
/* ************************************************************************* */ /* ************************************************************************* */