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

Slight refactor

release/4.3a0
dellaert 2014-12-08 12:35:14 +01:00
parent ccd843040e
commit 8cbab779b3
1 changed files with 21 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
gtsam/geometry/Quaternion.h
View File

@ -20,26 +20,33 @@
namespace gtsam { namespace gtsam {
/// Chart for Eigen Quaternions /// Chart for Eigen Quaternions
template<typename S, int O> template<typename _Scalar, int _Options>
struct QuaternionChart { struct MakeQuaternionChart {
// required // required
typedef Eigen::Quaternion<S, O> ManifoldType; typedef Eigen::Quaternion<_Scalar, _Options> ManifoldType;
// internal // internal
typedef ManifoldType Q; typedef ManifoldType Q;
typedef typename manifold::traits::TangentVector<Q>::type Omega; typedef typename manifold::traits::TangentVector<Q>::type Omega;
/// Exponential map, simply be converting omega to AngleAxis /// Exponential map given axis/angle representation of Lie algebra
static Q Expmap(const Omega& omega) { static Q Expmap(const _Scalar& angle, const Eigen::Ref<const Omega>& axis) {
double theta = omega.norm(); return Q(Eigen::AngleAxisd(angle, axis));
if (std::abs(theta) < 1e-10) }
/// Exponential map, simply be converting omega to axis/angle representation
static Q Expmap(const Eigen::Ref<const Omega>& omega) {
if (omega.isZero())
return Q::Identity(); return Q::Identity();
return Q(Eigen::AngleAxisd(theta, omega / theta)); else {
_Scalar angle = omega.norm();
return Q(Eigen::AngleAxisd(angle, omega / angle));
}
} }
/// retract, simply be converting omega to AngleAxis /// retract, simply be converting omega to AngleAxis
static Q Retract(const Q& p, const Omega& omega) { static Q Retract(const Q& p, const Eigen::Ref<const Omega>& omega) {
return p * Expmap(omega); return p * Expmap(omega);
} }
@ -57,7 +64,7 @@ struct QuaternionChart {
return (2 - 2 * (qw - 1) / 3) * q.vec(); return (2 - 2 * (qw - 1) / 3) * q.vec();
} else if (qw < NearlyNegativeOne) { } else if (qw < NearlyNegativeOne) {
// Angle is zero, return zero vector // Angle is zero, return zero vector
return Vector3::Zero(); return Omega::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);
@ -81,8 +88,9 @@ struct QuaternionChart {
GTSAM_MULTIPLICATIVE_GROUP(QUATERNION_TEMPLATE, QUATERNION_TYPE) GTSAM_MULTIPLICATIVE_GROUP(QUATERNION_TEMPLATE, QUATERNION_TYPE)
#define QUATERNION_TANGENT Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> #define QUATERNION_TANGENT Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1>
#define QUATERNION_CHART QuaternionChart<_Scalar,_Options> #define QUATERNION_CHART MakeQuaternionChart<_Scalar,_Options>
GTSAM_MANIFOLD(QUATERNION_TEMPLATE,QUATERNION_TYPE,3,QUATERNION_TANGENT,QUATERNION_CHART) GTSAM_MANIFOLD(QUATERNION_TEMPLATE, QUATERNION_TYPE, 3, QUATERNION_TANGENT,
QUATERNION_CHART)
/// Define Eigen::Quaternion to be a model of the Lie Group concept /// Define Eigen::Quaternion to be a model of the Lie Group concept
namespace traits { namespace traits {
@ -98,6 +106,7 @@ struct structure_category<Eigen::Quaternion<S, O> > {
* and and these pool allocators do not support alignment. * and and these pool allocators do not support alignment.
*/ */
typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion; typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;
typedef MakeQuaternionChart<double, Eigen::DontAlign> QuaternionChart;
} // \namespace gtsam } // \namespace gtsam