Now Rot3 uses QuaternionChart Expmap/Logmap in quaternion mode
parent
8cbab779b3
commit
9dc3d28bf2
|
@ -23,6 +23,10 @@
|
|||
|
||||
#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
|
||||
#ifndef ROT3_DEFAULT_COORDINATES_MODE
|
||||
#ifdef GTSAM_USE_QUATERNIONS
|
||||
|
@ -46,11 +50,6 @@
|
|||
|
||||
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
|
||||
* symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it
|
||||
|
|
|
@ -85,7 +85,8 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
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,
|
||||
|
@ -134,30 +135,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
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 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();
|
||||
}
|
||||
return QuaternionChart::Logmap(R.quaternion_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
Loading…
Reference in New Issue