diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index fd0d6f1e1..4dcf1c861 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -20,8 +20,10 @@ #pragma once -#include +#include + #include +#include #include #include @@ -33,11 +35,8 @@ namespace gtsam { * We guarantee (all but first) constructors only generate from sub-manifold. * However, round-off errors in repeated composition could move off it... */ -class SO3: public Matrix3, public LieGroup { - -protected: - -public: +class SO3 : public SOnBase, public Matrix3, public LieGroup { + public: enum { N = 3 }; enum { dimension = 3 }; @@ -45,20 +44,14 @@ public: /// @{ /// Default constructor creates identity - SO3() : - Matrix3(I_3x3) { - } + SO3() : Matrix3(I_3x3) {} /// Constructor from Eigen Matrix - template - SO3(const MatrixBase& R) : - Matrix3(R.eval()) { - } + template + SO3(const MatrixBase& R) : Matrix3(R.eval()) {} /// Constructor from AngleAxisd - SO3(const Eigen::AngleAxisd& angleAxis) : - Matrix3(angleAxis) { - } + SO3(const Eigen::AngleAxisd& angleAxis) : Matrix3(angleAxis) {} /// Static, named constructor. TODO(dellaert): think about relation with above GTSAM_EXPORT static SO3 AxisAngle(const Vector3& axis, double theta); @@ -163,7 +156,6 @@ public: ar & boost::serialization::make_nvp("R32", (*this)(2,1)); ar & boost::serialization::make_nvp("R33", (*this)(2,2)); } - }; namespace so3 { diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h index 59279b558..28fe5513e 100644 --- a/gtsam/geometry/SO4.h +++ b/gtsam/geometry/SO4.h @@ -19,6 +19,8 @@ #pragma once +#include + #include #include #include @@ -34,7 +36,7 @@ namespace gtsam { /** * True SO(4), i.e., 4*4 matrix subgroup */ -class SO4 : public Matrix4, public LieGroup { +class SO4 : public SOnBase, public Matrix4, public LieGroup { public: enum { N = 4 }; enum { dimension = 6 }; diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 475e71861..ad25a6f92 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -21,7 +21,6 @@ #include #include - #include #include @@ -34,22 +33,17 @@ constexpr int VecSize(int N) { return (N < 0) ? Eigen::Dynamic : N * N; } /** * Base class for rotation group objects. Template arguments: - * Derived: derived class + * Derived_: derived class + * Must have: * N : dimension of ambient space, or Eigen::Dynamic, e.g. 4 for SO4 - * D : dimension of rotation manifold, or Eigen::Dynamic, e.g. 6 for SO4 + * dimension: manifold dimension, or Eigen::Dynamic, e.g. 6 for SO4 */ -template +template class SOnBase { public: - enum { N2 = internal::VecSize(N) }; - using VectorN2 = Eigen::Matrix; - /// @name Basic functionality /// @{ - /// Get access to derived class - const Derived& derived() const { return static_cast(*this); } - /// @} /// @name Manifold /// @{ @@ -62,44 +56,13 @@ class SOnBase { /// @name Other methods /// @{ - /** - * Return vectorized rotation matrix in column order. - * Will use dynamic matrices as intermediate results, but returns a fixed size - * X and fixed-size Jacobian if dimension is known at compile time. - * */ - VectorN2 vec(OptionalJacobian H = boost::none) const { - const size_t n = derived().rows(), n2 = n * n; - const size_t d = (n2 - n) / 2; // manifold dimension - - // Calculate G matrix of vectorized generators - Matrix G(n2, d); - for (size_t j = 0; j < d; j++) { - // TODO(frank): this can't be right. Think about fixed vs dynamic. - const auto X = derived().Hat(n, Eigen::VectorXd::Unit(d, j)); - G.col(j) = Eigen::Map(X.data(), n2, 1); - } - - // Vectorize - Vector X(n2); - X << Eigen::Map(derived().data(), n2, 1); - - // If requested, calculate H as (I \oplus Q) * P - if (H) { - H->resize(n2, d); - for (size_t i = 0; i < n; i++) { - H->block(i * n, 0, n, d) = derived() * G.block(i * n, 0, n, d); - } - } - return X; - } /// @} }; /** * Variable size rotations */ -class SOn : public Eigen::MatrixXd, - public SOnBase { +class SOn : public Eigen::MatrixXd, public SOnBase { public: enum { N = Eigen::Dynamic }; enum { dimension = Eigen::Dynamic }; @@ -116,8 +79,9 @@ class SOn : public Eigen::MatrixXd, } /// Constructor from Eigen Matrix - template - explicit SOn(const Eigen::MatrixBase& R) : Eigen::MatrixXd(R.eval()) {} + template + explicit SOn(const Eigen::MatrixBase& R) + : Eigen::MatrixXd(R.eval()) {} /// Random SO(n) element (no big claims about uniformity) static SOn Random(boost::mt19937& rng, size_t n) { @@ -139,7 +103,7 @@ class SOn : public Eigen::MatrixXd, * Hat operator creates Lie algebra element corresponding to d-vector, where d * is the dimensionality of the manifold. This function is implemented * recursively, and the d-vector is assumed to laid out such that the last - * element corresponds to so(2), the last 3 to so(3), the last for to so(4) + * element corresponds to so(2), the last 3 to so(3), the last 6 to so(4) * etc... For example, the vector-space isomorphic to so(5) is laid out as: * a b c d | u v w | x y | z * where the latter elements correspond to "telescoping" sub-algebras: @@ -236,7 +200,38 @@ class SOn : public Eigen::MatrixXd, /// @{ /// Return matrix (for wrapper) - const Matrix &matrix() const { return *this; } + const Matrix& matrix() const { return *this; } + + /** + * Return vectorized rotation matrix in column order. + * Will use dynamic matrices as intermediate results, but returns a fixed size + * X and fixed-size Jacobian if dimension is known at compile time. + * */ + Vector vec(OptionalJacobian<-1, -1> H = boost::none) const { + const size_t n = rows(), n2 = n * n; + const size_t d = (n2 - n) / 2; // manifold dimension + + // Calculate G matrix of vectorized generators + Matrix G(n2, d); + for (size_t j = 0; j < d; j++) { + // TODO(frank): this can't be right. Think about fixed vs dynamic. + const auto X = Hat(n, Eigen::VectorXd::Unit(d, j)); + G.col(j) = Eigen::Map(X.data(), n2, 1); + } + + // Vectorize + Vector X(n2); + X << Eigen::Map(data(), n2, 1); + + // If requested, calculate H as (I \oplus Q) * P + if (H) { + H->resize(n2, d); + for (size_t i = 0; i < n; i++) { + H->block(i * n, 0, n, d) = derived() * G.block(i * n, 0, n, d); + } + } + return X; + } /// @} };