From 6cff36975ffe01da2d38166fb27501c982cd6d2d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 May 2019 11:07:09 -0400 Subject: [PATCH] Moved code into appropriate headers (SOt instead of SO3 for now) --- gtsam/geometry/SO3.h | 4 +- gtsam/geometry/SO4.cpp | 345 ++++++++-------- gtsam/geometry/SO4.h | 238 ++++++----- gtsam/geometry/SOn.h | 327 +++++++++------ gtsam/geometry/SOt.cpp | 281 +++++++++++++ gtsam/geometry/SOt.h | 204 ++++++++++ gtsam/geometry/tests/testSOn.cpp | 674 +------------------------------ 7 files changed, 1014 insertions(+), 1059 deletions(-) create mode 100644 gtsam/geometry/SOt.cpp create mode 100644 gtsam/geometry/SOt.h diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 4dcf1c861..f89e2f59a 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -20,8 +20,6 @@ #pragma once -#include - #include #include @@ -35,7 +33,7 @@ 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 SOnBase, public Matrix3, public LieGroup { +class SO3 : public Matrix3, public LieGroup { public: enum { N = 3 }; enum { dimension = 3 }; diff --git a/gtsam/geometry/SO4.cpp b/gtsam/geometry/SO4.cpp index ebbc91c01..1f265ecba 100644 --- a/gtsam/geometry/SO4.cpp +++ b/gtsam/geometry/SO4.cpp @@ -31,196 +31,199 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -static Vector3 randomOmega(boost::mt19937 &rng) { - static boost::uniform_real randomAngle(-M_PI, M_PI); - return Unit3::Random(rng).unitVector() * randomAngle(rng); -} +// /* ************************************************************************* */ +// static Vector3 randomOmega(boost::mt19937 &rng) { +// static boost::uniform_real randomAngle(-M_PI, M_PI); +// return Unit3::Random(rng).unitVector() * randomAngle(rng); +// } -/* ************************************************************************* */ -// Create random SO(4) element using direct product of lie algebras. -SO4 SO4::Random(boost::mt19937 &rng) { - Vector6 delta; - delta << randomOmega(rng), randomOmega(rng); - return SO4::Expmap(delta); -} +// /* ************************************************************************* */ +// // Create random SO(4) element using direct product of lie algebras. +// SO4 SO4::Random(boost::mt19937 &rng) { +// Vector6 delta; +// delta << randomOmega(rng), randomOmega(rng); +// return SO4::Expmap(delta); +// } -/* ************************************************************************* */ -void SO4::print(const string &s) const { cout << s << *this << endl; } +// /* ************************************************************************* */ +// void SO4::print(const string &s) const { cout << s << *this << endl; } -/* ************************************************************************* */ -bool SO4::equals(const SO4 &R, double tol) const { - return equal_with_abs_tol(*this, R, tol); -} +// /* ************************************************************************* */ +// bool SO4::equals(const SO4 &R, double tol) const { +// return equal_with_abs_tol(*this, R, tol); +// } -//****************************************************************************** -Matrix4 SO4::Hat(const Vector6 &xi) { - // skew symmetric matrix X = xi^ - // Unlike Luca, makes upper-left the SO(3) subgroup. - // See also - // http://scipp.ucsc.edu/~haber/archives/physics251_13/groups13_sol4.pdf - Matrix4 Y = Z_4x4; - Y(0, 1) = -xi(2); - Y(0, 2) = +xi(1); - Y(1, 2) = -xi(0); - Y(0, 3) = -xi(3); - Y(1, 3) = -xi(4); - Y(2, 3) = -xi(5); - return Y - Y.transpose(); -} -/* ************************************************************************* */ -Vector6 SO4::Vee(const Matrix4 &X) { - Vector6 xi; - xi(2) = -X(0, 1); - xi(1) = X(0, 2); - xi(0) = -X(1, 2); - xi(3) = -X(0, 3); - xi(4) = -X(1, 3); - xi(5) = -X(2, 3); - return xi; -} +// //****************************************************************************** +// Matrix4 SO4::Hat(const Vector6 &xi) { +// // skew symmetric matrix X = xi^ +// // Unlike Luca, makes upper-left the SO(3) subgroup. +// // See also +// // http://scipp.ucsc.edu/~haber/archives/physics251_13/groups13_sol4.pdf +// Matrix4 Y = Z_4x4; +// Y(0, 1) = -xi(2); +// Y(0, 2) = +xi(1); +// Y(1, 2) = -xi(0); +// Y(0, 3) = -xi(3); +// Y(1, 3) = -xi(4); +// Y(2, 3) = -xi(5); +// return Y - Y.transpose(); +// } +// /* ************************************************************************* */ +// Vector6 SO4::Vee(const Matrix4 &X) { +// Vector6 xi; +// xi(2) = -X(0, 1); +// xi(1) = X(0, 2); +// xi(0) = -X(1, 2); +// xi(3) = -X(0, 3); +// xi(4) = -X(1, 3); +// xi(5) = -X(2, 3); +// return xi; +// } -//****************************************************************************** -/* Exponential map, porting MATLAB implementation by Luca, which follows - * "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by - * Ramona-Andreaa Rohan */ -SO4 SO4::Expmap(const Vector6 &xi, ChartJacobian H) { - if (H) throw std::runtime_error("SO4::Expmap Jacobian"); - gttic(SO4_Expmap); +// //****************************************************************************** +// /* Exponential map, porting MATLAB implementation by Luca, which follows +// * "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by +// * Ramona-Andreaa Rohan */ +// template <> +// SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) { +// using namespace std; +// if (H) throw std::runtime_error("SO4::Expmap Jacobian"); - // skew symmetric matrix X = xi^ - const Matrix4 X = Hat(xi); +// // skew symmetric matrix X = xi^ +// const Matrix4 X = Hat(xi); - // do eigen-decomposition - auto eig = Eigen::EigenSolver(X); - Eigen::Vector4cd e = eig.eigenvalues(); - using std::abs; - sort(e.data(), e.data() + 4, [](complex a, complex b) { - return abs(a.imag()) > abs(b.imag()); - }); +// // do eigen-decomposition +// auto eig = Eigen::EigenSolver(X); +// Eigen::Vector4cd e = eig.eigenvalues(); +// using std::abs; +// sort(e.data(), e.data() + 4, [](complex a, complex b) { +// return abs(a.imag()) > abs(b.imag()); +// }); - // Get a and b from eigenvalues +/i ai and +/- bi - double a = e[0].imag(), b = e[2].imag(); - if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) { - throw runtime_error("SO4::Expmap: wrong eigenvalues."); - } +// // Get a and b from eigenvalues +/i ai and +/- bi +// double a = e[0].imag(), b = e[2].imag(); +// if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) { +// throw runtime_error("SO4::Expmap: wrong eigenvalues."); +// } - // Build expX = exp(xi^) - Matrix4 expX; - using std::cos; - using std::sin; - const auto X2 = X * X; - const auto X3 = X2 * X; - double a2 = a * a, a3 = a2 * a, b2 = b * b, b3 = b2 * b; - if (a != 0 && b == 0) { - double c2 = (1 - cos(a)) / a2, c3 = (a - sin(a)) / a3; - return I_4x4 + X + c2 * X2 + c3 * X3; - } else if (a == b && b != 0) { - double sin_a = sin(a), cos_a = cos(a); - double c0 = (a * sin_a + 2 * cos_a) / 2, - c1 = (3 * sin_a - a * cos_a) / (2 * a), c2 = sin_a / (2 * a), - c3 = (sin_a - a * cos_a) / (2 * a3); - return c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3; - } else if (a != b) { - double sin_a = sin(a), cos_a = cos(a); - double sin_b = sin(b), cos_b = cos(b); - double c0 = (b2 * cos_a - a2 * cos_b) / (b2 - a2), - c1 = (b3 * sin_a - a3 * sin_b) / (a * b * (b2 - a2)), - c2 = (cos_a - cos_b) / (b2 - a2), - c3 = (b * sin_a - a * sin_b) / (a * b * (b2 - a2)); - return c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3; - } else { - return I_4x4; - } -} +// // Build expX = exp(xi^) +// Matrix4 expX; +// using std::cos; +// using std::sin; +// const auto X2 = X * X; +// const auto X3 = X2 * X; +// double a2 = a * a, a3 = a2 * a, b2 = b * b, b3 = b2 * b; +// if (a != 0 && b == 0) { +// double c2 = (1 - cos(a)) / a2, c3 = (a - sin(a)) / a3; +// return SO4(I_4x4 + X + c2 * X2 + c3 * X3); +// } else if (a == b && b != 0) { +// double sin_a = sin(a), cos_a = cos(a); +// double c0 = (a * sin_a + 2 * cos_a) / 2, +// c1 = (3 * sin_a - a * cos_a) / (2 * a), c2 = sin_a / (2 * a), +// c3 = (sin_a - a * cos_a) / (2 * a3); +// return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3); +// } else if (a != b) { +// double sin_a = sin(a), cos_a = cos(a); +// double sin_b = sin(b), cos_b = cos(b); +// double c0 = (b2 * cos_a - a2 * cos_b) / (b2 - a2), +// c1 = (b3 * sin_a - a3 * sin_b) / (a * b * (b2 - a2)), +// c2 = (cos_a - cos_b) / (b2 - a2), +// c3 = (b * sin_a - a * sin_b) / (a * b * (b2 - a2)); +// return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3); +// } else { +// return SO4(); +// } +// } -//****************************************************************************** -Vector6 SO4::Logmap(const SO4 &Q, ChartJacobian H) { - if (H) throw std::runtime_error("SO4::Logmap Jacobian"); - throw std::runtime_error("SO4::Logmap"); -} +// //****************************************************************************** +// static SO4::VectorN2 vec4(const Matrix4& Q) { +// return Eigen::Map(Q.data()); +// } -/* ************************************************************************* */ -SO4 SO4::ChartAtOrigin::Retract(const Vector6 &xi, ChartJacobian H) { - if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian"); - gttic(SO4_Retract); - const Matrix4 X = Hat(xi / 2); - return (I_4x4 + X) * (I_4x4 - X).inverse(); -} +// static const std::vector G4( +// {SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)), +// SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)), +// SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))}); -/* ************************************************************************* */ -Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H) { - if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian"); - const Matrix4 X = (I_4x4 - Q) * (I_4x4 + Q).inverse(); - return -2 * Vee(X); -} +// static const Eigen::Matrix P4 = +// (Eigen::Matrix() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]), +// vec4(G4[3]), vec4(G4[4]), vec4(G4[5])) +// .finished(); -/* ************************************************************************* */ -static SO4::Vector16 vec(const SO4 &Q) { - return Eigen::Map(Q.data()); -} +// //****************************************************************************** +// template <> +// Matrix6 SO4::AdjointMap() const { +// // Elaborate way of calculating the AdjointMap +// // TODO(frank): find a closed form solution. In SO(3) is just R :-/ +// const Matrix4& Q = matrix_; +// const Matrix4 Qt = Q.transpose(); +// Matrix6 A; +// for (size_t i = 0; i < 6; i++) { +// // Calculate column i of linear map for coeffcient of Gi +// A.col(i) = SO4::Vee(Q * G4[i] * Qt); +// } +// return A; +// } -static const std::vector G( - {SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)), - SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)), - SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))}); +// //****************************************************************************** +// template <> +// SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const { +// const Matrix& Q = matrix_; +// if (H) { +// // As Luca calculated, this is (I4 \oplus Q) * P4 +// *H << Q * P4.block<4, 6>(0, 0), Q * P4.block<4, 6>(4, 0), +// Q * P4.block<4, 6>(8, 0), Q * P4.block<4, 6>(12, 0); +// } +// return gtsam::vec4(Q); +// } -static const Eigen::Matrix P = - (Eigen::Matrix() << vec(G[0]), vec(G[1]), vec(G[2]), - vec(G[3]), vec(G[4]), vec(G[5])) - .finished(); -/* ************************************************************************* */ -Matrix6 SO4::AdjointMap() const { - gttic(SO4_AdjointMap); - // Elaborate way of calculating the AdjointMap - // TODO(frank): find a closed form solution. In SO(3) is just R :-/ - const SO4 &Q = *this; - const SO4 Qt = this->inverse(); - Matrix6 A; - for (size_t i = 0; i < 6; i++) { - // Calculate column i of linear map for coeffcient of Gi - A.col(i) = SO4::Vee(Q * G[i] * Qt); - } - return A; -} +// //****************************************************************************** +// Vector6 SO4::Logmap(const SO4 &Q, ChartJacobian H) { +// if (H) throw std::runtime_error("SO4::Logmap Jacobian"); +// throw std::runtime_error("SO4::Logmap"); +// } -/* ************************************************************************* */ -SO4::Vector16 SO4::vec(OptionalJacobian<16, 6> H) const { - const SO4 &Q = *this; - if (H) { - // As Luca calculated, this is (I4 \oplus Q) * P - *H << Q * P.block<4, 6>(0, 0), Q * P.block<4, 6>(4, 0), - Q * P.block<4, 6>(8, 0), Q * P.block<4, 6>(12, 0); - } - return gtsam::vec(Q); -}; +// /* ************************************************************************* */ +// SO4 SO4::ChartAtOrigin::Retract(const Vector6 &xi, ChartJacobian H) { +// if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian"); +// gttic(SO4_Retract); +// const Matrix4 X = Hat(xi / 2); +// return (I_4x4 + X) * (I_4x4 - X).inverse(); +// } -/* ************************************************************************* */ -Matrix3 SO4::topLeft(OptionalJacobian<9, 6> H) const { - const Matrix3 M = this->topLeftCorner<3, 3>(); - const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2), - q = this->topRightCorner<3, 1>(); - if (H) { - *H << Z_3x1, -m3, m2, q, Z_3x1, Z_3x1, // - m3, Z_3x1, -m1, Z_3x1, q, Z_3x1, // - -m2, m1, Z_3x1, Z_3x1, Z_3x1, q; - } - return M; -} +// /* ************************************************************************* */ +// Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H) { +// if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian"); +// const Matrix4 X = (I_4x4 - Q) * (I_4x4 + Q).inverse(); +// return -2 * Vee(X); +// } -/* ************************************************************************* */ -Matrix43 SO4::stiefel(OptionalJacobian<12, 6> H) const { - const Matrix43 M = this->leftCols<3>(); - const auto &m1 = col(0), m2 = col(1), m3 = col(2), q = col(3); - if (H) { - *H << Z_4x1, -m3, m2, q, Z_4x1, Z_4x1, // - m3, Z_4x1, -m1, Z_4x1, q, Z_4x1, // - -m2, m1, Z_4x1, Z_4x1, Z_4x1, q; - } - return M; -} +// /* ************************************************************************* */ +// Matrix3 SO4::topLeft(OptionalJacobian<9, 6> H) const { +// const Matrix3 M = this->topLeftCorner<3, 3>(); +// const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2), +// q = this->topRightCorner<3, 1>(); +// if (H) { +// *H << Z_3x1, -m3, m2, q, Z_3x1, Z_3x1, // +// m3, Z_3x1, -m1, Z_3x1, q, Z_3x1, // +// -m2, m1, Z_3x1, Z_3x1, Z_3x1, q; +// } +// return M; +// } -/* ************************************************************************* */ +// /* ************************************************************************* */ +// Matrix43 SO4::stiefel(OptionalJacobian<12, 6> H) const { +// const Matrix43 M = this->leftCols<3>(); +// const auto &m1 = col(0), m2 = col(1), m3 = col(2), q = col(3); +// if (H) { +// *H << Z_4x1, -m3, m2, q, Z_4x1, Z_4x1, // +// m3, Z_4x1, -m1, Z_4x1, q, Z_4x1, // +// -m2, m1, Z_4x1, Z_4x1, Z_4x1, q; +// } +// return M; +// } + +// /* ************************************************************************* */ } // end namespace gtsam diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h index 28fe5513e..aed0d482d 100644 --- a/gtsam/geometry/SO4.h +++ b/gtsam/geometry/SO4.h @@ -33,111 +33,155 @@ namespace gtsam { -/** - * True SO(4), i.e., 4*4 matrix subgroup - */ -class SO4 : public SOnBase, public Matrix4, public LieGroup { - public: - enum { N = 4 }; - enum { dimension = 6 }; +using SO4 = SO<4>; - typedef Eigen::Matrix Vector16; +// /// Random SO(4) element (no big claims about uniformity) +// static SO4 Random(boost::mt19937 &rng); - /// @name Constructors - /// @{ +// static Matrix4 Hat(const Vector6 &xi); ///< make skew symmetric matrix +// static Vector6 Vee(const Matrix4 &X); ///< inverse of Hat +// static SO4 Expmap(const Vector6 &xi, +// ChartJacobian H = boost::none); ///< exponential map +// static Vector6 Logmap(const SO4 &Q, +// ChartJacobian H = boost::none); ///< and its inverse +// Matrix6 AdjointMap() const; - /// Default constructor creates identity - SO4() : Matrix4(I_4x4) {} +//****************************************************************************** +/* Exponential map, porting MATLAB implementation by Luca, which follows + * "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by + * Ramona-Andreaa Rohan */ +template <> +SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) { + using namespace std; + if (H) throw std::runtime_error("SO4::Expmap Jacobian"); - /// Constructor from Eigen Matrix - template - SO4(const MatrixBase &R) : Matrix4(R.eval()) {} + // skew symmetric matrix X = xi^ + const Matrix4 X = Hat(xi); - /// Random SO(4) element (no big claims about uniformity) - static SO4 Random(boost::mt19937 &rng); + // do eigen-decomposition + auto eig = Eigen::EigenSolver(X); + Eigen::Vector4cd e = eig.eigenvalues(); + using std::abs; + sort(e.data(), e.data() + 4, [](complex a, complex b) { + return abs(a.imag()) > abs(b.imag()); + }); - /// @} - /// @name Testable - /// @{ - - void print(const std::string &s) const; - bool equals(const SO4 &R, double tol) const; - - /// @} - /// @name Group - /// @{ - - /// identity rotation for group operation - static SO4 identity() { return I_4x4; } - - /// inverse of a rotation = transpose - SO4 inverse() const { return this->transpose(); } - - /// @} - /// @name Lie Group - /// @{ - - static Matrix4 Hat(const Vector6 &xi); ///< make skew symmetric matrix - static Vector6 Vee(const Matrix4 &X); ///< inverse of Hat - static SO4 Expmap(const Vector6 &xi, - ChartJacobian H = boost::none); ///< exponential map - static Vector6 Logmap(const SO4 &Q, - ChartJacobian H = boost::none); ///< and its inverse - Matrix6 AdjointMap() const; - - // Chart at origin - struct ChartAtOrigin { - static SO4 Retract(const Vector6 &omega, ChartJacobian H = boost::none); - static Vector6 Local(const SO4 &R, ChartJacobian H = boost::none); - }; - - using LieGroup::inverse; - - /// @} - /// @name Other methods - /// @{ - - /// Vectorize - Vector16 vec(OptionalJacobian<16, 6> H = boost::none) const; - - /// Project to top-left 3*3 matrix. Note this is *not* in general \in SO(3). - Matrix3 topLeft(OptionalJacobian<9, 6> H = boost::none) const; - - /// Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) -> S \in St(3,4). - Matrix43 stiefel(OptionalJacobian<12, 6> H = boost::none) const; - - /// Return matrix (for wrapper) - const Matrix4 &matrix() const { return *this; } - - /// @} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE &ar, const unsigned int /*version*/) { - ar &boost::serialization::make_nvp("Q11", (*this)(0, 0)); - ar &boost::serialization::make_nvp("Q12", (*this)(0, 1)); - ar &boost::serialization::make_nvp("Q13", (*this)(0, 2)); - ar &boost::serialization::make_nvp("Q14", (*this)(0, 3)); - - ar &boost::serialization::make_nvp("Q21", (*this)(1, 0)); - ar &boost::serialization::make_nvp("Q22", (*this)(1, 1)); - ar &boost::serialization::make_nvp("Q23", (*this)(1, 2)); - ar &boost::serialization::make_nvp("Q24", (*this)(1, 3)); - - ar &boost::serialization::make_nvp("Q31", (*this)(2, 0)); - ar &boost::serialization::make_nvp("Q32", (*this)(2, 1)); - ar &boost::serialization::make_nvp("Q33", (*this)(2, 2)); - ar &boost::serialization::make_nvp("Q34", (*this)(2, 3)); - - ar &boost::serialization::make_nvp("Q41", (*this)(3, 0)); - ar &boost::serialization::make_nvp("Q42", (*this)(3, 1)); - ar &boost::serialization::make_nvp("Q43", (*this)(3, 2)); - ar &boost::serialization::make_nvp("Q44", (*this)(3, 3)); + // Get a and b from eigenvalues +/i ai and +/- bi + double a = e[0].imag(), b = e[2].imag(); + if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) { + throw runtime_error("SO4::Expmap: wrong eigenvalues."); } -}; // SO4 + // Build expX = exp(xi^) + Matrix4 expX; + using std::cos; + using std::sin; + const auto X2 = X * X; + const auto X3 = X2 * X; + double a2 = a * a, a3 = a2 * a, b2 = b * b, b3 = b2 * b; + if (a != 0 && b == 0) { + double c2 = (1 - cos(a)) / a2, c3 = (a - sin(a)) / a3; + return SO4(I_4x4 + X + c2 * X2 + c3 * X3); + } else if (a == b && b != 0) { + double sin_a = sin(a), cos_a = cos(a); + double c0 = (a * sin_a + 2 * cos_a) / 2, + c1 = (3 * sin_a - a * cos_a) / (2 * a), c2 = sin_a / (2 * a), + c3 = (sin_a - a * cos_a) / (2 * a3); + return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3); + } else if (a != b) { + double sin_a = sin(a), cos_a = cos(a); + double sin_b = sin(b), cos_b = cos(b); + double c0 = (b2 * cos_a - a2 * cos_b) / (b2 - a2), + c1 = (b3 * sin_a - a3 * sin_b) / (a * b * (b2 - a2)), + c2 = (cos_a - cos_b) / (b2 - a2), + c3 = (b * sin_a - a * sin_b) / (a * b * (b2 - a2)); + return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3); + } else { + return SO4(); + } +} + +//****************************************************************************** +static SO4::VectorN2 vec4(const Matrix4& Q) { + return Eigen::Map(Q.data()); +} + +static const std::vector G4( + {SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)), + SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)), + SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))}); + +static const Eigen::Matrix P4 = + (Eigen::Matrix() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]), + vec4(G4[3]), vec4(G4[4]), vec4(G4[5])) + .finished(); + +//****************************************************************************** +template <> +Matrix6 SO4::AdjointMap() const { + // Elaborate way of calculating the AdjointMap + // TODO(frank): find a closed form solution. In SO(3) is just R :-/ + const Matrix4& Q = matrix_; + const Matrix4 Qt = Q.transpose(); + Matrix6 A; + for (size_t i = 0; i < 6; i++) { + // Calculate column i of linear map for coeffcient of Gi + A.col(i) = SO4::Vee(Q * G4[i] * Qt); + } + return A; +} + +//****************************************************************************** +template <> +SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const { + const Matrix& Q = matrix_; + if (H) { + // As Luca calculated, this is (I4 \oplus Q) * P4 + *H << Q * P4.block<4, 6>(0, 0), Q * P4.block<4, 6>(4, 0), + Q * P4.block<4, 6>(8, 0), Q * P4.block<4, 6>(12, 0); + } + return gtsam::vec4(Q); +} + +// /// Vectorize +// Vector16 vec(OptionalJacobian<16, 6> H = boost::none) const; + +// /// Project to top-left 3*3 matrix. Note this is *not* in general \in SO(3). +// Matrix3 topLeft(OptionalJacobian<9, 6> H = boost::none) const; + +// /// Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., +// pi(Q) -> S \in St(3,4). Matrix43 stiefel(OptionalJacobian<12, 6> H = +// boost::none) const; + +// private: +// /** Serialization function */ +// friend class boost::serialization::access; +// template +// void serialize(ARCHIVE &ar, const unsigned int /*version*/) { +// ar &boost::serialization::make_nvp("Q11", (*this)(0, 0)); +// ar &boost::serialization::make_nvp("Q12", (*this)(0, 1)); +// ar &boost::serialization::make_nvp("Q13", (*this)(0, 2)); +// ar &boost::serialization::make_nvp("Q14", (*this)(0, 3)); + +// ar &boost::serialization::make_nvp("Q21", (*this)(1, 0)); +// ar &boost::serialization::make_nvp("Q22", (*this)(1, 1)); +// ar &boost::serialization::make_nvp("Q23", (*this)(1, 2)); +// ar &boost::serialization::make_nvp("Q24", (*this)(1, 3)); + +// ar &boost::serialization::make_nvp("Q31", (*this)(2, 0)); +// ar &boost::serialization::make_nvp("Q32", (*this)(2, 1)); +// ar &boost::serialization::make_nvp("Q33", (*this)(2, 2)); +// ar &boost::serialization::make_nvp("Q34", (*this)(2, 3)); + +// ar &boost::serialization::make_nvp("Q41", (*this)(3, 0)); +// ar &boost::serialization::make_nvp("Q42", (*this)(3, 1)); +// ar &boost::serialization::make_nvp("Q43", (*this)(3, 2)); +// ar &boost::serialization::make_nvp("Q44", (*this)(3, 3)); +// } + +/* + * Define the traits. internal::LieGroup provides both Lie group and Testable + */ template <> struct traits : Testable, internal::LieGroupTraits {}; diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index ad25a6f92..4f4fcd157 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -11,94 +11,168 @@ /** * @file SOn.h - * @brief n*n matrix representation of SO(n) + * @brief n*n matrix representation of SO(n), template on N, which can be + * Eigen::Dynamic * @author Frank Dellaert * @date March 2019 */ #pragma once +#include #include #include #include #include +#include namespace gtsam { + namespace internal { +/// Calculate dimensionality of SO manifold, or return Dynamic if so +constexpr int DimensionSO(int N) { + return (N < 0) ? Eigen::Dynamic : N * (N - 1) / 2; +} + // Calculate N^2 at compile time, or return Dynamic if so -constexpr int VecSize(int N) { return (N < 0) ? Eigen::Dynamic : N * N; } +constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; } } // namespace internal /** - * Base class for rotation group objects. Template arguments: - * Derived_: derived class - * Must have: - * N : dimension of ambient space, or Eigen::Dynamic, e.g. 4 for SO4 - * dimension: manifold dimension, or Eigen::Dynamic, e.g. 6 for SO4 + * Manifold of special orthogonal rotation matrices SO. + * Template paramater N can be a fizxed integer or can be Eigen::Dynamic */ -template -class SOnBase { +template +class SO : public LieGroup, internal::DimensionSO(N)> { public: - /// @name Basic functionality - /// @{ + enum { dimension = internal::DimensionSO(N) }; + using MatrixNN = Eigen::Matrix; + using VectorN2 = Eigen::Matrix; + using MatrixDD = Eigen::Matrix; - /// @} - /// @name Manifold - /// @{ + protected: + MatrixNN matrix_; ///< Rotation matrix - /// @} - /// @name Lie Group - /// @{ + // enable_if_t aliases, used to specialize constructors/methods, see + // https://www.fluentcpp.com/2018/05/18/make-sfinae-pretty-2-hidden-beauty-sfinae/ + template + using IsDynamic = boost::enable_if_t; + template + using IsFixed = boost::enable_if_t= 2, void>; + template + using IsSO3 = boost::enable_if_t; - /// @} - /// @name Other methods - /// @{ - - /// @} -}; - -/** - * Variable size rotations - */ -class SOn : public Eigen::MatrixXd, public SOnBase { public: - enum { N = Eigen::Dynamic }; - enum { dimension = Eigen::Dynamic }; - /// @name Constructors /// @{ - /// Default constructor: only used for serialization and wrapping - SOn() {} + /// Construct SO identity for N >= 2 + template > + SO() : matrix_(MatrixNN::Identity()) {} - /// Construct SO(n) identity - explicit SOn(size_t n) : Eigen::MatrixXd(n, n) { - *this << Eigen::MatrixXd::Identity(n, n); + /// Construct SO identity for N == Eigen::Dynamic + template > + explicit SO(size_t n = 0) { + if (n == 0) throw std::runtime_error("SO: Dimensionality not known."); + matrix_ = Eigen::MatrixXd::Identity(n, n); } /// Constructor from Eigen Matrix - template - explicit SOn(const Eigen::MatrixBase& R) - : Eigen::MatrixXd(R.eval()) {} + template + explicit SO(const Eigen::MatrixBase& R) : matrix_(R.eval()) {} + + /// Constructor from AngleAxisd + template > + SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {} /// Random SO(n) element (no big claims about uniformity) - static SOn Random(boost::mt19937& rng, size_t n) { + template > + static SO Random(boost::mt19937& rng, size_t n = 0) { + if (n == 0) throw std::runtime_error("SO: Dimensionality not known."); // This needs to be re-thought! static boost::uniform_real randomAngle(-M_PI, M_PI); - const size_t d = n * (n - 1) / 2; + const size_t d = SO::Dimension(n); Vector xi(d); for (size_t j = 0; j < d; j++) { xi(j) = randomAngle(rng); } - return SOn::Retract(n, xi); + return SO::Retract(xi); } + /// Random SO(N) element (no big claims about uniformity) + template > + static SO Random(boost::mt19937& rng) { + // By default, use dynamic implementation above. Specialized for SO(3). + return SO(SO::Random(rng, N).matrix()); + } + + /// @} + /// @name Standard methods + /// @{ + + /// Return matrix + const MatrixNN& matrix() const { return matrix_; } + + size_t rows() const { return matrix_.rows(); } + size_t cols() const { return matrix_.cols(); } + + /// @} + /// @name Testable + /// @{ + + void print(const std::string& s) const { + std::cout << s << matrix_ << std::endl; + } + + bool equals(const SO& other, double tol) const { + return equal_with_abs_tol(matrix_, other.matrix_, tol); + } + + /// @} + /// @name Group + /// @{ + + /// Multiplication + SO operator*(const SO& other) const { return SO(matrix_ * other.matrix_); } + + /// SO identity for N >= 2 + template > + static SO identity() { + return SO(); + } + + /// SO identity for N == Eigen::Dynamic + template > + static SO identity(size_t n = 0) { + return SO(n); + } + + /// inverse of a rotation = transpose + SO inverse() const { return SO(matrix_.transpose()); } + /// @} /// @name Manifold /// @{ + using TangentVector = Eigen::Matrix; + using ChartJacobian = OptionalJacobian; + + /// Return compile-time dimensionality: fixed size N or Eigen::Dynamic + static int Dim() { return dimension; } + + // Calculate manifold dimensionality for SO(n). + // Available as dimension or Dim() for fixed N. + static size_t Dimension(size_t n) { return n * (n - 1) / 2; } + + // Calculate ambient dimension n from manifold dimensionality d. + static size_t AmbientDim(size_t d) { return (1 + std::sqrt(1 + 8 * d)) / 2; } + + // Calculate run-time dimensionality of manifold. + // Available as dimension or Dim() for fixed N. + size_t dim() const { return Dimension(matrix_.rows()); } + /** * Hat operator creates Lie algebra element corresponding to d-vector, where d * is the dimensionality of the manifold. This function is implemented @@ -114,7 +188,8 @@ class SOn : public Eigen::MatrixXd, public SOnBase { * -d c -b a 0 * This scheme behaves exactly as expected for SO(2) and SO(3). */ - static Matrix Hat(size_t n, const Vector& xi) { + static Matrix Hat(const Vector& xi) { + size_t n = AmbientDim(xi.size()); if (n < 2) throw std::invalid_argument("SOn::Hat: n<2 not supported"); Matrix X(n, n); // allocate space for n*n skew-symmetric matrix @@ -126,7 +201,7 @@ class SOn : public Eigen::MatrixXd, public SOnBase { } else { // Recursively call SO(n-1) call for top-left block const size_t dmin = (n - 1) * (n - 2) / 2; - X.topLeftCorner(n - 1, n - 1) = Hat(n - 1, xi.tail(dmin)); + X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin)); // Now fill last row and column double sign = 1.0; @@ -172,126 +247,114 @@ class SOn : public Eigen::MatrixXd, public SOnBase { } } - /** - * Retract uses Cayley map. See note about xi element order in Hat. - */ - static SOn Retract(size_t n, const Vector& xi) { - const Matrix X = Hat(n, xi / 2.0); - const auto I = Eigen::MatrixXd::Identity(n, n); - return SOn((I + X) * (I - X).inverse()); - } + // Chart at origin + struct ChartAtOrigin { + /** + * Retract uses Cayley map. See note about xi element order in Hat. + * Deafault implementation has no Jacobian implemented + */ + static SO Retract(const TangentVector& xi, ChartJacobian H = boost::none) { + const Matrix X = Hat(xi / 2.0); + size_t n = AmbientDim(xi.size()); + const auto I = Eigen::MatrixXd::Identity(n, n); + return SO((I + X) * (I - X).inverse()); + } + /** + * Inverse of Retract. See note about xi element order in Hat. + */ + static TangentVector Local(const SO& R, ChartJacobian H = boost::none) { + const size_t n = R.rows(); + const auto I = Eigen::MatrixXd::Identity(n, n); + const Matrix X = (I - R.matrix_) * (I + R.matrix_).inverse(); + return -2 * Vee(X); + } + }; - /** - * Inverse of Retract. See note about xi element order in Hat. - */ - static Vector Local(const SOn& R) { - const size_t n = R.rows(); - const auto I = Eigen::MatrixXd::Identity(n, n); - const Matrix X = (I - R) * (I + R).inverse(); - return -2 * Vee(X); + // Return dynamic identity DxD Jacobian for given SO(n) + template > + static MatrixDD IdentityJacobian(size_t n) { + const size_t d = Dimension(n); + return MatrixDD::Identity(d, d); } /// @} - /// @name Lie group + /// @name Lie Group /// @{ + MatrixDD AdjointMap() const { + throw std::runtime_error( + "SO::AdjointMap only implemented for SO3 and SO4."); + } + + /** + * Exponential map at identity - create a rotation from canonical coordinates + */ + static SO Expmap(const TangentVector& omega, ChartJacobian H = boost::none) { + throw std::runtime_error("SO::Expmap only implemented for SO3 and SO4."); + } + + /** + * Log map at identity - returns the canonical coordinates of this rotation + */ + static TangentVector Logmap(const SO& R, ChartJacobian H = boost::none) { + throw std::runtime_error("SO::Logmap only implemented for SO3 and SO4."); + } + + // inverse with optional derivative + using LieGroup, internal::DimensionSO(N)>::inverse; + /// @} /// @name Other methods /// @{ - /// Return matrix (for wrapper) - 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); - } + VectorN2 vec(OptionalJacobian H = + boost::none) const { + const size_t n = rows(); + const size_t n2 = n * n; // Vectorize - Vector X(n2); - X << Eigen::Map(data(), n2, 1); + VectorN2 X(n2); + X << Eigen::Map(matrix_.data(), n2, 1); // If requested, calculate H as (I \oplus Q) * P if (H) { + // Calculate P matrix of vectorized generators + const size_t d = dim(); + Matrix P(n2, d); + for (size_t j = 0; j < d; j++) { + const auto X = Hat(Eigen::VectorXd::Unit(d, j)); + P.col(j) = Eigen::Map(X.data(), n2, 1); + } 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); + H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d); } } return X; } - /// @} }; -template <> -struct traits { - typedef manifold_tag structure_category; +/* + * Fully specialize compose and between, because the derivative is unknowable by + * the LieGroup implementations, who return a fixed-size matrix for H2. + */ - /// @name Testable - /// @{ - static void Print(SOn R, const std::string& str = "") { - gtsam::print(R, str); - } - static bool Equals(SOn R1, SOn R2, double tol = 1e-8) { - return equal_with_abs_tol(R1, R2, tol); - } - /// @} +using SOn = SO; - /// @name Manifold - /// @{ - enum { dimension = Eigen::Dynamic }; - typedef Eigen::VectorXd TangentVector; - // typedef Eigen::MatrixXd Jacobian; - typedef OptionalJacobian ChartJacobian; - // typedef SOn ManifoldType; +/* + * Define the traits. internal::LieGroup provides both Lie group and Testable + */ - /** - * Calculate manifold dimension, e.g., - * n = 3 -> 3*2/2 = 3 - * n = 4 -> 4*3/2 = 6 - * n = 5 -> 5*4/2 = 10 - */ - static int GetDimension(const SOn& R) { - const size_t n = R.rows(); - return n * (n - 1) / 2; - } +template +struct traits> : public internal::LieGroup> {}; - // static Jacobian Eye(const SOn& R) { - // int dim = GetDimension(R); - // return Eigen::Matrix::Identity(dim, - // dim); - // } - - static SOn Retract(const SOn& R, const TangentVector& xi, // - ChartJacobian H1 = boost::none, - ChartJacobian H2 = boost::none) { - if (H1 || H2) throw std::runtime_error("SOn::Retract"); - const size_t n = R.rows(); - return SOn(R * SOn::Retract(n, xi)); - } - - static TangentVector Local(const SOn& R, const SOn& other, // - ChartJacobian H1 = boost::none, - ChartJacobian H2 = boost::none) { - if (H1 || H2) throw std::runtime_error("SOn::Local"); - const SOn between = SOn(R.inverse() * other); - return SOn::Local(between); - } - - /// @} -}; +template +struct traits> : public internal::LieGroup> {}; } // namespace gtsam diff --git a/gtsam/geometry/SOt.cpp b/gtsam/geometry/SOt.cpp new file mode 100644 index 000000000..64be9acbc --- /dev/null +++ b/gtsam/geometry/SOt.cpp @@ -0,0 +1,281 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SO3.cpp + * @brief 3*3 matrix representation of SO(3) + * @author Frank Dellaert + * @author Luca Carlone + * @author Duy Nguyen Ta + * @date December 2014 + */ + +#include +#include + +#include + +#include +#include +#include + +namespace gtsam { + +//****************************************************************************** +namespace sot { + +Matrix99 Dcompose(const SO3& Q) { + Matrix99 H; + auto R = Q.matrix(); + H << I_3x3 * R(0, 0), I_3x3 * R(1, 0), I_3x3 * R(2, 0), // + I_3x3 * R(0, 1), I_3x3 * R(1, 1), I_3x3 * R(2, 1), // + I_3x3 * R(0, 2), I_3x3 * R(1, 2), I_3x3 * R(2, 2); + return H; +} + +Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) { + Matrix3 MR = M * R.matrix(); + if (H) *H = Dcompose(R); + return MR; +} + +void ExpmapFunctor::init(bool nearZeroApprox) { + nearZero = + nearZeroApprox || (theta2 <= std::numeric_limits::epsilon()); + if (!nearZero) { + theta = std::sqrt(theta2); // rotation angle + sin_theta = std::sin(theta); + const double s2 = std::sin(theta / 2.0); + one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] + } +} + +ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) + : theta2(omega.dot(omega)) { + const double wx = omega.x(), wy = omega.y(), wz = omega.z(); + W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; + init(nearZeroApprox); + if (!nearZero) { + K = W / theta; + KK = K * K; + } +} + +ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, + bool nearZeroApprox) + : theta2(angle * angle) { + const double ax = axis.x(), ay = axis.y(), az = axis.z(); + K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; + W = K * angle; + init(nearZeroApprox); + if (!nearZero) { + KK = K * K; + } +} + +SO3 ExpmapFunctor::expmap() const { + if (nearZero) + return SO3(I_3x3 + W); + else + return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK); +} + +DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) + : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { + if (nearZero) + dexp_ = I_3x3 - 0.5 * W; + else { + a = one_minus_cos / theta; + b = 1.0 - sin_theta / theta; + dexp_ = I_3x3 - a * K + b * KK; + } +} + +Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + if (H1) { + if (nearZero) { + *H1 = 0.5 * skewSymmetric(v); + } else { + // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK + const Vector3 Kv = K * v; + const double Da = (sin_theta - 2.0 * a) / theta2; + const double Db = (one_minus_cos - 3.0 * b) / theta2; + *H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() - + skewSymmetric(Kv * b / theta) + + (a * I_3x3 - b * K) * skewSymmetric(v / theta); + } + } + if (H2) *H2 = dexp_; + return dexp_ * v; +} + +Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + const Matrix3 invDexp = dexp_.inverse(); + const Vector3 c = invDexp * v; + if (H1) { + Matrix3 D_dexpv_omega; + applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping + *H1 = -invDexp * D_dexpv_omega; + } + if (H2) *H2 = invDexp; + return c; +} + +} // namespace sot + +/* ************************************************************************* */ +// SO3 SO3::AxisAngle(const Vector3& axis, double theta) { +// return sot::ExpmapFunctor(axis, theta).expmap(); +// } + +/* ************************************************************************* */ +// SO3 SO3::ClosestTo(const Matrix3& M) { +// Eigen::JacobiSVD svd(M, Eigen::ComputeFullU | +// Eigen::ComputeFullV); const auto& U = svd.matrixU(); const auto& V = +// svd.matrixV(); const double det = (U * V.transpose()).determinant(); return +// U * Vector3(1, 1, det).asDiagonal() * V.transpose(); +// } + +/* ************************************************************************* */ +// SO3 SO3::ChordalMean(const std::vector& rotations) { +// // See Hartley13ijcv: +// // Cost function C(R) = \sum sqr(|R-R_i|_F) +// // Closed form solution = ClosestTo(C_e), where C_e = \sum R_i !!!! +// Matrix3 C_e{Z_3x3}; +// for (const auto& R_i : rotations) { +// C_e += R_i; +// } +// return ClosestTo(C_e); +// } + +//****************************************************************************** +// Matrix3 SO3::Hat(const Vector3& xi) { return skewSymmetric(xi); } + +// /* ************************************************************************* +// */ Vector3 SO3::Vee(const Matrix3& X) { +// Vector3 xi; +// xi(0) = -X(1, 2); +// xi(1) = X(0, 2); +// xi(2) = -X(0, 1); +// return xi; +// } + +template <> +SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { + if (H) { + sot::DexpFunctor impl(omega); + *H = impl.dexp(); + return impl.expmap(); + } else { + return sot::ExpmapFunctor(omega).expmap(); + } +} + +// Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { +// return sot::DexpFunctor(omega).dexp(); +// } + +/* ************************************************************************* */ +// Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { +// using std::sin; +// using std::sqrt; + +// // note switch to base 1 +// const double &R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2); +// const double &R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2); +// const double &R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2); + +// // Get trace(R) +// const double tr = R.trace(); + +// Vector3 omega; + +// // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. +// // we do something special +// if (std::abs(tr + 1.0) < 1e-10) { +// if (std::abs(R33 + 1.0) > 1e-10) +// omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); +// else if (std::abs(R22 + 1.0) > 1e-10) +// omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); +// else +// // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit +// omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); +// } else { +// double magnitude; +// const double tr_3 = tr - 3.0; // always negative +// if (tr_3 < -1e-7) { +// double theta = acos((tr - 1.0) / 2.0); +// magnitude = theta / (2.0 * sin(theta)); +// } else { +// // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) +// // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2) +// magnitude = 0.5 - tr_3 * tr_3 / 12.0; +// } +// omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); +// } + +// if (H) *H = LogmapDerivative(omega); +// return omega; +// } + +/* ************************************************************************* */ +// Matrix3 SO3::LogmapDerivative(const Vector3& omega) { +// using std::cos; +// using std::sin; + +// double theta2 = omega.dot(omega); +// if (theta2 <= std::numeric_limits::epsilon()) return I_3x3; +// double theta = std::sqrt(theta2); // rotation angle +// /** Right Jacobian for Log map in SO(3) - equation (10.86) and following +// * equations in G.S. Chirikjian, "Stochastic Models, Information Theory, +// and +// * Lie Groups", Volume 2, 2008. logmap( Rhat * expmap(omega) ) \approx +// logmap( +// * Rhat ) + Jrinv * omega where Jrinv = LogmapDerivative(omega); This maps +// a +// * perturbation on the manifold (expmap(omega)) to a perturbation in the +// * tangent space (Jrinv * omega) +// */ +// const Matrix3 W = +// skewSymmetric(omega); // element of Lie algebra so(3): W = omega^ +// return I_3x3 + 0.5 * W + +// (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) +// * +// W * W; +// } + +/* ************************************************************************* */ +// static Vector9 vec(const SO3& R) { return Eigen::Map(R.data()); } + +// static const std::vector G({SO3::Hat(Vector3::Unit(0)), +// SO3::Hat(Vector3::Unit(1)), +// SO3::Hat(Vector3::Unit(2))}); + +// static const Matrix93 P = +// (Matrix93() << vec(G[0]), vec(G[1]), vec(G[2])).finished(); + +/* ************************************************************************* */ +// Vector9 SO3::vec(OptionalJacobian<9, 3> H) const { +// const SO3& R = *this; +// if (H) { +// // As Luca calculated (for SO4), this is (I3 \oplus R) * P +// *H << R * P.block<3, 3>(0, 0), R * P.block<3, 3>(3, 0), +// R * P.block<3, 3>(6, 0); +// } +// return gtsam::vec(R); +// }; + +/* ************************************************************************* */ + +} // end namespace gtsam diff --git a/gtsam/geometry/SOt.h b/gtsam/geometry/SOt.h new file mode 100644 index 000000000..0fa8f5776 --- /dev/null +++ b/gtsam/geometry/SOt.h @@ -0,0 +1,204 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SO3.h + * @brief 3*3 matrix representation of SO(3) + * @author Frank Dellaert + * @author Luca Carlone + * @author Duy Nguyen Ta + * @date December 2014 + */ + +#pragma once + +#include + +#include +#include + +#include +#include +#include + +namespace gtsam { + +using SO3 = SO<3>; + +// /// Static, named constructor that finds SO(3) matrix closest to M in +// Frobenius norm. static SO3 ClosestTo(const Matrix3& M); + +// /// Static, named constructor that finds chordal mean = argmin_R \sum +// sqr(|R-R_i|_F). static SO3 ChordalMean(const std::vector& rotations); + +// static Matrix3 Hat(const Vector3 &xi); ///< make skew symmetric matrix +// static Vector3 Vee(const Matrix3 &X); ///< inverse of Hat + +//****************************************************************************** +template <> +SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H); + +// template<> +// Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { +// return sot::DexpFunctor(omega).dexp(); +// } + +/** + * Exponential map at identity - create a rotation from canonical coordinates + * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula + */ +// static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none); + +/// Derivative of Expmap +// static Matrix3 ExpmapDerivative(const Vector3& omega); + +/** + * Log map at identity - returns the canonical coordinates + * \f$ [R_x,R_y,R_z] \f$ of this rotation + */ +// static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none); + +/// Derivative of Logmap +// static Matrix3 LogmapDerivative(const Vector3& omega); + +// Matrix3 AdjointMap() const { +// return *this; +// } + +// // Chart at origin +// struct ChartAtOrigin { +// static SO3 Retract(const Vector3& omega, ChartJacobian H = boost::none) { +// return Expmap(omega, H); +// } +// static Vector3 Local(const SO3& R, ChartJacobian H = boost::none) { +// return Logmap(R, H); +// } +// }; + +//****************************************************************************** +static Vector9 vec3(const Matrix3& R) { + return Eigen::Map(R.data()); +} + +static const std::vector G3({SO3::Hat(Vector3::Unit(0)), + SO3::Hat(Vector3::Unit(1)), + SO3::Hat(Vector3::Unit(2))}); + +static const Matrix93 P3 = + (Matrix93() << vec3(G3[0]), vec3(G3[1]), vec3(G3[2])).finished(); + +//****************************************************************************** +template <> +Vector9 SO3::vec(OptionalJacobian<9, 3> H) const { + const Matrix3& R = matrix_; + if (H) { + // As Luca calculated (for SO4), this is (I3 \oplus R) * P3 + *H << R * P3.block<3, 3>(0, 0), R * P3.block<3, 3>(3, 0), + R * P3.block<3, 3>(6, 0); + } + return gtsam::vec3(R); +} + +// private: + +// /** Serialization function */ +// friend class boost::serialization::access; +// template +// void serialize(ARCHIVE & ar, const unsigned int /*version*/) +// { +// ar & boost::serialization::make_nvp("R11", (*this)(0,0)); +// ar & boost::serialization::make_nvp("R12", (*this)(0,1)); +// ar & boost::serialization::make_nvp("R13", (*this)(0,2)); +// ar & boost::serialization::make_nvp("R21", (*this)(1,0)); +// ar & boost::serialization::make_nvp("R22", (*this)(1,1)); +// ar & boost::serialization::make_nvp("R23", (*this)(1,2)); +// ar & boost::serialization::make_nvp("R31", (*this)(2,0)); +// ar & boost::serialization::make_nvp("R32", (*this)(2,1)); +// ar & boost::serialization::make_nvp("R33", (*this)(2,2)); +// } + +namespace sot { + +/** + * Compose general matrix with an SO(3) element. + * We only provide the 9*9 derivative in the first argument M. + */ +Matrix3 compose(const Matrix3& M, const SO3& R, + OptionalJacobian<9, 9> H = boost::none); + +/// (constant) Jacobian of compose wrpt M +Matrix99 Dcompose(const SO3& R); + +// Below are two functors that allow for saving computation when exponential map +// and its derivatives are needed at the same location in so<3>. The second +// functor also implements dedicated methods to apply dexp and/or inv(dexp). + +/// Functor implementing Exponential map +class GTSAM_EXPORT ExpmapFunctor { + protected: + const double theta2; + Matrix3 W, K, KK; + bool nearZero; + double theta, sin_theta, one_minus_cos; // only defined if !nearZero + + void init(bool nearZeroApprox = false); + + public: + /// Constructor with element of Lie algebra so(3) + explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false); + + /// Constructor with axis-angle + ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false); + + /// Rodrigues formula + SO3 expmap() const; +}; + +/// Functor that implements Exponential map *and* its derivatives +class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { + const Vector3 omega; + double a, b; + Matrix3 dexp_; + + public: + /// Constructor with element of Lie algebra so(3) + explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); + + // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation + // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models, + // Information Theory, and Lie Groups", Volume 2, 2008. + // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v) + // This maps a perturbation v in the tangent space to + // a perturbation on the manifold Expmap(dexp * v) */ + const Matrix3& dexp() const { return dexp_; } + + /// Multiplies with dexp(), with optional derivatives + Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; + + /// Multiplies with dexp().inverse(), with optional derivatives + Vector3 applyInvDexp(const Vector3& v, + OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; +}; +} // namespace sot + +/* + * Define the traits. internal::LieGroup provides both Lie group and Testable + */ + +template <> +struct traits : public internal::LieGroup {}; + +template <> +struct traits : public internal::LieGroup {}; + +} // end namespace gtsam diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index 9f17ce269..30f20b495 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -10,16 +10,25 @@ * -------------------------------------------------------------------------- */ /** - * @file testSOnBase.cpp - * @brief Unit tests for Base class of SO(n) classes. + * @file testSOn.cpp + * @brief Unit tests for dynamic SO(n) classes. * @author Frank Dellaert **/ +#include +#include +#include + #include #include #include #include -#include +#include +#include +#include +#include + +#include #include @@ -28,325 +37,6 @@ #include namespace gtsam { - -namespace internal { -/// Calculate dimensionality of SO manifold, or return Dynamic if so -constexpr int DimensionSO(int N) { - return (N < 0) ? Eigen::Dynamic : N * (N - 1) / 2; -} - -// Calculate N^2 at compile time, or return Dynamic if so -constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; } -} // namespace internal - -/** - * Space of special orthogonal rotation matrices SO - */ -template -class SO : public LieGroup, internal::DimensionSO(N)> { - public: - enum { dimension = internal::DimensionSO(N) }; - using MatrixNN = Eigen::Matrix; - using VectorN2 = Eigen::Matrix; - using MatrixDD = Eigen::Matrix; - - protected: - MatrixNN matrix_; ///< Rotation matrix - - // enable_if_t aliases, used to specialize constructors/methods, see - // https://www.fluentcpp.com/2018/05/18/make-sfinae-pretty-2-hidden-beauty-sfinae/ - template - using IsDynamic = boost::enable_if_t; - template - using IsFixed = boost::enable_if_t= 2, void>; - template - using IsSO3 = boost::enable_if_t; - - public: - /// @name Constructors - /// @{ - - /// Construct SO identity for N >= 2 - template > - SO() : matrix_(MatrixNN::Identity()) {} - - /// Construct SO identity for N == Eigen::Dynamic - template > - explicit SO(size_t n = 0) { - if (n == 0) throw std::runtime_error("SO: Dimensionality not known."); - matrix_ = Eigen::MatrixXd::Identity(n, n); - } - - /// Constructor from Eigen Matrix - template - explicit SO(const Eigen::MatrixBase& R) : matrix_(R.eval()) {} - - /// Constructor from AngleAxisd - template > - SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {} - - /// Random SO(n) element (no big claims about uniformity) - template > - static SO Random(boost::mt19937& rng, size_t n = 0) { - if (n == 0) throw std::runtime_error("SO: Dimensionality not known."); - // This needs to be re-thought! - static boost::uniform_real randomAngle(-M_PI, M_PI); - const size_t d = SO::Dimension(n); - Vector xi(d); - for (size_t j = 0; j < d; j++) { - xi(j) = randomAngle(rng); - } - return SO::Retract(xi); - } - - /// Random SO(N) element (no big claims about uniformity) - template > - static SO Random(boost::mt19937& rng) { - // By default, use dynamic implementation above. Specialized for SO(3). - return SO(SO::Random(rng, N).matrix()); - } - - /// @} - /// @name Standard methods - /// @{ - - /// Return matrix - const MatrixNN& matrix() const { return matrix_; } - - size_t rows() const { return matrix_.rows(); } - size_t cols() const { return matrix_.cols(); } - - /// @} - /// @name Testable - /// @{ - - void print(const std::string& s) const { - std::cout << s << matrix_ << std::endl; - } - - bool equals(const SO& other, double tol) const { - return equal_with_abs_tol(matrix_, other.matrix_, tol); - } - - /// @} - /// @name Group - /// @{ - - /// Multiplication - SO operator*(const SO& other) const { return SO(matrix_ * other.matrix_); } - - /// SO identity for N >= 2 - template > - static SO identity() { - return SO(); - } - - /// SO identity for N == Eigen::Dynamic - template > - static SO identity(size_t n = 0) { - return SO(n); - } - - /// inverse of a rotation = transpose - SO inverse() const { return SO(matrix_.transpose()); } - - /// @} - /// @name Manifold - /// @{ - - using TangentVector = Eigen::Matrix; - using ChartJacobian = OptionalJacobian; - - /// Return compile-time dimensionality: fixed size N or Eigen::Dynamic - static int Dim() { return dimension; } - - // Calculate manifold dimensionality for SO(n). - // Available as dimension or Dim() for fixed N. - static size_t Dimension(size_t n) { return n * (n - 1) / 2; } - - // Calculate ambient dimension n from manifold dimensionality d. - static size_t AmbientDim(size_t d) { return (1 + std::sqrt(1 + 8 * d)) / 2; } - - // Calculate run-time dimensionality of manifold. - // Available as dimension or Dim() for fixed N. - size_t dim() const { return Dimension(matrix_.rows()); } - - /** - * 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 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: - * 0 -z y -w d - * z 0 -x v -c - * -y x 0 -u b - * w -v u 0 -a - * -d c -b a 0 - * This scheme behaves exactly as expected for SO(2) and SO(3). - */ - static Matrix Hat(const Vector& xi) { - size_t n = AmbientDim(xi.size()); - if (n < 2) throw std::invalid_argument("SOn::Hat: n<2 not supported"); - - Matrix X(n, n); // allocate space for n*n skew-symmetric matrix - X.setZero(); - if (n == 2) { - // Handle SO(2) case as recursion bottom - assert(xi.size() == 1); - X << 0, -xi(0), xi(0), 0; - } else { - // Recursively call SO(n-1) call for top-left block - const size_t dmin = (n - 1) * (n - 2) / 2; - X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin)); - - // Now fill last row and column - double sign = 1.0; - for (size_t i = 0; i < n - 1; i++) { - const size_t j = n - 2 - i; - X(n - 1, j) = sign * xi(i); - X(j, n - 1) = -X(n - 1, j); - sign = -sign; - } - } - return X; - } - - /** - * Inverse of Hat. See note about xi element order in Hat. - */ - static Vector Vee(const Matrix& X) { - const size_t n = X.rows(); - if (n < 2) throw std::invalid_argument("SOn::Hat: n<2 not supported"); - - if (n == 2) { - // Handle SO(2) case as recursion bottom - Vector xi(1); - xi(0) = X(1, 0); - return xi; - } else { - // Calculate dimension and allocate space - const size_t d = n * (n - 1) / 2; - Vector xi(d); - - // Fill first n-1 spots from last row of X - double sign = 1.0; - for (size_t i = 0; i < n - 1; i++) { - const size_t j = n - 2 - i; - xi(i) = sign * X(n - 1, j); - sign = -sign; - } - - // Recursively call Vee to fill remainder of x - const size_t dmin = (n - 1) * (n - 2) / 2; - xi.tail(dmin) = Vee(X.topLeftCorner(n - 1, n - 1)); - return xi; - } - } - - // Chart at origin - struct ChartAtOrigin { - /** - * Retract uses Cayley map. See note about xi element order in Hat. - * Deafault implementation has no Jacobian implemented - */ - static SO Retract(const TangentVector& xi, ChartJacobian H = boost::none) { - const Matrix X = Hat(xi / 2.0); - size_t n = AmbientDim(xi.size()); - const auto I = Eigen::MatrixXd::Identity(n, n); - return SO((I + X) * (I - X).inverse()); - } - /** - * Inverse of Retract. See note about xi element order in Hat. - */ - static TangentVector Local(const SO& R, ChartJacobian H = boost::none) { - const size_t n = R.rows(); - const auto I = Eigen::MatrixXd::Identity(n, n); - const Matrix X = (I - R.matrix_) * (I + R.matrix_).inverse(); - return -2 * Vee(X); - } - }; - - // Return dynamic identity DxD Jacobian for given SO(n) - template > - static MatrixDD IdentityJacobian(size_t n) { - const size_t d = Dimension(n); - return MatrixDD::Identity(d, d); - } - - /// @} - /// @name Lie Group - /// @{ - - MatrixDD AdjointMap() const { - throw std::runtime_error( - "SO::AdjointMap only implemented for SO3 and SO4."); - } - - /** - * Exponential map at identity - create a rotation from canonical coordinates - */ - static SO Expmap(const TangentVector& omega, ChartJacobian H = boost::none) { - throw std::runtime_error("SO::Expmap only implemented for SO3 and SO4."); - } - - /** - * Log map at identity - returns the canonical coordinates of this rotation - */ - static TangentVector Logmap(const SO& R, ChartJacobian H = boost::none) { - throw std::runtime_error("SO::Logmap only implemented for SO3 and SO4."); - } - - // inverse with optional derivative - using LieGroup, internal::DimensionSO(N)>::inverse; - - /// @} - /// @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 = rows(); - const size_t n2 = n * n; - - // Vectorize - VectorN2 X(n2); - X << Eigen::Map(matrix_.data(), n2, 1); - - // If requested, calculate H as (I \oplus Q) * P - if (H) { - // Calculate P matrix of vectorized generators - const size_t d = dim(); - Matrix P(n2, d); - for (size_t j = 0; j < d; j++) { - const auto X = Hat(Eigen::VectorXd::Unit(d, j)); - P.col(j) = Eigen::Map(X.data(), n2, 1); - } - H->resize(n2, d); - for (size_t i = 0; i < n; i++) { - H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d); - } - } - return X; - } - /// @} -}; - -/* - * Fully specialize compose and between, because the derivative is unknowable by - * the LieGroup implementations, who return a fixed-size matrix for H2. - */ - -using SO3 = SO<3>; -using SO4 = SO<4>; -using SOn = SO; - using DynamicJacobian = OptionalJacobian; template <> @@ -365,336 +55,8 @@ SOn LieGroup::between(const SOn& g, DynamicJacobian H1, if (H2) *H2 = SOn::IdentityJacobian(g.rows()); return result; } - -/* - * Define the traits. internal::LieGroup provides both Lie group and Testable - */ - -template -struct traits> : public internal::LieGroup> {}; - -template -struct traits> : public internal::LieGroup> {}; - -namespace so3 { - -/** - * Compose general matrix with an SO(3) element. - * We only provide the 9*9 derivative in the first argument M. - */ -Matrix3 compose(const Matrix3& M, const SO3& R, - OptionalJacobian<9, 9> H = boost::none); - -/// (constant) Jacobian of compose wrpt M -Matrix99 Dcompose(const SO3& R); - -// Below are two functors that allow for saving computation when exponential map -// and its derivatives are needed at the same location in so<3>. The second -// functor also implements dedicated methods to apply dexp and/or inv(dexp). - -/// Functor implementing Exponential map -class GTSAM_EXPORT ExpmapFunctor { - protected: - const double theta2; - Matrix3 W, K, KK; - bool nearZero; - double theta, sin_theta, one_minus_cos; // only defined if !nearZero - - void init(bool nearZeroApprox = false); - - public: - /// Constructor with element of Lie algebra so(3) - ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false); - - /// Constructor with axis-angle - ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false); - - /// Rodrigues formula - SO3 expmap() const; -}; - -/// Functor that implements Exponential map *and* its derivatives -class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { - const Vector3 omega; - double a, b; - Matrix3 dexp_; - - public: - /// Constructor with element of Lie algebra so(3) - DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); - - // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation - // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models, - // Information Theory, and Lie Groups", Volume 2, 2008. - // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v) - // This maps a perturbation v in the tangent space to - // a perturbation on the manifold Expmap(dexp * v) */ - const Matrix3& dexp() const { return dexp_; } - - /// Multiplies with dexp(), with optional derivatives - Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const; - - /// Multiplies with dexp().inverse(), with optional derivatives - Vector3 applyInvDexp(const Vector3& v, - OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const; -}; -} // namespace so3 - -//****************************************************************************** -namespace so3 { - -Matrix99 Dcompose(const SO3& Q) { - Matrix99 H; - auto R = Q.matrix(); - H << I_3x3 * R(0, 0), I_3x3 * R(1, 0), I_3x3 * R(2, 0), // - I_3x3 * R(0, 1), I_3x3 * R(1, 1), I_3x3 * R(2, 1), // - I_3x3 * R(0, 2), I_3x3 * R(1, 2), I_3x3 * R(2, 2); - return H; -} - -Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) { - Matrix3 MR = M * R.matrix(); - if (H) *H = Dcompose(R); - return MR; -} - -void ExpmapFunctor::init(bool nearZeroApprox) { - nearZero = - nearZeroApprox || (theta2 <= std::numeric_limits::epsilon()); - if (!nearZero) { - theta = std::sqrt(theta2); // rotation angle - sin_theta = std::sin(theta); - const double s2 = std::sin(theta / 2.0); - one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] - } -} - -ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) - : theta2(omega.dot(omega)) { - const double wx = omega.x(), wy = omega.y(), wz = omega.z(); - W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; - init(nearZeroApprox); - if (!nearZero) { - K = W / theta; - KK = K * K; - } -} - -ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, - bool nearZeroApprox) - : theta2(angle * angle) { - const double ax = axis.x(), ay = axis.y(), az = axis.z(); - K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; - W = K * angle; - init(nearZeroApprox); - if (!nearZero) { - KK = K * K; - } -} - -SO3 ExpmapFunctor::expmap() const { - if (nearZero) - return SO3(I_3x3 + W); - else - return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK); -} - -DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) - : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { - if (nearZero) - dexp_ = I_3x3 - 0.5 * W; - else { - a = one_minus_cos / theta; - b = 1.0 - sin_theta / theta; - dexp_ = I_3x3 - a * K + b * KK; - } -} - -Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, - OptionalJacobian<3, 3> H2) const { - if (H1) { - if (nearZero) { - *H1 = 0.5 * skewSymmetric(v); - } else { - // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK - const Vector3 Kv = K * v; - const double Da = (sin_theta - 2.0 * a) / theta2; - const double Db = (one_minus_cos - 3.0 * b) / theta2; - *H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() - - skewSymmetric(Kv * b / theta) + - (a * I_3x3 - b * K) * skewSymmetric(v / theta); - } - } - if (H2) *H2 = dexp_; - return dexp_ * v; -} - -Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, - OptionalJacobian<3, 3> H2) const { - const Matrix3 invDexp = dexp_.inverse(); - const Vector3 c = invDexp * v; - if (H1) { - Matrix3 D_dexpv_omega; - applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping - *H1 = -invDexp * D_dexpv_omega; - } - if (H2) *H2 = invDexp; - return c; -} - -} // namespace so3 - -//****************************************************************************** -template <> -SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { - if (H) { - so3::DexpFunctor impl(omega); - *H = impl.dexp(); - return impl.expmap(); - } else - return so3::ExpmapFunctor(omega).expmap(); -} - -// template<> -// Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { -// return so3::DexpFunctor(omega).dexp(); -// } - -//****************************************************************************** -static Vector9 vec3(const Matrix3& R) { - return Eigen::Map(R.data()); -} - -static const std::vector G3({SO3::Hat(Vector3::Unit(0)), - SO3::Hat(Vector3::Unit(1)), - SO3::Hat(Vector3::Unit(2))}); - -static const Matrix93 P3 = - (Matrix93() << vec3(G3[0]), vec3(G3[1]), vec3(G3[2])).finished(); - -//****************************************************************************** -template <> -Vector9 SO3::vec(OptionalJacobian<9, 3> H) const { - const Matrix3& R = matrix_; - if (H) { - // As Luca calculated (for SO4), this is (I3 \oplus R) * P3 - *H << R * P3.block<3, 3>(0, 0), R * P3.block<3, 3>(3, 0), - R * P3.block<3, 3>(6, 0); - } - return gtsam::vec3(R); -}; - -//****************************************************************************** -/* Exponential map, porting MATLAB implementation by Luca, which follows - * "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by - * Ramona-Andreaa Rohan */ -template <> -SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) { - using namespace std; - if (H) throw std::runtime_error("SO4::Expmap Jacobian"); - - // skew symmetric matrix X = xi^ - const Matrix4 X = Hat(xi); - - // do eigen-decomposition - auto eig = Eigen::EigenSolver(X); - Eigen::Vector4cd e = eig.eigenvalues(); - using std::abs; - sort(e.data(), e.data() + 4, [](complex a, complex b) { - return abs(a.imag()) > abs(b.imag()); - }); - - // Get a and b from eigenvalues +/i ai and +/- bi - double a = e[0].imag(), b = e[2].imag(); - if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) { - throw runtime_error("SO4::Expmap: wrong eigenvalues."); - } - - // Build expX = exp(xi^) - Matrix4 expX; - using std::cos; - using std::sin; - const auto X2 = X * X; - const auto X3 = X2 * X; - double a2 = a * a, a3 = a2 * a, b2 = b * b, b3 = b2 * b; - if (a != 0 && b == 0) { - double c2 = (1 - cos(a)) / a2, c3 = (a - sin(a)) / a3; - return SO4(I_4x4 + X + c2 * X2 + c3 * X3); - } else if (a == b && b != 0) { - double sin_a = sin(a), cos_a = cos(a); - double c0 = (a * sin_a + 2 * cos_a) / 2, - c1 = (3 * sin_a - a * cos_a) / (2 * a), c2 = sin_a / (2 * a), - c3 = (sin_a - a * cos_a) / (2 * a3); - return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3); - } else if (a != b) { - double sin_a = sin(a), cos_a = cos(a); - double sin_b = sin(b), cos_b = cos(b); - double c0 = (b2 * cos_a - a2 * cos_b) / (b2 - a2), - c1 = (b3 * sin_a - a3 * sin_b) / (a * b * (b2 - a2)), - c2 = (cos_a - cos_b) / (b2 - a2), - c3 = (b * sin_a - a * sin_b) / (a * b * (b2 - a2)); - return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3); - } else { - return SO4(); - } -} - -//****************************************************************************** -static SO4::VectorN2 vec4(const Matrix4& Q) { - return Eigen::Map(Q.data()); -} - -static const std::vector G4( - {SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)), - SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)), - SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))}); - -static const Eigen::Matrix P4 = - (Eigen::Matrix() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]), - vec4(G4[3]), vec4(G4[4]), vec4(G4[5])) - .finished(); - -//****************************************************************************** -template <> -Matrix6 SO4::AdjointMap() const { - // Elaborate way of calculating the AdjointMap - // TODO(frank): find a closed form solution. In SO(3) is just R :-/ - const Matrix4& Q = matrix_; - const Matrix4 Qt = Q.transpose(); - Matrix6 A; - for (size_t i = 0; i < 6; i++) { - // Calculate column i of linear map for coeffcient of Gi - A.col(i) = SO4::Vee(Q * G4[i] * Qt); - } - return A; -} - -//****************************************************************************** -template <> -SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const { - const Matrix& Q = matrix_; - if (H) { - // As Luca calculated, this is (I4 \oplus Q) * P4 - *H << Q * P4.block<4, 6>(0, 0), Q * P4.block<4, 6>(4, 0), - Q * P4.block<4, 6>(8, 0), Q * P4.block<4, 6>(12, 0); - } - return gtsam::vec4(Q); -} - } // namespace gtsam -#include -#include -#include -// #include -// #include -// #include -#include - -#include - using namespace std; using namespace gtsam; @@ -1130,11 +492,11 @@ TEST(SO3, ApplyDexp) { for (bool nearZeroApprox : {true, false}) { boost::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v); + return sot::DexpFunctor(omega, nearZeroApprox).applyDexp(v); }; for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { - so3::DexpFunctor local(omega, nearZeroApprox); + sot::DexpFunctor local(omega, nearZeroApprox); for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), Vector3(0.4, 0.3, 0.2)}) { EXPECT(assert_equal(Vector3(local.dexp() * v), @@ -1153,11 +515,11 @@ TEST(SO3, ApplyInvDexp) { for (bool nearZeroApprox : {true, false}) { boost::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v); + return sot::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v); }; for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { - so3::DexpFunctor local(omega, nearZeroApprox); + sot::DexpFunctor local(omega, nearZeroApprox); Matrix invDexp = local.dexp().inverse(); for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), Vector3(0.4, 0.3, 0.2)}) { @@ -1189,10 +551,10 @@ TEST(SO3, vec) { // SO3 R = SO3::Expmap(Vector3(1, 2, 3)); // const Matrix3 expected = M * R.matrix(); // Matrix actualH; -// const Matrix3 actual = so3::compose(M, R, actualH); +// const Matrix3 actual = sot::compose(M, R, actualH); // CHECK(assert_equal(expected, actual)); // boost::function f = [R](const Matrix3& M) { -// return so3::compose(M, R); +// return sot::compose(M, R); // }; // Matrix numericalH = numericalDerivative11(f, M, 1e-2); // CHECK(assert_equal(numericalH, actualH));