From 6e076363026aeb377266f6d3a5bbffa182e50652 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Apr 2019 23:45:27 -0400 Subject: [PATCH 01/47] Added several methods --- gtsam/geometry/SO3.cpp | 72 ++++++++++++++++++++++++++++++++ gtsam/geometry/SO3.h | 66 +++++++++++++++++++++++++---- gtsam/geometry/tests/testSO3.cpp | 31 +++++++++++++- 3 files changed, 159 insertions(+), 10 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 034956e99..cd321b126 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -20,14 +20,32 @@ #include #include + +#include + #include #include #include namespace gtsam { +/* ************************************************************************* */ namespace so3 { +Matrix99 Dcompose(const SO3& R) { + Matrix99 H; + 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) { @@ -116,11 +134,44 @@ SO3 SO3::AxisAngle(const Vector3& axis, double theta) { return so3::ExpmapFunctor(axis, theta).expmap(); } +/* ************************************************************************* */ +SO3 SO3::ClosestTo(const Matrix3& M) { + Eigen::JacobiSVD svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV); + 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); +} + /* ************************************************************************* */ void SO3::print(const std::string& s) const { std::cout << s << *this << std::endl; } +//****************************************************************************** + 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; +} + /* ************************************************************************* */ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { if (H) { @@ -199,6 +250,27 @@ Matrix3 SO3::LogmapDerivative(const Vector3& omega) { 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/SO3.h b/gtsam/geometry/SO3.h index 5f1c7d1bf..fd0d6f1e1 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -38,14 +38,13 @@ class SO3: public Matrix3, public LieGroup { protected: public: - enum { - dimension = 3 - }; + enum { N = 3 }; + enum { dimension = 3 }; /// @name Constructors /// @{ - /// Constructor from AngleAxisd + /// Default constructor creates identity SO3() : Matrix3(I_3x3) { } @@ -61,9 +60,15 @@ public: Matrix3(angleAxis) { } - /// Static, named constructor TODO think about relation with above + /// Static, named constructor. TODO(dellaert): think about relation with above GTSAM_EXPORT static SO3 AxisAngle(const Vector3& axis, double theta); + /// 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); + /// @} /// @name Testable /// @{ @@ -85,13 +90,16 @@ public: /// inverse of a rotation = transpose SO3 inverse() const { - return this->Matrix3::inverse(); + return this->transpose(); } /// @} /// @name Lie Group /// @{ + static Matrix3 Hat(const Vector3 &xi); ///< make skew symmetric matrix + static Vector3 Vee(const Matrix3 &X); ///< inverse of Hat + /** * Exponential map at identity - create a rotation from canonical coordinates * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula @@ -127,13 +135,53 @@ public: using LieGroup::inverse; /// @} + /// @name Other methods + /// @{ + + /// Vectorize + Vector9 vec(OptionalJacobian<9, 3> H = boost::none) const; + + /// Return matrix (for wrapper) + const Matrix3& 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("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)); + } + }; -// This namespace exposes 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) 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: diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 56751fa06..94c130f9f 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testQuaternion.cpp + * @file testSO3.cpp * @brief Unit tests for SO3, as a GTSAM-adapted Lie Group * @author Frank Dellaert **/ @@ -278,6 +278,35 @@ TEST(SO3, ApplyInvDexp) { } } +/* ************************************************************************* */ +TEST(SO3, vec) { + const Vector9 expected = Eigen::Map(R2.data()); + Matrix actualH; + const Vector9 actual = R2.vec(actualH); + CHECK(assert_equal(expected, actual)); + boost::function f = [](const SO3& Q) { + return Q.vec(); + }; + const Matrix numericalH = numericalDerivative11(f, R2, 1e-5); + CHECK(assert_equal(numericalH, actualH)); +} + +//****************************************************************************** +TEST(Matrix, compose) { + Matrix3 M; + M << 1, 2, 3, 4, 5, 6, 7, 8, 9; + SO3 R = SO3::Expmap(Vector3(1, 2, 3)); + const Matrix3 expected = M * R.matrix(); + Matrix actualH; + const Matrix3 actual = so3::compose(M, R, actualH); + CHECK(assert_equal(expected, actual)); + boost::function f = [R](const Matrix3& M) { + return so3::compose(M, R); + }; + Matrix numericalH = numericalDerivative11(f, M, 1e-2); + CHECK(assert_equal(numericalH, actualH)); +} + //****************************************************************************** int main() { TestResult tr; From cc9b4bb497807283d180b657a6701e6a80fcedc2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Apr 2019 23:45:53 -0400 Subject: [PATCH 02/47] Added SO(4) class --- gtsam/geometry/SO4.cpp | 226 +++++++++++++++++++++++++++++++ gtsam/geometry/SO4.h | 146 ++++++++++++++++++++ gtsam/geometry/tests/testSO4.cpp | 172 +++++++++++++++++++++++ 3 files changed, 544 insertions(+) create mode 100644 gtsam/geometry/SO4.cpp create mode 100644 gtsam/geometry/SO4.h create mode 100644 gtsam/geometry/tests/testSO4.cpp diff --git a/gtsam/geometry/SO4.cpp b/gtsam/geometry/SO4.cpp new file mode 100644 index 000000000..ebbc91c01 --- /dev/null +++ b/gtsam/geometry/SO4.cpp @@ -0,0 +1,226 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, 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 SO4.cpp + * @brief 4*4 matrix representation of SO(4) + * @author Frank Dellaert + * @author Luca Carlone + */ + +#include +#include +#include +#include + +#include +#include + +#include +#include + +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); +} + +/* ************************************************************************* */ +// 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; } + +/* ************************************************************************* */ +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; +} + +//****************************************************************************** +/* 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); + + // 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 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; + } +} + +//****************************************************************************** +Vector6 SO4::Logmap(const SO4 &Q, ChartJacobian H) { + if (H) throw std::runtime_error("SO4::Logmap Jacobian"); + throw std::runtime_error("SO4::Logmap"); +} + +/* ************************************************************************* */ +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(); +} + +/* ************************************************************************* */ +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 SO4::Vector16 vec(const SO4 &Q) { + return Eigen::Map(Q.data()); +} + +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))}); + +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; +} + +/* ************************************************************************* */ +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); +}; + +/* ************************************************************************* */ +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 new file mode 100644 index 000000000..e270c9555 --- /dev/null +++ b/gtsam/geometry/SO4.h @@ -0,0 +1,146 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, 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 SO4.h + * @brief 4*4 matrix representation of SO(4) + * @author Frank Dellaert + * @author Luca Carlone + * @date March 2019 + */ + +#pragma once + +#include +#include +#include +#include + +#include + +#include +#include + +namespace gtsam { + +/** + * True SO(4), i.e., 4*4 matrix subgroup + */ +class SO4 : public Matrix4, public LieGroup { + public: + enum { N = 4 }; + enum { dimension = 6 }; + + typedef Eigen::Matrix Vector16; + + /// @name Constructors + /// @{ + + /// Default constructor creates identity + SO4() : Matrix4(I_4x4) {} + + /// Constructor from Eigen Matrix + template + SO4(const MatrixBase &R) : Matrix4(R.eval()) {} + + /// Random SO(4) element (no big claims about uniformity) + static SO4 Random(boost::mt19937 &rng); + + /// @} + /// @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)); + } + +}; // SO4 + +template <> +struct traits : Testable, internal::LieGroupTraits {}; + +template <> +struct traits : Testable, internal::LieGroupTraits {}; + +} // end namespace gtsam diff --git a/gtsam/geometry/tests/testSO4.cpp b/gtsam/geometry/tests/testSO4.cpp new file mode 100644 index 000000000..037280136 --- /dev/null +++ b/gtsam/geometry/tests/testSO4.cpp @@ -0,0 +1,172 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSO4.cpp + * @brief Unit tests for SO4, as a GTSAM-adapted Lie Group + * @author Frank Dellaert + **/ + +#include +#include +#include +#include +#include +#include + + +#include + +#include + +using namespace std; +using namespace gtsam; + +//****************************************************************************** +TEST(SO4, Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +SO4 id; +Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished(); +SO4 Q1 = SO4::Expmap(v1); +Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.00, 0.00, 0.00).finished(); +SO4 Q2 = SO4::Expmap(v2); +Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished(); +SO4 Q3 = SO4::Expmap(v3); + +//****************************************************************************** +TEST(SO4, Random) { + boost::mt19937 rng(42); + auto Q = SO4::Random(rng); + EXPECT_LONGS_EQUAL(4, Q.matrix().rows()); +} +//****************************************************************************** +TEST(SO4, Expmap) { + // If we do exponential map in SO(3) subgroup, topleft should be equal to R1. + auto R1 = SO3::Expmap(v1.head<3>()); + EXPECT((Q1.topLeft().isApprox(R1))); + + // Same here + auto R2 = SO3::Expmap(v2.head<3>()); + EXPECT((Q2.topLeft().isApprox(R2))); + + // Check commutative subgroups + for (size_t i = 0; i < 6; i++) { + Vector6 xi = Vector6::Zero(); + xi[i] = 2; + SO4 Q1 = SO4::Expmap(xi); + xi[i] = 3; + SO4 Q2 = SO4::Expmap(xi); + EXPECT(((Q1 * Q2).isApprox(Q2 * Q1))); + } +} + +//****************************************************************************** +TEST(SO4, Cayley) { + CHECK(assert_equal(id.retract(v1 / 100), SO4::Expmap(v1 / 100))); + CHECK(assert_equal(id.retract(v2 / 100), SO4::Expmap(v2 / 100))); +} + +//****************************************************************************** +TEST(SO4, Retract) { + auto v = Vector6::Zero(); + SO4 actual = traits::Retract(id, v); + EXPECT(actual.isApprox(id)); +} + +//****************************************************************************** +TEST(SO4, Local) { + auto v0 = Vector6::Zero(); + Vector6 actual = traits::Local(id, id); + EXPECT(assert_equal((Vector)v0, actual)); +} + +//****************************************************************************** +TEST(SO4, Invariants) { + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, Q1)); + EXPECT(check_group_invariants(Q2, id)); + EXPECT(check_group_invariants(Q2, Q1)); + EXPECT(check_group_invariants(Q1, Q2)); + + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, Q1)); + EXPECT(check_manifold_invariants(Q2, id)); + EXPECT(check_manifold_invariants(Q2, Q1)); + EXPECT(check_manifold_invariants(Q1, Q2)); +} + +/* ************************************************************************* */ +TEST(SO4, compose) { + SO4 expected = Q1 * Q2; + Matrix actualH1, actualH2; + SO4 actual = Q1.compose(Q2, actualH1, actualH2); + CHECK(assert_equal(expected, actual)); + + Matrix numericalH1 = + numericalDerivative21(testing::compose, Q1, Q2, 1e-2); + CHECK(assert_equal(numericalH1, actualH1)); + + Matrix numericalH2 = + numericalDerivative22(testing::compose, Q1, Q2, 1e-2); + CHECK(assert_equal(numericalH2, actualH2)); +} + +/* ************************************************************************* */ +TEST(SO4, vec) { + using Vector16 = SO4::Vector16; + const Vector16 expected = Eigen::Map(Q2.data()); + Matrix actualH; + const Vector16 actual = Q2.vec(actualH); + CHECK(assert_equal(expected, actual)); + boost::function f = [](const SO4& Q) { + return Q.vec(); + }; + const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5); + CHECK(assert_equal(numericalH, actualH)); +} + +/* ************************************************************************* */ +TEST(SO4, topLeft) { + const Matrix3 expected = Q3.topLeftCorner<3, 3>(); + Matrix actualH; + const Matrix3 actual = Q3.topLeft(actualH); + CHECK(assert_equal(expected, actual)); + boost::function f = [](const SO4& Q3) { + return Q3.topLeft(); + }; + const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); + CHECK(assert_equal(numericalH, actualH)); +} + +/* ************************************************************************* */ +TEST(SO4, stiefel) { + const Matrix43 expected = Q3.leftCols<3>(); + Matrix actualH; + const Matrix43 actual = Q3.stiefel(actualH); + CHECK(assert_equal(expected, actual)); + boost::function f = [](const SO4& Q3) { + return Q3.stiefel(); + }; + const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); + CHECK(assert_equal(numericalH, actualH)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** From 137afaecbba3c09e0450579434bf97083e335f45 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Apr 2019 23:46:08 -0400 Subject: [PATCH 03/47] Added variable dimension SO(n) class --- gtsam/geometry/SOn.h | 291 +++++++++++++++++++++++++++++++ gtsam/geometry/tests/testSOn.cpp | 109 ++++++++++++ 2 files changed, 400 insertions(+) create mode 100644 gtsam/geometry/SOn.h create mode 100644 gtsam/geometry/tests/testSOn.cpp diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h new file mode 100644 index 000000000..bc6282fe4 --- /dev/null +++ b/gtsam/geometry/SOn.h @@ -0,0 +1,291 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, 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 SOn.h + * @brief n*n matrix representation of SO(n) + * @author Frank Dellaert + * @date March 2019 + */ + +#pragma once + +#include + +#include + +#include + +#include + +namespace gtsam { +namespace internal { +// Calculate N^2 at compile time, or return Dynamic if so +constexpr int VecSize(int N) { return (N < 0) ? Eigen::Dynamic : N * N; } +} // namespace internal + +/** + * Base class for rotation group objects. Template arguments: + * Derived: derived class + * 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 + */ +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 + /// @{ + + /// @} + /// @name Lie Group + /// @{ + + /// @} + /// @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. + G.col(j) << Derived::Hat(n, Eigen::VectorXd::Unit(d, j)); + } + + // Vectorize + Vector X(n2); + X << derived(); + + // 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 { + public: + enum { N = Eigen::Dynamic }; + enum { dimension = Eigen::Dynamic }; + + /// @name Constructors + /// @{ + + /// Construct SO(n) identity + SOn(size_t n) : Eigen::MatrixXd(n, n) { + *this << Eigen::MatrixXd::Identity(n, n); + } + + /// Constructor from Eigen Matrix + template + 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) { + // This needs to be re-thought! + static boost::uniform_real randomAngle(-M_PI, M_PI); + const size_t d = n * (n - 1) / 2; + Vector xi(d); + for (size_t j = 0; j < d; j++) { + xi(j) = randomAngle(rng); + } + return SOn::Retract(n, xi); + } + + /// @} + /// @name Manifold + /// @{ + + /** + * 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) + * 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(size_t n, const Vector& xi) { + 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(n - 1, 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; + } + } + + /** + * 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 (I + X) * (I - X).inverse(); + } + + /** + * 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); + } + + /// @} + /// @name Lie group + /// @{ + + /// @} +}; + +template <> +struct traits { + typedef manifold_tag structure_category; + + /// @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); + } + /// @} + + /// @name Manifold + /// @{ + enum { dimension = Eigen::Dynamic }; + typedef Eigen::VectorXd TangentVector; + // typedef Eigen::MatrixXd Jacobian; + typedef OptionalJacobian ChartJacobian; + // typedef SOn ManifoldType; + + /** + * 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; + } + + // 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 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"); + Matrix between = R.inverse() * other; + return SOn::Local(between); + } + + /// @} +}; + +} // namespace gtsam diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp new file mode 100644 index 000000000..868833ce1 --- /dev/null +++ b/gtsam/geometry/tests/testSOn.cpp @@ -0,0 +1,109 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSOnBase.cpp + * @brief Unit tests for Base class of SO(n) classes. + * @author Frank Dellaert + **/ + +// #include +// #include +// #include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(SOn, SO5) { + const auto R = SOn(5); + EXPECT_LONGS_EQUAL(5, R.rows()); + Values values; + const Key key(0); + values.insert(key, R); + const auto B = values.at(key); + EXPECT_LONGS_EQUAL(5, B.rows()); +} + +/* ************************************************************************* */ +TEST(SOn, Random) { + static boost::mt19937 rng(42); + EXPECT_LONGS_EQUAL(3, SOn::Random(rng, 3).rows()); + EXPECT_LONGS_EQUAL(4, SOn::Random(rng, 4).rows()); + EXPECT_LONGS_EQUAL(5, SOn::Random(rng, 5).rows()); +} + +/* ************************************************************************* */ +TEST(SOn, HatVee) { + Vector6 v; + v << 1, 2, 3, 4, 5, 6; + + Matrix expected2(2, 2); + expected2 << 0, -1, 1, 0; + const auto actual2 = SOn::Hat(2, v.head<1>()); + CHECK(assert_equal(expected2, actual2)); + CHECK(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2))); + + Matrix expected3(3, 3); + expected3 << 0, -3, 2, // + 3, 0, -1, // + -2, 1, 0; + const auto actual3 = SOn::Hat(3, v.head<3>()); + CHECK(assert_equal(expected3, actual3)); + CHECK(assert_equal(skewSymmetric(1, 2, 3), actual3)); + CHECK(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3))); + + Matrix expected4(4, 4); + expected4 << 0, -6, 5, -3, // + 6, 0, -4, 2, // + -5, 4, 0, -1, // + 3, -2, 1, 0; + const auto actual4 = SOn::Hat(4, v); + CHECK(assert_equal(expected4, actual4)); + CHECK(assert_equal((Vector)v, SOn::Vee(actual4))); +} + +/* ************************************************************************* */ +TEST(SOn, RetractLocal) { + // If we do expmap in SO(3) subgroup, topleft should be equal to R1. + Vector6 v1 = (Vector(6) << 0, 0, 0, 0.01, 0, 0).finished(); + Matrix R1 = SO3::Retract(v1.tail<3>()); + SOn Q1 = SOn::Retract(4, v1); + CHECK(assert_equal(R1, Q1.block(0, 0, 3, 3), 1e-7)); + CHECK(assert_equal(v1, SOn::Local(Q1), 1e-7)); +} + +/* ************************************************************************* */ +TEST(SOn, vec) { + auto x = SOn(5); + Vector6 v; + v << 1, 2, 3, 4, 5, 6; + x.block(0, 0, 4, 4) = SO4::Expmap(v).matrix(); + Matrix actualH; + const Vector actual = x.vec(actualH); + boost::function h = [](const SOn& x) { return x.vec(); }; + const Matrix H = numericalDerivative11(h, x, 1e-5); + CHECK(assert_equal(H, actualH)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** From 2fb4668bba398ee9454e32def8711ad5562a3491 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 17 Apr 2019 00:05:53 -0400 Subject: [PATCH 04/47] Added factor for Karcher mean constraint: fixes the geodesic mean to impose a global gauge constraint. --- gtsam/slam/KarcherMeanFactor-inl.h | 62 ++++++++++++ gtsam/slam/KarcherMeanFactor.h | 70 +++++++++++++ gtsam/slam/tests/testKarcherMeanFactor.cpp | 109 +++++++++++++++++++++ 3 files changed, 241 insertions(+) create mode 100644 gtsam/slam/KarcherMeanFactor-inl.h create mode 100644 gtsam/slam/KarcherMeanFactor.h create mode 100644 gtsam/slam/tests/testKarcherMeanFactor.cpp diff --git a/gtsam/slam/KarcherMeanFactor-inl.h b/gtsam/slam/KarcherMeanFactor-inl.h new file mode 100644 index 000000000..cfe071ee5 --- /dev/null +++ b/gtsam/slam/KarcherMeanFactor-inl.h @@ -0,0 +1,62 @@ +/* ---------------------------------------------------------------------------- + + * 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 KarcherMeanFactor.cpp + * @author Frank Dellaert + * @date March 2019 + */ + +#pragma once + +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + +template +T FindKarcherMean(const vector& rotations) { + // Cost function C(R) = \sum PriorFactor(R_i)::error(R) + // No closed form solution. + NonlinearFactorGraph graph; + static const Key kKey(0); + for (const auto& R : rotations) { + graph.emplace_shared >(kKey, R); + } + Values initial; + initial.insert(kKey, T()); + auto result = GaussNewtonOptimizer(graph, initial).optimize(); + return result.at(kKey); +} + +template +template +KarcherMeanFactor::KarcherMeanFactor(const CONTAINER& keys, int d) + : NonlinearFactor(keys) { + if (d <= 0) { + throw std::invalid_argument( + "KarcherMeanFactor needs dimension for dynamic types."); + } + // Create the constant Jacobian made of D*D identity matrices, + // where D is the dimensionality of the manifold. + const auto I = Eigen::MatrixXd::Identity(d, d); + std::map terms; + for (Key j : keys) { + terms[j] = I; + } + jacobian_ = + boost::make_shared(terms, Eigen::VectorXd::Zero(d)); +} +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h new file mode 100644 index 000000000..d0f2f57db --- /dev/null +++ b/gtsam/slam/KarcherMeanFactor.h @@ -0,0 +1,70 @@ +/* ---------------------------------------------------------------------------- + + * 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 KarcherMeanFactor.h + * @author Frank Dellaert + * @date March 2019ƒ + */ + +#pragma once + +#include +#include + +#include +#include + +namespace gtsam { +/** + * Optimize for the Karcher mean, minimizing the geodesic distance to each of + * the given rotations, ,by constructing a factor graph out of simple + * PriorFactors. + */ +template +T FindKarcherMean(const std::vector& rotations); + +/** + * The KarcherMeanFactor creates a constraint on all SO(3) variables with + * given keys that the Karcher mean (see above) will stay the same. Note the + * mean itself is irrelevant to the constraint and is not a parameter: the + * constraint is implemented as enforcing that the sum of local updates is + * equal to zero, hence creating a rank-3 constraint. Note it is implemented as + * a soft constraint, as typically it is used to fix a gauge freedom. + * */ +template +class KarcherMeanFactor : public NonlinearFactor { + /// Constant Jacobian made of d*d identity matrices + boost::shared_ptr jacobian_; + + enum {D = traits::dimension}; + + public: + /// Construct from given keys. + template + KarcherMeanFactor(const CONTAINER& keys, int d=D); + + /// Destructor + virtual ~KarcherMeanFactor() {} + + /// Calculate the error of the factor: always zero + double error(const Values& c) const override { return 0; } + + /// get the dimension of the factor (number of rows on linearization) + size_t dim() const override { return D; } + + /// linearize to a GaussianFactor + boost::shared_ptr linearize(const Values& c) const override { + return jacobian_; + } +}; +// \KarcherMeanFactor +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/slam/tests/testKarcherMeanFactor.cpp b/gtsam/slam/tests/testKarcherMeanFactor.cpp new file mode 100644 index 000000000..bcc20c62a --- /dev/null +++ b/gtsam/slam/tests/testKarcherMeanFactor.cpp @@ -0,0 +1,109 @@ +/* ---------------------------------------------------------------------------- + + * 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 testKarcherMeanFactor.cpp + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +// Rot3 version +/* ************************************************************************* */ +static const Rot3 R = Rot3::Expmap(Vector3(0.1, 0, 0)); + +/* ************************************************************************* */ +// Check that optimizing for Karcher mean (which minimizes Between distance) +// gets correct result. +TEST(KarcherMean, FindRot3) { + std::vector rotations = {R, R.inverse()}; + Rot3 expected; + auto actual = FindKarcherMean(rotations); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +// Check that the InnerConstraint factor leaves the mean unchanged. +TEST(KarcherMean, FactorRot3) { + // Make a graph with two variables, one between, and one InnerConstraint + // The optimal result should satisfy the between, while moving the other + // variable to make the mean the same as before. + // Mean of R and R' is identity. Let's make a BetweenFactor making R21 = + // R*R*R, i.e. geodesic length is 3 rather than 2. + NonlinearFactorGraph graph; + graph.emplace_shared>(1, 2, R * R * R); + std::vector keys{1, 2}; + graph.emplace_shared>(keys); + + Values initial; + initial.insert(1, R.inverse()); + initial.insert(2, R); + const auto expected = FindKarcherMean({R, R.inverse()}); + + auto result = GaussNewtonOptimizer(graph, initial).optimize(); + const auto actual = + FindKarcherMean({result.at(1), result.at(2)}); + EXPECT(assert_equal(expected, actual)); + EXPECT( + assert_equal(R * R * R, result.at(1).between(result.at(2)))); +} + +/* ************************************************************************* */ +// SO(4) version +/* ************************************************************************* */ +static const SO4 Q = SO4::Expmap((Vector6() << 1, 2, 3, 4, 5, 6).finished()); + +/* ************************************************************************* */ +TEST(KarcherMean, FindSO4) { + std::vector rotations = {Q, Q.inverse()}; + auto expected = SO4(); //::ChordalMean(rotations); + auto actual = FindKarcherMean(rotations); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(KarcherMean, FactorSO4) { + NonlinearFactorGraph graph; + graph.emplace_shared>(1, 2, Q * Q * Q); + std::vector keys{1, 2}; + graph.emplace_shared>(keys); + + Values initial; + initial.insert(1, Q.inverse()); + initial.insert(2, Q); + const auto expected = FindKarcherMean({Q, Q.inverse()}); + + auto result = GaussNewtonOptimizer(graph, initial).optimize(); + const auto actual = + FindKarcherMean({result.at(1), result.at(2)}); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal((Matrix)(Q * Q * Q).matrix(), + result.at(1).between(result.at(2)))); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From 6eefc56e171e18a8238dbf57f9792336f8cd026e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 17 Apr 2019 00:07:38 -0400 Subject: [PATCH 05/47] Wrapped SO(3), SO(4) and new factor, and added SO(4) tests in python --- cython/gtsam/tests/test_Pose3.py | 3 +- cython/gtsam/tests/test_SO4.py | 59 ++++++++++++++++++++++++++ gtsam.h | 73 ++++++++++++++++++++++++++++++-- 3 files changed, 130 insertions(+), 5 deletions(-) create mode 100644 cython/gtsam/tests/test_SO4.py diff --git a/cython/gtsam/tests/test_Pose3.py b/cython/gtsam/tests/test_Pose3.py index 577a13304..138f1ff13 100644 --- a/cython/gtsam/tests/test_Pose3.py +++ b/cython/gtsam/tests/test_Pose3.py @@ -6,8 +6,9 @@ All Rights Reserved See LICENSE for the license information Pose3 unit tests. -Author: Frank Dellaert & Duy Nguyen Ta (Python) +Author: Frank Dellaert, Duy Nguyen Ta """ +# pylint: disable=no-name-in-module import math import unittest diff --git a/cython/gtsam/tests/test_SO4.py b/cython/gtsam/tests/test_SO4.py new file mode 100644 index 000000000..3b30a8e89 --- /dev/null +++ b/cython/gtsam/tests/test_SO4.py @@ -0,0 +1,59 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +SO4 unit tests. +Author: Frank Dellaert +""" +# pylint: disable=no-name-in-module, import-error +import unittest + +import numpy as np +from gtsam import SO4 + +id = SO4() +v1 = np.array([0.1, 0, 0, 0, 0, 0]) +Q1 = SO4.Expmap(v1) +v2 = np.array([0.01, 0.02, 0.03, 0, 0, 0]) +Q2 = SO4.Expmap(v2) + + +class TestSO4(unittest.TestCase): + """Test selected SO4 methods.""" + + def test_constructor(self): + """Construct from matrix.""" + matrix = np.eye(4) + so4 = SO4(matrix) + self.assertIsInstance(so4, SO4) + + def test_retract(self): + """Test retraction to manifold.""" + v = np.zeros((6,), np.float) + actual = id.retract(v) + self.assertTrue(actual.equals(id, 1e-9)) + + def test_local(self): + """Check localCoordinates for trivial case.""" + v0 = np.zeros((6,), np.float) + actual = id.localCoordinates(id) + np.testing.assert_array_almost_equal(actual, v0) + + def test_compose(self): + """Check compose works in subgroup.""" + expected = SO4.Expmap(2*v1) + actual = Q1.compose(Q1) + self.assertTrue(actual.equals(expected, 1e-9)) + + def test_vec(self): + """Check wrapping of vec.""" + expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1]) + actual = id.vec() + np.testing.assert_array_equal(actual, expected) + + +if __name__ == "__main__": + unittest.main() diff --git a/gtsam.h b/gtsam.h index bf3575580..d06d83a56 100644 --- a/gtsam.h +++ b/gtsam.h @@ -537,6 +537,60 @@ class Rot2 { void serialize() const; }; +#include +class SO3 { + // Standard Constructors + SO3(); + SO3(Matrix R); + static gtsam::SO3 AxisAngle(const Vector axis, double theta); + static gtsam::SO3 ClosestTo(const Matrix M); + + // Testable + void print(string s) const; + bool equals(const gtsam::SO3& other, double tol) const; + + // Group + static gtsam::SO3 identity(); + gtsam::SO3 inverse() const; + gtsam::SO3 between(const gtsam::SO3& R) const; + gtsam::SO3 compose(const gtsam::SO3& R) const; + + // Manifold + gtsam::SO3 retract(Vector v) const; + Vector localCoordinates(const gtsam::SO3& R) const; + static gtsam::SO3 Expmap(Vector v); + + // Other methods + Vector vec() const; + Matrix matrix() const; +}; + +#include +class SO4 { + // Standard Constructors + SO4(); + SO4(Matrix R); + + // Testable + void print(string s) const; + bool equals(const gtsam::SO4& other, double tol) const; + + // Group + static gtsam::SO4 identity(); + gtsam::SO4 inverse() const; + gtsam::SO4 between(const gtsam::SO4& Q) const; + gtsam::SO4 compose(const gtsam::SO4& Q) const; + + // Manifold + gtsam::SO4 retract(Vector v) const; + Vector localCoordinates(const gtsam::SO4& Q) const; + static gtsam::SO4 Expmap(Vector v); + + // Other methods + Vector vec() const; + Matrix matrix() const; +}; + #include class Rot3 { // Standard Constructors and Named Constructors @@ -559,6 +613,7 @@ class Rot3 { static gtsam::Rot3 Quaternion(double w, double x, double y, double z); static gtsam::Rot3 Rodrigues(Vector v); static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); + static gtsam::Rot3 ClosestTo(const Matrix M); // Testable void print(string s) const; @@ -1974,6 +2029,8 @@ class Values { void insert(size_t j, const gtsam::Point3& point3); void insert(size_t j, const gtsam::Rot2& rot2); void insert(size_t j, const gtsam::Pose2& pose2); + void insert(size_t j, const gtsam::SO3& R); + void insert(size_t j, const gtsam::SO4& Q); void insert(size_t j, const gtsam::Rot3& rot3); void insert(size_t j, const gtsam::Pose3& pose3); void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); @@ -1989,6 +2046,8 @@ class Values { void update(size_t j, const gtsam::Point3& point3); void update(size_t j, const gtsam::Rot2& rot2); void update(size_t j, const gtsam::Pose2& pose2); + void update(size_t j, const gtsam::SO3& R); + void update(size_t j, const gtsam::SO4& Q); void update(size_t j, const gtsam::Rot3& rot3); void update(size_t j, const gtsam::Pose3& pose3); void update(size_t j, const gtsam::Cal3_S2& cal3_s2); @@ -1999,7 +2058,7 @@ class Values { void update(size_t j, Vector vector); void update(size_t j, Matrix matrix); - template + template T at(size_t j); /// version for double @@ -2332,7 +2391,7 @@ class NonlinearISAM { #include #include -template +template virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2343,7 +2402,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor { #include -template +template virtual class BetweenFactor : gtsam::NoiseModelFactor { BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); T measured() const; @@ -2355,7 +2414,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { #include -template +template virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation NonlinearEquality(size_t j, const T& feasible); @@ -2614,6 +2673,12 @@ pair readG2o(string filename, bool void writeG2o(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& estimate, string filename); +#include +template +virtual class KarcherMeanFactor : gtsam::NonlinearFactor { + KarcherMeanFactor(const gtsam::KeyVector& keys); +}; + //************************************************************************* // Navigation //************************************************************************* From 6c00ff163f235570a656e1527af35878a3207f9a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 17 Apr 2019 00:24:13 -0400 Subject: [PATCH 06/47] Closest --- gtsam/geometry/Pose3.cpp | 20 +++----------------- gtsam/geometry/Rot3.h | 3 +++ gtsam/geometry/tests/testRot3.cpp | 17 +++++++++++++++++ gtsam/slam/InitializePose3.cpp | 18 +++--------------- 4 files changed, 26 insertions(+), 32 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 01611d739..0acadf5bc 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -52,7 +52,6 @@ Pose3 Pose3::inverse() const { /* ************************************************************************* */ // Calculate Adjoint map // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) -// Experimental - unit tests of derivatives based on it do not check out yet Matrix6 Pose3::AdjointMap() const { const Matrix3 R = R_.matrix(); Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R; @@ -418,24 +417,11 @@ boost::optional Pose3::Align(const std::vector& abPointPairs) for(const Point3Pair& abPair: abPointPairs) { Point3 da = abPair.first - aCentroid; Point3 db = abPair.second - bCentroid; - H += db * da.transpose(); + H += da * db.transpose(); } - // Compute SVD - Eigen::JacobiSVD svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV); - Matrix U = svd.matrixU(); - Vector S = svd.singularValues(); - Matrix V = svd.matrixV(); - - // Check rank - if (S[1] < 1e-10) - return boost::none; - - // Recover transform with correction from Eggert97machinevisionandapplications - Matrix3 UVtranspose = U * V.transpose(); - Matrix3 detWeighting = I_3x3; - detWeighting(2, 2) = UVtranspose.determinant(); - Rot3 aRb(Matrix(V * detWeighting * U.transpose())); + // ClosestTo finds rotation matrix closest to H in Frobenius sense + Rot3 aRb = Rot3::ClosestTo(H); Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid); return Pose3(aRb, aTb); } diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 985c521a2..d4f1301e9 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -228,6 +228,9 @@ namespace gtsam { static Rot3 AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, // const Unit3& a_q, const Unit3& b_q); + /// Static, named constructor that finds Rot3 element closest to M in Frobenius norm. + static Rot3 ClosestTo(const Matrix3& M) { return Rot3(SO3::ClosestTo(M)); } + /// @} /// @name Testable /// @{ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index e468eaf55..0365bc659 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -645,6 +645,23 @@ TEST(Rot3 , ChartDerivatives) { } } +/* ************************************************************************* */ +TEST(Rot3, ClosestTo) { + // Example top-left of SO(4) matrix not quite on SO(3) manifold + Matrix3 M; + M << 0.79067393, 0.6051136, -0.0930814, // + 0.4155925, -0.64214347, -0.64324489, // + -0.44948549, 0.47046326, -0.75917576; + + Matrix expected(3, 3); + expected << 0.790687, 0.605096, -0.0931312, // + 0.415746, -0.642355, -0.643844, // + -0.449411, 0.47036, -0.759468; + + auto actual = Rot3::ClosestTo(3*M); + EXPECT(assert_equal(expected, actual.matrix(), 1e-6)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index a1baab5fa..1e398cd99 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -75,27 +75,15 @@ Values InitializePose3::normalizeRelaxedRotations( const VectorValues& relaxedRot3) { gttic(InitializePose3_computeOrientationsChordal); - Matrix ppm = Z_3x3; // plus plus minus - ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1; - Values validRot3; for(const auto& it: relaxedRot3) { Key key = it.first; if (key != kAnchorKey) { - Matrix3 R; - R << Eigen::Map(it.second.data()); // Recover M from vectorized + Matrix3 M; + M << Eigen::Map(it.second.data()); // Recover M from vectorized // ClosestTo finds rotation matrix closest to H in Frobenius sense - // Rot3 initRot = Rot3::ClosestTo(M.transpose()); - - Matrix U, V; Vector s; - svd(R.transpose(), U, s, V); - Matrix3 normalizedRotMat = U * V.transpose(); - - if(normalizedRotMat.determinant() < 0) - normalizedRotMat = U * ppm * V.transpose(); - - Rot3 initRot = Rot3(normalizedRotMat); + Rot3 initRot = Rot3::ClosestTo(M.transpose()); validRot3.insert(key, initRot); } } From b0e4075089bbabf76a27f950120e4f915899ebf9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 17 Apr 2019 00:39:38 -0400 Subject: [PATCH 07/47] Added python test for Karcher mean --- cython/gtsam/tests/test_KarcherMeanFactor.py | 80 ++++++++++++++++++++ 1 file changed, 80 insertions(+) create mode 100644 cython/gtsam/tests/test_KarcherMeanFactor.py diff --git a/cython/gtsam/tests/test_KarcherMeanFactor.py b/cython/gtsam/tests/test_KarcherMeanFactor.py new file mode 100644 index 000000000..6976decc1 --- /dev/null +++ b/cython/gtsam/tests/test_KarcherMeanFactor.py @@ -0,0 +1,80 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +KarcherMeanFactor unit tests. +Author: Frank Dellaert +""" + +# pylint: disable=invalid-name, no-name-in-module, no-member + +import unittest + +import gtsam +import numpy as np +from gtsam.utils.test_case import GtsamTestCase + +KEY = 0 +MODEL = gtsam.noiseModel_Unit.Create(3) + + +def find_Karcher_mean_Rot3(rotations): + """Find the Karcher mean of given values.""" + # Cost function C(R) = \sum PriorFactor(R_i)::error(R) + # No closed form solution. + graph = gtsam.NonlinearFactorGraph() + for R in rotations: + graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL)) + initial = gtsam.Values() + initial.insert(KEY, gtsam.Rot3()) + result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() + return result.atRot3(KEY) + + +# Rot3 version +R = gtsam.Rot3.Expmap(np.array([0.1, 0, 0])) + + +class TestKarcherMean(GtsamTestCase): + + def test_find(self): + # Check that optimizing for Karcher mean (which minimizes Between distance) + # gets correct result. + rotations = {R, R.inverse()} + expected = gtsam.Rot3() + actual = find_Karcher_mean_Rot3(rotations) + self.gtsamAssertEquals(expected, actual) + + def test_factor(self): + """Check that the InnerConstraint factor leaves the mean unchanged.""" + # Make a graph with two variables, one between, and one InnerConstraint + # The optimal result should satisfy the between, while moving the other + # variable to make the mean the same as before. + # Mean of R and R' is identity. Let's make a BetweenFactor making R21 = + # R*R*R, i.e. geodesic length is 3 rather than 2. + graph = gtsam.NonlinearFactorGraph() + R12 = R.compose(R.compose(R)) + graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL)) + keys = gtsam.KeyVector() + keys.push_back(1) + keys.push_back(2) + graph.add(gtsam.KarcherMeanFactorRot3(keys)) + + initial = gtsam.Values() + initial.insert(1, R.inverse()) + initial.insert(2, R) + expected = find_Karcher_mean_Rot3([R, R.inverse()]) + + result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() + actual = find_Karcher_mean_Rot3( + [result.atRot3(1), result.atRot3(2)]) + self.gtsamAssertEquals(expected, actual) + self.gtsamAssertEquals( + R12, result.atRot3(1).between(result.atRot3(2))) + + +if __name__ == "__main__": + unittest.main() From ad0bb7953366db3552f13da2247b27974291bc27 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Apr 2019 14:28:43 -0400 Subject: [PATCH 08/47] Fixed issues with Closest and << in SOn.h --- gtsam/geometry/SO3.cpp | 2 +- gtsam/geometry/SOn.h | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index cd321b126..a509bcf74 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -136,7 +136,7 @@ SO3 SO3::AxisAngle(const Vector3& axis, double theta) { /* ************************************************************************* */ SO3 SO3::ClosestTo(const Matrix3& M) { - Eigen::JacobiSVD svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV); + 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(); diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index bc6282fe4..1b5228be0 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -75,12 +75,13 @@ class SOnBase { Matrix G(n2, d); for (size_t j = 0; j < d; j++) { // TODO(frank): this can't be right. Think about fixed vs dynamic. - G.col(j) << Derived::Hat(n, Eigen::VectorXd::Unit(d, j)); + 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 << derived(); + X << Eigen::Map(derived().data(), n2, 1); // If requested, calculate H as (I \oplus Q) * P if (H) { From 3e1db48ced0c9460622411edae8dd907d5429b4d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Apr 2019 16:49:20 -0400 Subject: [PATCH 09/47] Made constructor explicit, expose SOn in wrapper --- gtsam.h | 15 ++++++++++++++- gtsam/geometry/SO4.h | 2 +- gtsam/geometry/SOn.h | 20 +++++++++++++++----- 3 files changed, 30 insertions(+), 7 deletions(-) diff --git a/gtsam.h b/gtsam.h index d06d83a56..efe36a595 100644 --- a/gtsam.h +++ b/gtsam.h @@ -591,6 +591,17 @@ class SO4 { Matrix matrix() const; }; +#include +class SOn { + // Standard Constructors + SOn(size_t n); + SOn(Matrix R); + + // Other methods + Vector vec() const; + Matrix matrix() const; +}; + #include class Rot3 { // Standard Constructors and Named Constructors @@ -2031,6 +2042,7 @@ class Values { void insert(size_t j, const gtsam::Pose2& pose2); void insert(size_t j, const gtsam::SO3& R); void insert(size_t j, const gtsam::SO4& Q); + void insert(size_t j, const gtsam::SOn& P); void insert(size_t j, const gtsam::Rot3& rot3); void insert(size_t j, const gtsam::Pose3& pose3); void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); @@ -2048,6 +2060,7 @@ class Values { void update(size_t j, const gtsam::Pose2& pose2); void update(size_t j, const gtsam::SO3& R); void update(size_t j, const gtsam::SO4& Q); + void update(size_t j, const gtsam::SOn& P); void update(size_t j, const gtsam::Rot3& rot3); void update(size_t j, const gtsam::Pose3& pose3); void update(size_t j, const gtsam::Cal3_S2& cal3_s2); @@ -2058,7 +2071,7 @@ class Values { void update(size_t j, Vector vector); void update(size_t j, Matrix matrix); - template + template T at(size_t j); /// version for double diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h index e270c9555..59279b558 100644 --- a/gtsam/geometry/SO4.h +++ b/gtsam/geometry/SO4.h @@ -107,7 +107,7 @@ class SO4 : public Matrix4, public LieGroup { /// Return matrix (for wrapper) const Matrix4 &matrix() const { return *this; } - /// @ + /// @} private: /** Serialization function */ diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 1b5228be0..475e71861 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -107,14 +107,17 @@ class SOn : public Eigen::MatrixXd, /// @name Constructors /// @{ + /// Default constructor: only used for serialization and wrapping + SOn() {} + /// Construct SO(n) identity - SOn(size_t n) : Eigen::MatrixXd(n, n) { + explicit SOn(size_t n) : Eigen::MatrixXd(n, n) { *this << Eigen::MatrixXd::Identity(n, n); } /// Constructor from Eigen Matrix template - SOn(const Eigen::MatrixBase& R) : Eigen::MatrixXd(R.eval()) {} + 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) { @@ -211,7 +214,7 @@ class SOn : public Eigen::MatrixXd, 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 (I + X) * (I - X).inverse(); + return SOn((I + X) * (I - X).inverse()); } /** @@ -228,6 +231,13 @@ class SOn : public Eigen::MatrixXd, /// @name Lie group /// @{ + /// @} + /// @name Other methods + /// @{ + + /// Return matrix (for wrapper) + const Matrix &matrix() const { return *this; } + /// @} }; @@ -275,14 +285,14 @@ struct traits { ChartJacobian H2 = boost::none) { if (H1 || H2) throw std::runtime_error("SOn::Retract"); const size_t n = R.rows(); - return R * SOn::Retract(n, xi); + 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"); - Matrix between = R.inverse() * other; + const SOn between = SOn(R.inverse() * other); return SOn::Local(between); } From 6b2f6349d7605db1ac5463c62730155a5f0bde12 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 30 Apr 2019 08:47:34 -0400 Subject: [PATCH 10/47] Derive from SOnBase --- gtsam/geometry/SO3.h | 26 +++++-------- gtsam/geometry/SO4.h | 4 +- gtsam/geometry/SOn.h | 87 +++++++++++++++++++++----------------------- 3 files changed, 53 insertions(+), 64 deletions(-) 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; + } /// @} }; From 37c53aa23f64ca66624e063bfcf55b9c024b2fc0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 2 May 2019 23:54:15 -0400 Subject: [PATCH 11/47] SO3 and SO4 both Lie groups. --- gtsam/geometry/tests/testSOn.cpp | 673 ++++++++++++++++++++++++++++--- 1 file changed, 612 insertions(+), 61 deletions(-) diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index 868833ce1..9b852a91d 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -15,13 +15,142 @@ * @author Frank Dellaert **/ +#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; +} +} // namespace internal + +template +class SO : public Eigen::Matrix, + public LieGroup, internal::DimensionSO(N)> { + public: + enum { dimension = internal::DimensionSO(N) }; + using MatrixNN = Eigen::Matrix; + using VectorD = Eigen::Matrix; + using MatrixDD = Eigen::Matrix; + + /// @name Constructors + /// @{ + + /// Construct SO identity for N >= 2 + template = 2, void>> + SO() { + *this << 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."); + this->resize(n, n); + *this << Eigen::MatrixXd::Identity(n, n); + } + + /// Constructor from Eigen Matrix + template + SO(const Eigen::MatrixBase& R) : MatrixNN(R.eval()) {} + + /// @} + /// @name Testable + /// @{ + + void print(const std::string& s) const { + std::cout << s << *this << std::endl; + } + + bool equals(const SO& R, double tol) const { + return equal_with_abs_tol(*this, R, tol); + } + + /// @} + /// @name Group + /// @{ + + /// SO identity for N >= 2 + template = 2, void>> + 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 this->transpose(); } + + /// @} + /// @name Manifold + /// @{ + + // Chart at origin + struct ChartAtOrigin { + static SO Retract(const VectorD& xi, + OptionalJacobian H = boost::none) { + return SO(); // TODO(frank): Expmap(xi, H); + } + static VectorD Local( + const SO& R, OptionalJacobian H = boost::none) { + return VectorD(); // TODO(frank): Logmap(R, H); + } + }; + + /// @} + /// @name Lie Group + /// @{ + + MatrixDD AdjointMap() const { return MatrixDD(); } + + /** + * Exponential map at identity - create a rotation from canonical coordinates + */ + static SO Expmap(const VectorD& omega, OptionalJacobian H = boost::none); + + /** + * Log map at identity - returns the canonical coordinates of this rotation + */ + static VectorD Logmap(const SO& R, OptionalJacobian H = boost::none); + + // inverse with optional derivative + using LieGroup, internal::DimensionSO(N)>::inverse; + + /// @} +}; + +using SOn = SO; +using SO3 = SO<3>; +using SO4 = SO<4>; + +template +struct traits> : public internal::LieGroup> {}; + +template +struct traits> : public internal::LieGroup> {}; + +} // namespace gtsam + // #include // #include // #include #include -#include -#include -#include +// #include +// #include +// #include #include #include @@ -30,76 +159,498 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -TEST(SOn, SO5) { - const auto R = SOn(5); - EXPECT_LONGS_EQUAL(5, R.rows()); - Values values; - const Key key(0); - values.insert(key, R); - const auto B = values.at(key); - EXPECT_LONGS_EQUAL(5, B.rows()); +// TEST(SOn, SO5) { +// const auto R = SOn(5); +// EXPECT_LONGS_EQUAL(5, R.rows()); +// } + +/* ************************************************************************* */ +TEST(SOn, Concept) { + // BOOST_CONCEPT_ASSERT((IsGroup)); + // BOOST_CONCEPT_ASSERT((IsManifold)); + // BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +// /* ************************************************************************* +// */ TEST(SOn, Values) { +// const auto R = SOn(5); +// Values values; +// const Key key(0); +// values.insert(key, R); +// const auto B = values.at(key); +// EXPECT_LONGS_EQUAL(5, B.rows()); +// } + +// /* ************************************************************************* +// */ TEST(SOn, Random) { +// static boost::mt19937 rng(42); +// EXPECT_LONGS_EQUAL(3, SOn::Random(rng, 3).rows()); +// EXPECT_LONGS_EQUAL(4, SOn::Random(rng, 4).rows()); +// EXPECT_LONGS_EQUAL(5, SOn::Random(rng, 5).rows()); +// } + +// /* ************************************************************************* +// */ TEST(SOn, HatVee) { +// Vector6 v; +// v << 1, 2, 3, 4, 5, 6; + +// Matrix expected2(2, 2); +// expected2 << 0, -1, 1, 0; +// const auto actual2 = SOn::Hat(2, v.head<1>()); +// CHECK(assert_equal(expected2, actual2)); +// CHECK(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2))); + +// Matrix expected3(3, 3); +// expected3 << 0, -3, 2, // +// 3, 0, -1, // +// -2, 1, 0; +// const auto actual3 = SOn::Hat(3, v.head<3>()); +// CHECK(assert_equal(expected3, actual3)); +// CHECK(assert_equal(skewSymmetric(1, 2, 3), actual3)); +// CHECK(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3))); + +// Matrix expected4(4, 4); +// expected4 << 0, -6, 5, -3, // +// 6, 0, -4, 2, // +// -5, 4, 0, -1, // +// 3, -2, 1, 0; +// const auto actual4 = SOn::Hat(4, v); +// CHECK(assert_equal(expected4, actual4)); +// CHECK(assert_equal((Vector)v, SOn::Vee(actual4))); +// } + +// /* ************************************************************************* +// */ TEST(SOn, RetractLocal) { +// // If we do expmap in SO(3) subgroup, topleft should be equal to R1. +// Vector6 v1 = (Vector(6) << 0, 0, 0, 0.01, 0, 0).finished(); +// Matrix R1 = SO3::Retract(v1.tail<3>()); +// SOn Q1 = SOn::Retract(4, v1); +// CHECK(assert_equal(R1, Q1.block(0, 0, 3, 3), 1e-7)); +// CHECK(assert_equal(v1, SOn::Local(Q1), 1e-7)); +// } + +// /* ************************************************************************* +// */ TEST(SOn, vec) { +// auto x = SOn(5); +// Vector6 v; +// v << 1, 2, 3, 4, 5, 6; +// x.block(0, 0, 4, 4) = SO4::Expmap(v).matrix(); +// Matrix actualH; +// const Vector actual = x.vec(actualH); +// boost::function h = [](const SOn& x) { return x.vec(); +// }; const Matrix H = numericalDerivative11(h, x, 1e-5); +// CHECK(assert_equal(H, actualH)); +// } + +//****************************************************************************** +// SO4 +//****************************************************************************** + +TEST(SO4, Identity) { + const SO4 R; + EXPECT_LONGS_EQUAL(4, R.rows()); } /* ************************************************************************* */ -TEST(SOn, Random) { - static boost::mt19937 rng(42); - EXPECT_LONGS_EQUAL(3, SOn::Random(rng, 3).rows()); - EXPECT_LONGS_EQUAL(4, SOn::Random(rng, 4).rows()); - EXPECT_LONGS_EQUAL(5, SOn::Random(rng, 5).rows()); +TEST(SO4, Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); } -/* ************************************************************************* */ -TEST(SOn, HatVee) { - Vector6 v; - v << 1, 2, 3, 4, 5, 6; +// //****************************************************************************** +// SO4 id; +// Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished(); +// SO4 Q1 = SO4::Expmap(v1); +// Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.00, 0.00, 0.00).finished(); +// SO4 Q2 = SO4::Expmap(v2); +// Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished(); +// SO4 Q3 = SO4::Expmap(v3); - Matrix expected2(2, 2); - expected2 << 0, -1, 1, 0; - const auto actual2 = SOn::Hat(2, v.head<1>()); - CHECK(assert_equal(expected2, actual2)); - CHECK(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2))); +// //****************************************************************************** +// TEST(SO4, Random) { +// boost::mt19937 rng(42); +// auto Q = SO4::Random(rng); +// EXPECT_LONGS_EQUAL(4, Q.matrix().rows()); +// } +// //****************************************************************************** +// TEST(SO4, Expmap) { +// // If we do exponential map in SO(3) subgroup, topleft should be equal to +// R1. auto R1 = SO3::Expmap(v1.head<3>()); +// EXPECT((Q1.topLeft().isApprox(R1))); - Matrix expected3(3, 3); - expected3 << 0, -3, 2, // - 3, 0, -1, // - -2, 1, 0; - const auto actual3 = SOn::Hat(3, v.head<3>()); - CHECK(assert_equal(expected3, actual3)); - CHECK(assert_equal(skewSymmetric(1, 2, 3), actual3)); - CHECK(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3))); +// // Same here +// auto R2 = SO3::Expmap(v2.head<3>()); +// EXPECT((Q2.topLeft().isApprox(R2))); - Matrix expected4(4, 4); - expected4 << 0, -6, 5, -3, // - 6, 0, -4, 2, // - -5, 4, 0, -1, // - 3, -2, 1, 0; - const auto actual4 = SOn::Hat(4, v); - CHECK(assert_equal(expected4, actual4)); - CHECK(assert_equal((Vector)v, SOn::Vee(actual4))); +// // Check commutative subgroups +// for (size_t i = 0; i < 6; i++) { +// Vector6 xi = Vector6::Zero(); +// xi[i] = 2; +// SO4 Q1 = SO4::Expmap(xi); +// xi[i] = 3; +// SO4 Q2 = SO4::Expmap(xi); +// EXPECT(((Q1 * Q2).isApprox(Q2 * Q1))); +// } +// } + +// //****************************************************************************** +// TEST(SO4, Cayley) { +// CHECK(assert_equal(id.retract(v1 / 100), SO4::Expmap(v1 / 100))); +// CHECK(assert_equal(id.retract(v2 / 100), SO4::Expmap(v2 / 100))); +// } + +// //****************************************************************************** +// TEST(SO4, Retract) { +// auto v = Vector6::Zero(); +// SO4 actual = traits::Retract(id, v); +// EXPECT(actual.isApprox(id)); +// } + +// //****************************************************************************** +// TEST(SO4, Local) { +// auto v0 = Vector6::Zero(); +// Vector6 actual = traits::Local(id, id); +// EXPECT(assert_equal((Vector)v0, actual)); +// } + +// //****************************************************************************** +// TEST(SO4, Invariants) { +// EXPECT(check_group_invariants(id, id)); +// EXPECT(check_group_invariants(id, Q1)); +// EXPECT(check_group_invariants(Q2, id)); +// EXPECT(check_group_invariants(Q2, Q1)); +// EXPECT(check_group_invariants(Q1, Q2)); + +// EXPECT(check_manifold_invariants(id, id)); +// EXPECT(check_manifold_invariants(id, Q1)); +// EXPECT(check_manifold_invariants(Q2, id)); +// EXPECT(check_manifold_invariants(Q2, Q1)); +// EXPECT(check_manifold_invariants(Q1, Q2)); +// } + +// /* ************************************************************************* +// */ TEST(SO4, compose) { +// SO4 expected = Q1 * Q2; +// Matrix actualH1, actualH2; +// SO4 actual = Q1.compose(Q2, actualH1, actualH2); +// CHECK(assert_equal(expected, actual)); + +// Matrix numericalH1 = +// numericalDerivative21(testing::compose, Q1, Q2, 1e-2); +// CHECK(assert_equal(numericalH1, actualH1)); + +// Matrix numericalH2 = +// numericalDerivative22(testing::compose, Q1, Q2, 1e-2); +// CHECK(assert_equal(numericalH2, actualH2)); +// } + +// /* ************************************************************************* +// */ TEST(SO4, vec) { +// using Vector16 = SO4::Vector16; +// const Vector16 expected = Eigen::Map(Q2.data()); +// Matrix actualH; +// const Vector16 actual = Q2.vec(actualH); +// CHECK(assert_equal(expected, actual)); +// boost::function f = [](const SO4& Q) { +// return Q.vec(); +// }; +// const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5); +// CHECK(assert_equal(numericalH, actualH)); +// } + +// /* ************************************************************************* +// */ TEST(SO4, topLeft) { +// const Matrix3 expected = Q3.topLeftCorner<3, 3>(); +// Matrix actualH; +// const Matrix3 actual = Q3.topLeft(actualH); +// CHECK(assert_equal(expected, actual)); +// boost::function f = [](const SO4& Q3) { +// return Q3.topLeft(); +// }; +// const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); +// CHECK(assert_equal(numericalH, actualH)); +// } + +// /* ************************************************************************* +// */ TEST(SO4, stiefel) { +// const Matrix43 expected = Q3.leftCols<3>(); +// Matrix actualH; +// const Matrix43 actual = Q3.stiefel(actualH); +// CHECK(assert_equal(expected, actual)); +// boost::function f = [](const SO4& Q3) { +// return Q3.stiefel(); +// }; +// const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); +// CHECK(assert_equal(numericalH, actualH)); +// } + +//****************************************************************************** +// SO3 +//****************************************************************************** + +TEST(SO3, Identity) { + const SO3 R; + EXPECT_LONGS_EQUAL(3, R.rows()); } -/* ************************************************************************* */ -TEST(SOn, RetractLocal) { - // If we do expmap in SO(3) subgroup, topleft should be equal to R1. - Vector6 v1 = (Vector(6) << 0, 0, 0, 0.01, 0, 0).finished(); - Matrix R1 = SO3::Retract(v1.tail<3>()); - SOn Q1 = SOn::Retract(4, v1); - CHECK(assert_equal(R1, Q1.block(0, 0, 3, 3), 1e-7)); - CHECK(assert_equal(v1, SOn::Local(Q1), 1e-7)); +//****************************************************************************** +TEST(SO3, Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); } -/* ************************************************************************* */ -TEST(SOn, vec) { - auto x = SOn(5); - Vector6 v; - v << 1, 2, 3, 4, 5, 6; - x.block(0, 0, 4, 4) = SO4::Expmap(v).matrix(); - Matrix actualH; - const Vector actual = x.vec(actualH); - boost::function h = [](const SOn& x) { return x.vec(); }; - const Matrix H = numericalDerivative11(h, x, 1e-5); - CHECK(assert_equal(H, actualH)); -} +// //****************************************************************************** +// TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } + +// //****************************************************************************** +// SO3 id; +// Vector3 z_axis(0, 0, 1); +// SO3 R1(Eigen::AngleAxisd(0.1, z_axis)); +// SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); + +// //****************************************************************************** +// TEST(SO3, Local) { +// Vector3 expected(0, 0, 0.1); +// Vector3 actual = traits::Local(R1, R2); +// EXPECT(assert_equal((Vector)expected, actual)); +// } + +// //****************************************************************************** +// TEST(SO3, Retract) { +// Vector3 v(0, 0, 0.1); +// SO3 actual = traits::Retract(R1, v); +// EXPECT(actual.isApprox(R2)); +// } + +// //****************************************************************************** +// TEST(SO3, Invariants) { +// EXPECT(check_group_invariants(id, id)); +// EXPECT(check_group_invariants(id, R1)); +// EXPECT(check_group_invariants(R2, id)); +// EXPECT(check_group_invariants(R2, R1)); + +// EXPECT(check_manifold_invariants(id, id)); +// EXPECT(check_manifold_invariants(id, R1)); +// EXPECT(check_manifold_invariants(R2, id)); +// EXPECT(check_manifold_invariants(R2, R1)); +// } + +// //****************************************************************************** +// TEST(SO3, LieGroupDerivatives) { +// CHECK_LIE_GROUP_DERIVATIVES(id, id); +// CHECK_LIE_GROUP_DERIVATIVES(id, R2); +// CHECK_LIE_GROUP_DERIVATIVES(R2, id); +// CHECK_LIE_GROUP_DERIVATIVES(R2, R1); +// } + +// //****************************************************************************** +// TEST(SO3, ChartDerivatives) { +// CHECK_CHART_DERIVATIVES(id, id); +// CHECK_CHART_DERIVATIVES(id, R2); +// CHECK_CHART_DERIVATIVES(R2, id); +// CHECK_CHART_DERIVATIVES(R2, R1); +// } + +// /* ************************************************************************* +// */ namespace exmap_derivative { static const Vector3 w(0.1, 0.27, -0.2); +// } +// // Left trivialized Derivative of exp(w) wrpt w: +// // How does exp(w) change when w changes? +// // We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 +// // => y = log (exp(-w) * exp(w+dw)) +// Vector3 testDexpL(const Vector3& dw) { +// using exmap_derivative::w; +// return SO3::Logmap(SO3::Expmap(-w) * SO3::Expmap(w + dw)); +// } + +// TEST(SO3, ExpmapDerivative) { +// using exmap_derivative::w; +// const Matrix actualDexpL = SO3::ExpmapDerivative(w); +// const Matrix expectedDexpL = +// numericalDerivative11(testDexpL, Vector3::Zero(), +// 1e-2); +// EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-7)); + +// const Matrix actualDexpInvL = SO3::LogmapDerivative(w); +// EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-7)); +// } + +// /* ************************************************************************* +// */ TEST(SO3, ExpmapDerivative2) { +// const Vector3 theta(0.1, 0, 0.1); +// const Matrix Jexpected = numericalDerivative11( +// boost::bind(&SO3::Expmap, _1, boost::none), theta); + +// CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); +// CHECK(assert_equal(Matrix3(Jexpected.transpose()), +// SO3::ExpmapDerivative(-theta))); +// } + +// /* ************************************************************************* +// */ TEST(SO3, ExpmapDerivative3) { +// const Vector3 theta(10, 20, 30); +// const Matrix Jexpected = numericalDerivative11( +// boost::bind(&SO3::Expmap, _1, boost::none), theta); + +// CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); +// CHECK(assert_equal(Matrix3(Jexpected.transpose()), +// SO3::ExpmapDerivative(-theta))); +// } + +// /* ************************************************************************* +// */ TEST(SO3, ExpmapDerivative4) { +// // Iserles05an (Lie-group Methods) says: +// // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) +// // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t) +// // where A(t): R -> so(3) is a trajectory in the tangent space of SO(3) +// // and dexp[A] is a linear map from 3*3 to 3*3 derivatives of se(3) +// // Hence, the above matrix equation is typed: 3*3 = SO(3) * linear_map(3*3) + +// // In GTSAM, we don't work with the skew-symmetric matrices A directly, but +// // with 3-vectors. +// // omega is easy: d Expmap(w(t)) / dt = ExmapDerivative[w(t)] * w'(t) + +// // Let's verify the above formula. + +// auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; +// auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; + +// // We define a function R +// auto R = [w](double t) { return SO3::Expmap(w(t)); }; + +// for (double t = -2.0; t < 2.0; t += 0.3) { +// const Matrix expected = numericalDerivative11(R, t); +// const Matrix actual = SO3::ExpmapDerivative(w(t)) * w_dot(t); +// CHECK(assert_equal(expected, actual, 1e-7)); +// } +// } + +// /* ************************************************************************* +// */ TEST(SO3, ExpmapDerivative5) { +// auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; +// auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; + +// // Now define R as mapping local coordinates to neighborhood around R0. +// const SO3 R0 = SO3::Expmap(Vector3(0.1, 0.4, 0.2)); +// auto R = [R0, w](double t) { return R0.expmap(w(t)); }; + +// for (double t = -2.0; t < 2.0; t += 0.3) { +// const Matrix expected = numericalDerivative11(R, t); +// const Matrix actual = SO3::ExpmapDerivative(w(t)) * w_dot(t); +// CHECK(assert_equal(expected, actual, 1e-7)); +// } +// } + +// /* ************************************************************************* +// */ TEST(SO3, ExpmapDerivative6) { +// const Vector3 thetahat(0.1, 0, 0.1); +// const Matrix Jexpected = numericalDerivative11( +// boost::bind(&SO3::Expmap, _1, boost::none), thetahat); +// Matrix3 Jactual; +// SO3::Expmap(thetahat, Jactual); +// EXPECT(assert_equal(Jexpected, Jactual)); +// } + +// /* ************************************************************************* +// */ TEST(SO3, LogmapDerivative) { +// const Vector3 thetahat(0.1, 0, 0.1); +// const SO3 R = SO3::Expmap(thetahat); // some rotation +// const Matrix Jexpected = numericalDerivative11( +// boost::bind(&SO3::Logmap, _1, boost::none), R); +// const Matrix3 Jactual = SO3::LogmapDerivative(thetahat); +// EXPECT(assert_equal(Jexpected, Jactual)); +// } + +// /* ************************************************************************* +// */ TEST(SO3, JacobianLogmap) { +// const Vector3 thetahat(0.1, 0, 0.1); +// const SO3 R = SO3::Expmap(thetahat); // some rotation +// const Matrix Jexpected = numericalDerivative11( +// boost::bind(&SO3::Logmap, _1, boost::none), R); +// Matrix3 Jactual; +// SO3::Logmap(R, Jactual); +// EXPECT(assert_equal(Jexpected, Jactual)); +// } + +// /* ************************************************************************* +// */ TEST(SO3, ApplyDexp) { +// Matrix aH1, aH2; +// for (bool nearZeroApprox : {true, false}) { +// boost::function f = +// [=](const Vector3& omega, const Vector3& v) { +// return so3::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); +// 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), +// local.applyDexp(v, aH1, aH2))); +// EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); +// EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); +// EXPECT(assert_equal(local.dexp(), aH2)); +// } +// } +// } +// } + +// /* ************************************************************************* +// */ TEST(SO3, ApplyInvDexp) { +// Matrix aH1, aH2; +// for (bool nearZeroApprox : {true, false}) { +// boost::function f = +// [=](const Vector3& omega, const Vector3& v) { +// return so3::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); +// 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)}) { +// EXPECT(assert_equal(Vector3(invDexp * v), +// local.applyInvDexp(v, aH1, aH2))); +// EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); +// EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); +// EXPECT(assert_equal(invDexp, aH2)); +// } +// } +// } +// } + +// /* ************************************************************************* +// */ TEST(SO3, vec) { +// const Vector9 expected = Eigen::Map(R2.data()); +// Matrix actualH; +// const Vector9 actual = R2.vec(actualH); +// CHECK(assert_equal(expected, actual)); +// boost::function f = [](const SO3& Q) { +// return Q.vec(); +// }; +// const Matrix numericalH = numericalDerivative11(f, R2, 1e-5); +// CHECK(assert_equal(numericalH, actualH)); +// } + +// //****************************************************************************** +// TEST(Matrix, compose) { +// Matrix3 M; +// M << 1, 2, 3, 4, 5, 6, 7, 8, 9; +// SO3 R = SO3::Expmap(Vector3(1, 2, 3)); +// const Matrix3 expected = M * R.matrix(); +// Matrix actualH; +// const Matrix3 actual = so3::compose(M, R, actualH); +// CHECK(assert_equal(expected, actual)); +// boost::function f = [R](const Matrix3& M) { +// return so3::compose(M, R); +// }; +// Matrix numericalH = numericalDerivative11(f, M, 1e-2); +// CHECK(assert_equal(numericalH, actualH)); +// } //****************************************************************************** int main() { From a35709295a90b65efe1f28cca3d8e867efe72b85 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 3 May 2019 08:16:04 -0400 Subject: [PATCH 12/47] Made matrix a member variable --- gtsam/geometry/tests/testSOn.cpp | 41 +++++++++++++++++++++----------- 1 file changed, 27 insertions(+), 14 deletions(-) diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index 9b852a91d..5d9f5773f 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -31,53 +31,64 @@ constexpr int DimensionSO(int N) { } } // namespace internal +/** + * Space of special orthogonal rotation matrices SO + */ template -class SO : public Eigen::Matrix, - public LieGroup, internal::DimensionSO(N)> { +class SO : public LieGroup, internal::DimensionSO(N)> { public: enum { dimension = internal::DimensionSO(N) }; using MatrixNN = Eigen::Matrix; using VectorD = Eigen::Matrix; using MatrixDD = Eigen::Matrix; + MatrixNN matrix_; ///< Rotation matrix + /// @name Constructors /// @{ /// Construct SO identity for N >= 2 template = 2, void>> - SO() { - *this << MatrixNN::Identity(); - } + 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."); - this->resize(n, n); - *this << Eigen::MatrixXd::Identity(n, n); + matrix_ = Eigen::MatrixXd::Identity(n, n); } /// Constructor from Eigen Matrix template - SO(const Eigen::MatrixBase& R) : MatrixNN(R.eval()) {} + explicit SO(const Eigen::MatrixBase& R) : matrix_(R.eval()) {} + + /// @} + /// @name Standard methods + /// @{ + + 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 << *this << std::endl; + std::cout << s << matrix_ << std::endl; } - bool equals(const SO& R, double tol) const { - return equal_with_abs_tol(*this, R, tol); + 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 = 2, void>> static SO identity() { @@ -92,7 +103,7 @@ class SO : public Eigen::Matrix, } /// inverse of a rotation = transpose - SO inverse() const { return this->transpose(); } + SO inverse() const { return SO(matrix_.transpose()); } /// @} /// @name Manifold @@ -119,12 +130,14 @@ class SO : public Eigen::Matrix, /** * Exponential map at identity - create a rotation from canonical coordinates */ - static SO Expmap(const VectorD& omega, OptionalJacobian H = boost::none); + static SO Expmap(const VectorD& omega, + OptionalJacobian H = boost::none); /** * Log map at identity - returns the canonical coordinates of this rotation */ - static VectorD Logmap(const SO& R, OptionalJacobian H = boost::none); + static VectorD Logmap(const SO& R, + OptionalJacobian H = boost::none); // inverse with optional derivative using LieGroup, internal::DimensionSO(N)>::inverse; From 676f93ebd88b4d62ab71434b2fd8d75139434d98 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 May 2019 17:18:16 -0400 Subject: [PATCH 13/47] Add pointer constructor for dynamic case --- gtsam/base/OptionalJacobian.h | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 166297d5f..82a5ae7f4 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -176,10 +176,11 @@ public: pointer_(NULL) { } - /// Constructor that will resize a dynamic matrix (unless already correct) - OptionalJacobian(Eigen::MatrixXd& dynamic) : - pointer_(&dynamic) { - } + /// Construct from pointer to dynamic matrix + OptionalJacobian(Jacobian* pointer) : pointer_(pointer) {} + + /// Construct from refrence to dynamic matrix + OptionalJacobian(Jacobian& dynamic) : pointer_(&dynamic) {} #ifndef OPTIONALJACOBIAN_NOBOOST From 6f071928bc5aefe84db80b2d9dae3caea732b97c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 May 2019 17:18:31 -0400 Subject: [PATCH 14/47] Allow dynamic dimension Lie groups --- gtsam/base/Lie.h | 6 ------ 1 file changed, 6 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index bb49a84d6..641597044 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -35,9 +35,6 @@ namespace gtsam { template struct LieGroup { - BOOST_STATIC_ASSERT_MSG(N != Eigen::Dynamic, - "LieGroup not yet specialized for dynamically sized types."); - enum { dimension = N }; typedef OptionalJacobian ChartJacobian; typedef Eigen::Matrix Jacobian; @@ -190,9 +187,6 @@ struct LieGroupTraits { typedef Eigen::Matrix TangentVector; typedef OptionalJacobian ChartJacobian; - BOOST_STATIC_ASSERT_MSG(dimension != Eigen::Dynamic, - "LieGroupTraits not yet specialized for dynamically sized types."); - static int GetDimension(const Class&) {return dimension;} static TangentVector Local(const Class& origin, const Class& other, From cc2f0f242cc16aeb11c417ca6e83366b65d41a62 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 May 2019 17:18:59 -0400 Subject: [PATCH 15/47] Succeeded in making SOn a Lie group --- gtsam/geometry/tests/testSOn.cpp | 40 +++++++++++++++++++++++++++++--- 1 file changed, 37 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index 5d9f5773f..74a1bb0cf 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -29,6 +29,12 @@ namespace internal { constexpr int DimensionSO(int N) { return (N < 0) ? Eigen::Dynamic : N * (N - 1) / 2; } + +// Return dynamic identity matrix for given SO(n) dimensionality d +Matrix IdentitySO(size_t n) { + const size_t d = n * (n - 1) / 2; + return Matrix::Identity(d, d); +} } // namespace internal /** @@ -149,6 +155,34 @@ using SOn = SO; using SO3 = SO<3>; using SO4 = SO<4>; +/* + * Fully specialize compose and between, because the derivative is unknowable by + * the LieGroup implementations, who return a fixed-size matrix for H2. + */ + +using DynamicJacobian = OptionalJacobian; + +template <> +SOn LieGroup::compose(const SOn& g, DynamicJacobian H1, + DynamicJacobian H2) const { + if (H1) *H1 = g.inverse().AdjointMap(); + if (H2) *H2 = internal::IdentitySO(g.rows()); + return derived() * g; +} + +template <> +SOn LieGroup::between(const SOn& g, DynamicJacobian H1, + DynamicJacobian H2) const { + SOn result = derived().inverse() * g; + if (H1) *H1 = -result.inverse().AdjointMap(); + if (H2) *H2 = internal::IdentitySO(g.rows()); + return result; +} + +/* + * Define the traits. internal::LieGroup provides both Lie group and Testable + */ + template struct traits> : public internal::LieGroup> {}; @@ -179,9 +213,9 @@ using namespace gtsam; /* ************************************************************************* */ TEST(SOn, Concept) { - // BOOST_CONCEPT_ASSERT((IsGroup)); - // BOOST_CONCEPT_ASSERT((IsManifold)); - // BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); } // /* ************************************************************************* From be83a6bad7e3fb53a7dd945b8c2f8093a580975a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 5 May 2019 16:59:37 -0400 Subject: [PATCH 16/47] Hat/Vee etc, working Random for SOn --- gtsam/geometry/tests/testSOn.cpp | 260 ++++++++++++++++++++++++------- 1 file changed, 203 insertions(+), 57 deletions(-) diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index 74a1bb0cf..df41b64da 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -17,6 +17,9 @@ #include #include +#include + +#include #include #include @@ -25,16 +28,10 @@ namespace gtsam { namespace internal { -// Calculate dimensionality of SO manifold, or return Dynamic if so +/// Calculate dimensionality of SO manifold, or return Dynamic if so constexpr int DimensionSO(int N) { return (N < 0) ? Eigen::Dynamic : N * (N - 1) / 2; } - -// Return dynamic identity matrix for given SO(n) dimensionality d -Matrix IdentitySO(size_t n) { - const size_t d = n * (n - 1) / 2; - return Matrix::Identity(d, d); -} } // namespace internal /** @@ -45,21 +42,30 @@ class SO : public LieGroup, internal::DimensionSO(N)> { public: enum { dimension = internal::DimensionSO(N) }; using MatrixNN = Eigen::Matrix; - using VectorD = 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 = 2, void>> + template > SO() : matrix_(MatrixNN::Identity()) {} /// Construct SO identity for N == Eigen::Dynamic - template > + template > explicit SO(size_t n = 0) { if (n == 0) throw std::runtime_error("SO: Dimensionality not known."); matrix_ = Eigen::MatrixXd::Identity(n, n); @@ -69,6 +75,31 @@ class SO : public LieGroup, internal::DimensionSO(N)> { 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 /// @{ @@ -96,14 +127,13 @@ class SO : public LieGroup, internal::DimensionSO(N)> { SO operator*(const SO& other) const { return SO(matrix_ * other.matrix_); } /// SO identity for N >= 2 - template = 2, void>> + template > static SO identity() { return SO(); } /// SO identity for N == Eigen::Dynamic - template > + template > static SO identity(size_t n = 0) { return SO(n); } @@ -115,18 +145,127 @@ class SO : public LieGroup, internal::DimensionSO(N)> { /// @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 { - static SO Retract(const VectorD& xi, - OptionalJacobian H = boost::none) { - return SO(); // TODO(frank): Expmap(xi, H); + /** + * 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()); } - static VectorD Local( - const SO& R, OptionalJacobian H = boost::none) { - return VectorD(); // TODO(frank): Logmap(R, H); + /** + * 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 /// @{ @@ -136,14 +275,12 @@ class SO : public LieGroup, internal::DimensionSO(N)> { /** * Exponential map at identity - create a rotation from canonical coordinates */ - static SO Expmap(const VectorD& omega, - OptionalJacobian H = boost::none); + static SO Expmap(const TangentVector& omega, ChartJacobian H = boost::none); /** * Log map at identity - returns the canonical coordinates of this rotation */ - static VectorD Logmap(const SO& R, - OptionalJacobian H = boost::none); + static TangentVector Logmap(const SO& R, ChartJacobian H = boost::none); // inverse with optional derivative using LieGroup, internal::DimensionSO(N)>::inverse; @@ -151,22 +288,22 @@ class SO : public LieGroup, internal::DimensionSO(N)> { /// @} }; -using SOn = SO; -using SO3 = SO<3>; -using SO4 = SO<4>; - /* * 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 <> SOn LieGroup::compose(const SOn& g, DynamicJacobian H1, DynamicJacobian H2) const { if (H1) *H1 = g.inverse().AdjointMap(); - if (H2) *H2 = internal::IdentitySO(g.rows()); + if (H2) *H2 = SOn::IdentityJacobian(g.rows()); return derived() * g; } @@ -175,7 +312,7 @@ SOn LieGroup::between(const SOn& g, DynamicJacobian H1, DynamicJacobian H2) const { SOn result = derived().inverse() * g; if (H1) *H1 = -result.inverse().AdjointMap(); - if (H2) *H2 = internal::IdentitySO(g.rows()); + if (H2) *H2 = SOn::IdentityJacobian(g.rows()); return result; } @@ -206,10 +343,13 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -// TEST(SOn, SO5) { -// const auto R = SOn(5); -// EXPECT_LONGS_EQUAL(5, R.rows()); -// } +TEST(SOn, SO5) { + const auto R = SOn(5); + EXPECT_LONGS_EQUAL(5, R.rows()); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim()); + EXPECT_LONGS_EQUAL(10, R.dim()); +} /* ************************************************************************* */ TEST(SOn, Concept) { @@ -218,23 +358,23 @@ TEST(SOn, Concept) { BOOST_CONCEPT_ASSERT((IsLieGroup)); } -// /* ************************************************************************* -// */ TEST(SOn, Values) { -// const auto R = SOn(5); -// Values values; -// const Key key(0); -// values.insert(key, R); -// const auto B = values.at(key); -// EXPECT_LONGS_EQUAL(5, B.rows()); -// } +/* ************************************************************************* */ +TEST(SOn, Values) { + const auto R = SOn(5); + Values values; + const Key key(0); + values.insert(key, R); + const auto B = values.at(key); + EXPECT_LONGS_EQUAL(5, B.rows()); +} -// /* ************************************************************************* -// */ TEST(SOn, Random) { -// static boost::mt19937 rng(42); -// EXPECT_LONGS_EQUAL(3, SOn::Random(rng, 3).rows()); -// EXPECT_LONGS_EQUAL(4, SOn::Random(rng, 4).rows()); -// EXPECT_LONGS_EQUAL(5, SOn::Random(rng, 5).rows()); -// } +/* ************************************************************************* */ +TEST(SOn, Random) { + static boost::mt19937 rng(42); + EXPECT_LONGS_EQUAL(3, SOn::Random(rng, 3).rows()); + EXPECT_LONGS_EQUAL(4, SOn::Random(rng, 4).rows()); + EXPECT_LONGS_EQUAL(5, SOn::Random(rng, 5).rows()); +} // /* ************************************************************************* // */ TEST(SOn, HatVee) { @@ -296,6 +436,9 @@ TEST(SOn, Concept) { TEST(SO4, Identity) { const SO4 R; EXPECT_LONGS_EQUAL(4, R.rows()); + EXPECT_LONGS_EQUAL(6, SO4::dimension); + EXPECT_LONGS_EQUAL(6, SO4::Dim()); + EXPECT_LONGS_EQUAL(6, R.dim()); } /* ************************************************************************* */ @@ -439,6 +582,9 @@ TEST(SO4, Concept) { TEST(SO3, Identity) { const SO3 R; EXPECT_LONGS_EQUAL(3, R.rows()); + EXPECT_LONGS_EQUAL(3, SO3::dimension); + EXPECT_LONGS_EQUAL(3, SO3::Dim()); + EXPECT_LONGS_EQUAL(3, R.dim()); } //****************************************************************************** @@ -448,14 +594,14 @@ TEST(SO3, Concept) { BOOST_CONCEPT_ASSERT((IsLieGroup)); } -// //****************************************************************************** -// TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } +//****************************************************************************** +TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } -// //****************************************************************************** -// SO3 id; -// Vector3 z_axis(0, 0, 1); -// SO3 R1(Eigen::AngleAxisd(0.1, z_axis)); -// SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); +//****************************************************************************** +SO3 id; +Vector3 z_axis(0, 0, 1); +SO3 R1(Eigen::AngleAxisd(0.1, z_axis)); +SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); // //****************************************************************************** // TEST(SO3, Local) { From 96392c74c0e0a948084c4c6612b0c5f072457b4f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 May 2019 02:08:29 -0400 Subject: [PATCH 17/47] Expmap and vec, specialized to SO3 and SO4 --- gtsam/geometry/tests/testSOn.cpp | 853 ++++++++++++++++++++++--------- 1 file changed, 603 insertions(+), 250 deletions(-) diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index df41b64da..9f17ce269 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -16,7 +16,9 @@ **/ #include +#include #include +#include #include #include @@ -32,6 +34,9 @@ namespace internal { 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 /** @@ -42,6 +47,7 @@ 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: @@ -97,13 +103,16 @@ class SO : public LieGroup, internal::DimensionSO(N)> { template > static SO Random(boost::mt19937& rng) { // By default, use dynamic implementation above. Specialized for SO(3). - return SO(SO::Random(rng, N).matrix_); + 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(); } @@ -270,22 +279,63 @@ class SO : public LieGroup, internal::DimensionSO(N)> { /// @name Lie Group /// @{ - MatrixDD AdjointMap() const { return MatrixDD(); } + 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); + 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); + 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; + } + /// @} }; /* @@ -326,12 +376,318 @@ 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 // #include @@ -342,7 +698,7 @@ struct traits> : public internal::LieGroup> {}; using namespace std; using namespace gtsam; -/* ************************************************************************* */ +//****************************************************************************** TEST(SOn, SO5) { const auto R = SOn(5); EXPECT_LONGS_EQUAL(5, R.rows()); @@ -351,14 +707,14 @@ TEST(SOn, SO5) { EXPECT_LONGS_EQUAL(10, R.dim()); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SOn, Concept) { BOOST_CONCEPT_ASSERT((IsGroup)); BOOST_CONCEPT_ASSERT((IsManifold)); BOOST_CONCEPT_ASSERT((IsLieGroup)); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SOn, Values) { const auto R = SOn(5); Values values; @@ -368,7 +724,7 @@ TEST(SOn, Values) { EXPECT_LONGS_EQUAL(5, B.rows()); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SOn, Random) { static boost::mt19937 rng(42); EXPECT_LONGS_EQUAL(3, SOn::Random(rng, 3).rows()); @@ -376,58 +732,57 @@ TEST(SOn, Random) { EXPECT_LONGS_EQUAL(5, SOn::Random(rng, 5).rows()); } -// /* ************************************************************************* -// */ TEST(SOn, HatVee) { -// Vector6 v; -// v << 1, 2, 3, 4, 5, 6; +//****************************************************************************** +TEST(SOn, HatVee) { + Vector6 v; + v << 1, 2, 3, 4, 5, 6; -// Matrix expected2(2, 2); -// expected2 << 0, -1, 1, 0; -// const auto actual2 = SOn::Hat(2, v.head<1>()); -// CHECK(assert_equal(expected2, actual2)); -// CHECK(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2))); + Matrix expected2(2, 2); + expected2 << 0, -1, 1, 0; + const auto actual2 = SOn::Hat(v.head<1>()); + CHECK(assert_equal(expected2, actual2)); + CHECK(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2))); -// Matrix expected3(3, 3); -// expected3 << 0, -3, 2, // -// 3, 0, -1, // -// -2, 1, 0; -// const auto actual3 = SOn::Hat(3, v.head<3>()); -// CHECK(assert_equal(expected3, actual3)); -// CHECK(assert_equal(skewSymmetric(1, 2, 3), actual3)); -// CHECK(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3))); + Matrix expected3(3, 3); + expected3 << 0, -3, 2, // + 3, 0, -1, // + -2, 1, 0; + const auto actual3 = SOn::Hat(v.head<3>()); + CHECK(assert_equal(expected3, actual3)); + CHECK(assert_equal(skewSymmetric(1, 2, 3), actual3)); + CHECK(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3))); -// Matrix expected4(4, 4); -// expected4 << 0, -6, 5, -3, // -// 6, 0, -4, 2, // -// -5, 4, 0, -1, // -// 3, -2, 1, 0; -// const auto actual4 = SOn::Hat(4, v); -// CHECK(assert_equal(expected4, actual4)); -// CHECK(assert_equal((Vector)v, SOn::Vee(actual4))); -// } + Matrix expected4(4, 4); + expected4 << 0, -6, 5, -3, // + 6, 0, -4, 2, // + -5, 4, 0, -1, // + 3, -2, 1, 0; + const auto actual4 = SOn::Hat(v); + CHECK(assert_equal(expected4, actual4)); + CHECK(assert_equal((Vector)v, SOn::Vee(actual4))); +} -// /* ************************************************************************* -// */ TEST(SOn, RetractLocal) { -// // If we do expmap in SO(3) subgroup, topleft should be equal to R1. -// Vector6 v1 = (Vector(6) << 0, 0, 0, 0.01, 0, 0).finished(); -// Matrix R1 = SO3::Retract(v1.tail<3>()); -// SOn Q1 = SOn::Retract(4, v1); -// CHECK(assert_equal(R1, Q1.block(0, 0, 3, 3), 1e-7)); -// CHECK(assert_equal(v1, SOn::Local(Q1), 1e-7)); -// } +//****************************************************************************** +TEST(SOn, RetractLocal) { + // If we do expmap in SO(3) subgroup, topleft should be equal to R1. + Vector6 v1 = (Vector(6) << 0, 0, 0, 0.01, 0, 0).finished(); + Matrix R1 = SO3::Retract(v1.tail<3>()).matrix(); + SOn Q1 = SOn::Retract(v1); + CHECK(assert_equal(R1, Q1.matrix().block(0, 0, 3, 3), 1e-7)); + CHECK(assert_equal(v1, SOn::ChartAtOrigin::Local(Q1), 1e-7)); +} -// /* ************************************************************************* -// */ TEST(SOn, vec) { -// auto x = SOn(5); -// Vector6 v; -// v << 1, 2, 3, 4, 5, 6; -// x.block(0, 0, 4, 4) = SO4::Expmap(v).matrix(); -// Matrix actualH; -// const Vector actual = x.vec(actualH); -// boost::function h = [](const SOn& x) { return x.vec(); -// }; const Matrix H = numericalDerivative11(h, x, 1e-5); -// CHECK(assert_equal(H, actualH)); -// } +//****************************************************************************** +TEST(SOn, vec) { + Vector10 v; + v << 0, 0, 0, 0, 1, 2, 3, 4, 5, 6; + SOn Q = SOn::ChartAtOrigin::Retract(v); + Matrix actualH; + const Vector actual = Q.vec(actualH); + boost::function h = [](const SOn& Q) { return Q.vec(); }; + const Matrix H = numericalDerivative11(h, Q, 1e-5); + CHECK(assert_equal(H, actualH)); +} //****************************************************************************** // SO4 @@ -441,113 +796,113 @@ TEST(SO4, Identity) { EXPECT_LONGS_EQUAL(6, R.dim()); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO4, Concept) { BOOST_CONCEPT_ASSERT((IsGroup)); BOOST_CONCEPT_ASSERT((IsManifold)); BOOST_CONCEPT_ASSERT((IsLieGroup)); } -// //****************************************************************************** -// SO4 id; -// Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished(); -// SO4 Q1 = SO4::Expmap(v1); -// Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.00, 0.00, 0.00).finished(); -// SO4 Q2 = SO4::Expmap(v2); -// Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished(); -// SO4 Q3 = SO4::Expmap(v3); +//****************************************************************************** +SO4 I4; +Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished(); +SO4 Q1 = SO4::Expmap(v1); +Vector6 v2 = (Vector(6) << 0.00, 0.00, 0.00, 0.01, 0.02, 0.03).finished(); +SO4 Q2 = SO4::Expmap(v2); +Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished(); +SO4 Q3 = SO4::Expmap(v3); -// //****************************************************************************** -// TEST(SO4, Random) { -// boost::mt19937 rng(42); -// auto Q = SO4::Random(rng); -// EXPECT_LONGS_EQUAL(4, Q.matrix().rows()); -// } -// //****************************************************************************** -// TEST(SO4, Expmap) { -// // If we do exponential map in SO(3) subgroup, topleft should be equal to -// R1. auto R1 = SO3::Expmap(v1.head<3>()); -// EXPECT((Q1.topLeft().isApprox(R1))); +//****************************************************************************** +TEST(SO4, Random) { + boost::mt19937 rng(42); + auto Q = SO4::Random(rng); + EXPECT_LONGS_EQUAL(4, Q.matrix().rows()); +} +//****************************************************************************** +TEST(SO4, Expmap) { + // If we do exponential map in SO(3) subgroup, topleft should be equal to R1. + auto R1 = SO3::Expmap(v1.tail<3>()).matrix(); + EXPECT((Q1.matrix().topLeftCorner<3, 3>().isApprox(R1))); -// // Same here -// auto R2 = SO3::Expmap(v2.head<3>()); -// EXPECT((Q2.topLeft().isApprox(R2))); + // Same here + auto R2 = SO3::Expmap(v2.tail<3>()).matrix(); + EXPECT((Q2.matrix().topLeftCorner<3, 3>().isApprox(R2))); -// // Check commutative subgroups -// for (size_t i = 0; i < 6; i++) { -// Vector6 xi = Vector6::Zero(); -// xi[i] = 2; -// SO4 Q1 = SO4::Expmap(xi); -// xi[i] = 3; -// SO4 Q2 = SO4::Expmap(xi); -// EXPECT(((Q1 * Q2).isApprox(Q2 * Q1))); -// } -// } + // Check commutative subgroups + for (size_t i = 0; i < 6; i++) { + Vector6 xi = Vector6::Zero(); + xi[i] = 2; + SO4 Q1 = SO4::Expmap(xi); + xi[i] = 3; + SO4 Q2 = SO4::Expmap(xi); + EXPECT(assert_equal(Q1 * Q2, Q2 * Q1)); + } +} -// //****************************************************************************** -// TEST(SO4, Cayley) { -// CHECK(assert_equal(id.retract(v1 / 100), SO4::Expmap(v1 / 100))); -// CHECK(assert_equal(id.retract(v2 / 100), SO4::Expmap(v2 / 100))); -// } +//****************************************************************************** +TEST(SO4, Cayley) { + CHECK(assert_equal(I4.retract(v1 / 100), SO4::Expmap(v1 / 100))); + CHECK(assert_equal(I4.retract(v2 / 100), SO4::Expmap(v2 / 100))); +} -// //****************************************************************************** -// TEST(SO4, Retract) { -// auto v = Vector6::Zero(); -// SO4 actual = traits::Retract(id, v); -// EXPECT(actual.isApprox(id)); -// } +//****************************************************************************** +TEST(SO4, Retract) { + auto v = Vector6::Zero(); + SO4 actual = traits::Retract(I4, v); + EXPECT(assert_equal(I4, actual)); +} -// //****************************************************************************** -// TEST(SO4, Local) { -// auto v0 = Vector6::Zero(); -// Vector6 actual = traits::Local(id, id); -// EXPECT(assert_equal((Vector)v0, actual)); -// } +//****************************************************************************** +TEST(SO4, Local) { + auto v0 = Vector6::Zero(); + Vector6 actual = traits::Local(I4, I4); + EXPECT(assert_equal((Vector)v0, actual)); +} -// //****************************************************************************** -// TEST(SO4, Invariants) { -// EXPECT(check_group_invariants(id, id)); -// EXPECT(check_group_invariants(id, Q1)); -// EXPECT(check_group_invariants(Q2, id)); -// EXPECT(check_group_invariants(Q2, Q1)); -// EXPECT(check_group_invariants(Q1, Q2)); +//****************************************************************************** +TEST(SO4, Invariants) { + EXPECT(check_group_invariants(I4, I4)); + EXPECT(check_group_invariants(I4, Q1)); + EXPECT(check_group_invariants(Q2, I4)); + EXPECT(check_group_invariants(Q2, Q1)); + EXPECT(check_group_invariants(Q1, Q2)); -// EXPECT(check_manifold_invariants(id, id)); -// EXPECT(check_manifold_invariants(id, Q1)); -// EXPECT(check_manifold_invariants(Q2, id)); -// EXPECT(check_manifold_invariants(Q2, Q1)); -// EXPECT(check_manifold_invariants(Q1, Q2)); -// } + EXPECT(check_manifold_invariants(I4, I4)); + EXPECT(check_manifold_invariants(I4, Q1)); + EXPECT(check_manifold_invariants(Q2, I4)); + EXPECT(check_manifold_invariants(Q2, Q1)); + EXPECT(check_manifold_invariants(Q1, Q2)); +} -// /* ************************************************************************* -// */ TEST(SO4, compose) { -// SO4 expected = Q1 * Q2; -// Matrix actualH1, actualH2; -// SO4 actual = Q1.compose(Q2, actualH1, actualH2); -// CHECK(assert_equal(expected, actual)); +//****************************************************************************** +TEST(SO4, compose) { + SO4 expected = Q1 * Q2; + Matrix actualH1, actualH2; + SO4 actual = Q1.compose(Q2, actualH1, actualH2); + CHECK(assert_equal(expected, actual)); -// Matrix numericalH1 = -// numericalDerivative21(testing::compose, Q1, Q2, 1e-2); -// CHECK(assert_equal(numericalH1, actualH1)); + Matrix numericalH1 = + numericalDerivative21(testing::compose, Q1, Q2, 1e-2); + CHECK(assert_equal(numericalH1, actualH1)); -// Matrix numericalH2 = -// numericalDerivative22(testing::compose, Q1, Q2, 1e-2); -// CHECK(assert_equal(numericalH2, actualH2)); -// } + Matrix numericalH2 = + numericalDerivative22(testing::compose, Q1, Q2, 1e-2); + CHECK(assert_equal(numericalH2, actualH2)); +} -// /* ************************************************************************* -// */ TEST(SO4, vec) { -// using Vector16 = SO4::Vector16; -// const Vector16 expected = Eigen::Map(Q2.data()); -// Matrix actualH; -// const Vector16 actual = Q2.vec(actualH); -// CHECK(assert_equal(expected, actual)); -// boost::function f = [](const SO4& Q) { -// return Q.vec(); -// }; -// const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5); -// CHECK(assert_equal(numericalH, actualH)); -// } +//****************************************************************************** +TEST(SO4, vec) { + using Vector16 = SO4::VectorN2; + const Vector16 expected = Eigen::Map(Q2.matrix().data()); + Matrix actualH; + const Vector16 actual = Q2.vec(actualH); + CHECK(assert_equal(expected, actual)); + boost::function f = [](const SO4& Q) { + return Q.vec(); + }; + const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5); + CHECK(assert_equal(numericalH, actualH)); +} // /* ************************************************************************* // */ TEST(SO4, topLeft) { @@ -598,56 +953,57 @@ TEST(SO3, Concept) { TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } //****************************************************************************** -SO3 id; +SO3 I3; Vector3 z_axis(0, 0, 1); SO3 R1(Eigen::AngleAxisd(0.1, z_axis)); SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); -// //****************************************************************************** -// TEST(SO3, Local) { +//****************************************************************************** +// TEST(SO3, Logmap) { // Vector3 expected(0, 0, 0.1); -// Vector3 actual = traits::Local(R1, R2); +// Vector3 actual = SO3::Logmap(R1.between(R2)); // EXPECT(assert_equal((Vector)expected, actual)); // } -// //****************************************************************************** -// TEST(SO3, Retract) { -// Vector3 v(0, 0, 0.1); -// SO3 actual = traits::Retract(R1, v); -// EXPECT(actual.isApprox(R2)); -// } +//****************************************************************************** +TEST(SO3, Expmap) { + Vector3 v(0, 0, 0.1); + SO3 actual = R1 * SO3::Expmap(v); + EXPECT(assert_equal(R2, actual)); +} -// //****************************************************************************** -// TEST(SO3, Invariants) { -// EXPECT(check_group_invariants(id, id)); -// EXPECT(check_group_invariants(id, R1)); -// EXPECT(check_group_invariants(R2, id)); -// EXPECT(check_group_invariants(R2, R1)); +//****************************************************************************** +TEST(SO3, Invariants) { + EXPECT(check_group_invariants(I3, I3)); + EXPECT(check_group_invariants(I3, R1)); + EXPECT(check_group_invariants(R2, I3)); + EXPECT(check_group_invariants(R2, R1)); -// EXPECT(check_manifold_invariants(id, id)); -// EXPECT(check_manifold_invariants(id, R1)); -// EXPECT(check_manifold_invariants(R2, id)); -// EXPECT(check_manifold_invariants(R2, R1)); -// } + EXPECT(check_manifold_invariants(I3, I3)); + EXPECT(check_manifold_invariants(I3, R1)); + EXPECT(check_manifold_invariants(R2, I3)); + EXPECT(check_manifold_invariants(R2, R1)); +} -// //****************************************************************************** +//****************************************************************************** // TEST(SO3, LieGroupDerivatives) { -// CHECK_LIE_GROUP_DERIVATIVES(id, id); -// CHECK_LIE_GROUP_DERIVATIVES(id, R2); -// CHECK_LIE_GROUP_DERIVATIVES(R2, id); +// CHECK_LIE_GROUP_DERIVATIVES(I3, I3); +// CHECK_LIE_GROUP_DERIVATIVES(I3, R2); +// CHECK_LIE_GROUP_DERIVATIVES(R2, I3); // CHECK_LIE_GROUP_DERIVATIVES(R2, R1); // } -// //****************************************************************************** +//****************************************************************************** // TEST(SO3, ChartDerivatives) { -// CHECK_CHART_DERIVATIVES(id, id); -// CHECK_CHART_DERIVATIVES(id, R2); -// CHECK_CHART_DERIVATIVES(R2, id); +// CHECK_CHART_DERIVATIVES(I3, I3); +// CHECK_CHART_DERIVATIVES(I3, R2); +// CHECK_CHART_DERIVATIVES(R2, I3); // CHECK_CHART_DERIVATIVES(R2, R1); // } -// /* ************************************************************************* -// */ namespace exmap_derivative { static const Vector3 w(0.1, 0.27, -0.2); +// //****************************************************************************** +// namespace exmap_derivative { +// static const Vector3 w(0.1, 0.27, -0.2); // } // // Left trivialized Derivative of exp(w) wrpt w: // // How does exp(w) change when w changes? @@ -670,8 +1026,8 @@ SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); // EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-7)); // } -// /* ************************************************************************* -// */ TEST(SO3, ExpmapDerivative2) { +// //****************************************************************************** +// TEST(SO3, ExpmapDerivative2) { // const Vector3 theta(0.1, 0, 0.1); // const Matrix Jexpected = numericalDerivative11( // boost::bind(&SO3::Expmap, _1, boost::none), theta); @@ -681,8 +1037,8 @@ SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); // SO3::ExpmapDerivative(-theta))); // } -// /* ************************************************************************* -// */ TEST(SO3, ExpmapDerivative3) { +// //****************************************************************************** +// TEST(SO3, ExpmapDerivative3) { // const Vector3 theta(10, 20, 30); // const Matrix Jexpected = numericalDerivative11( // boost::bind(&SO3::Expmap, _1, boost::none), theta); @@ -692,8 +1048,8 @@ SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); // SO3::ExpmapDerivative(-theta))); // } -// /* ************************************************************************* -// */ TEST(SO3, ExpmapDerivative4) { +// //****************************************************************************** +// TEST(SO3, ExpmapDerivative4) { // // Iserles05an (Lie-group Methods) says: // // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) // // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t) @@ -720,8 +1076,8 @@ SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); // } // } -// /* ************************************************************************* -// */ TEST(SO3, ExpmapDerivative5) { +// //****************************************************************************** +// TEST(SO3, ExpmapDerivative5) { // auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; // auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; @@ -736,8 +1092,8 @@ SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); // } // } -// /* ************************************************************************* -// */ TEST(SO3, ExpmapDerivative6) { +// //****************************************************************************** +// TEST(SO3, ExpmapDerivative6) { // const Vector3 thetahat(0.1, 0, 0.1); // const Matrix Jexpected = numericalDerivative11( // boost::bind(&SO3::Expmap, _1, boost::none), thetahat); @@ -747,7 +1103,8 @@ SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); // } // /* ************************************************************************* -// */ TEST(SO3, LogmapDerivative) { +// */ +// TEST(SO3, LogmapDerivative) { // const Vector3 thetahat(0.1, 0, 0.1); // const SO3 R = SO3::Expmap(thetahat); // some rotation // const Matrix Jexpected = numericalDerivative11( @@ -756,8 +1113,8 @@ SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); // EXPECT(assert_equal(Jexpected, Jactual)); // } -// /* ************************************************************************* -// */ TEST(SO3, JacobianLogmap) { +// //****************************************************************************** +// TEST(SO3, JacobianLogmap) { // const Vector3 thetahat(0.1, 0, 0.1); // const SO3 R = SO3::Expmap(thetahat); // some rotation // const Matrix Jexpected = numericalDerivative11( @@ -767,67 +1124,63 @@ SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); // EXPECT(assert_equal(Jexpected, Jactual)); // } -// /* ************************************************************************* -// */ TEST(SO3, ApplyDexp) { -// Matrix aH1, aH2; -// for (bool nearZeroApprox : {true, false}) { -// boost::function f = -// [=](const Vector3& omega, const Vector3& v) { -// return so3::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); -// 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), -// local.applyDexp(v, aH1, aH2))); -// EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); -// EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); -// EXPECT(assert_equal(local.dexp(), aH2)); -// } -// } -// } -// } +//****************************************************************************** +TEST(SO3, ApplyDexp) { + Matrix aH1, aH2; + for (bool nearZeroApprox : {true, false}) { + boost::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::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); + 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), + local.applyDexp(v, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); + EXPECT(assert_equal(local.dexp(), aH2)); + } + } + } +} -// /* ************************************************************************* -// */ TEST(SO3, ApplyInvDexp) { -// Matrix aH1, aH2; -// for (bool nearZeroApprox : {true, false}) { -// boost::function f = -// [=](const Vector3& omega, const Vector3& v) { -// return so3::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); -// 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)}) { -// EXPECT(assert_equal(Vector3(invDexp * v), -// local.applyInvDexp(v, aH1, aH2))); -// EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); -// EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); -// EXPECT(assert_equal(invDexp, aH2)); -// } -// } -// } -// } +//****************************************************************************** +TEST(SO3, ApplyInvDexp) { + Matrix aH1, aH2; + for (bool nearZeroApprox : {true, false}) { + boost::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::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); + 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)}) { + EXPECT(assert_equal(Vector3(invDexp * v), + local.applyInvDexp(v, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); + EXPECT(assert_equal(invDexp, aH2)); + } + } + } +} -// /* ************************************************************************* -// */ TEST(SO3, vec) { -// const Vector9 expected = Eigen::Map(R2.data()); -// Matrix actualH; -// const Vector9 actual = R2.vec(actualH); -// CHECK(assert_equal(expected, actual)); -// boost::function f = [](const SO3& Q) { -// return Q.vec(); -// }; -// const Matrix numericalH = numericalDerivative11(f, R2, 1e-5); -// CHECK(assert_equal(numericalH, actualH)); -// } +//****************************************************************************** +TEST(SO3, vec) { + const Vector9 expected = Eigen::Map(R2.matrix().data()); + Matrix actualH; + const Vector9 actual = R2.vec(actualH); + CHECK(assert_equal(expected, actual)); + boost::function f = [](const SO3& Q) { return Q.vec(); }; + const Matrix numericalH = numericalDerivative11(f, R2, 1e-5); + CHECK(assert_equal(numericalH, actualH)); +} // //****************************************************************************** // TEST(Matrix, compose) { From 6cff36975ffe01da2d38166fb27501c982cd6d2d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 May 2019 11:07:09 -0400 Subject: [PATCH 18/47] 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)); From 7213fd2c62b3a8f2b9402c2c20c32a364714846f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 May 2019 15:37:04 -0400 Subject: [PATCH 19/47] Distributed SOn functionality over three files --- gtsam/geometry/SOn-inl.h | 127 +++++++++ gtsam/geometry/SOn.cpp | 40 +++ gtsam/geometry/SOn.h | 113 ++------ gtsam/geometry/tests/testSOn.cpp | 436 ------------------------------- 4 files changed, 189 insertions(+), 527 deletions(-) create mode 100644 gtsam/geometry/SOn-inl.h create mode 100644 gtsam/geometry/SOn.cpp diff --git a/gtsam/geometry/SOn-inl.h b/gtsam/geometry/SOn-inl.h new file mode 100644 index 000000000..26dc2229b --- /dev/null +++ b/gtsam/geometry/SOn-inl.h @@ -0,0 +1,127 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, 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 + + * -------------------------------------------------------------------------- */ + +#pragma once + +/** + * @file SOn-inl.h + * @brief Template implementations for SO(n) + * @author Frank Dellaert + * @date March 2019 + */ + +#include + +namespace gtsam { + +template +Matrix SO::Hat(const Vector& xi) { + size_t n = AmbientDim(xi.size()); + if (n < 2) throw std::invalid_argument("SO::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; +} + +template +Vector SO::Vee(const Matrix& X) { + const size_t n = X.rows(); + if (n < 2) throw std::invalid_argument("SO::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; + } +} + +template +SO SO::ChartAtOrigin::Retract(const TangentVector& xi, ChartJacobian H) { + 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()); +} + +template +typename SO::TangentVector SO::ChartAtOrigin::Local(const SO& R, + ChartJacobian H) { + 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); +} + +template +typename SO::VectorN2 SO::vec( + OptionalJacobian H) 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; +} + +} // namespace gtsam diff --git a/gtsam/geometry/SOn.cpp b/gtsam/geometry/SOn.cpp new file mode 100644 index 000000000..67b780db8 --- /dev/null +++ b/gtsam/geometry/SOn.cpp @@ -0,0 +1,40 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, 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 SOn.cpp + * @brief Definitions of dynamic specializations of SO(n) + * @author Frank Dellaert + * @date March 2019 + */ + +#include + +namespace gtsam { + +template <> +SOn LieGroup::compose(const SOn& g, DynamicJacobian H1, + DynamicJacobian H2) const { + if (H1) *H1 = g.inverse().AdjointMap(); + if (H2) *H2 = SOn::IdentityJacobian(g.rows()); + return derived() * g; +} + +template <> +SOn LieGroup::between(const SOn& g, DynamicJacobian H1, + DynamicJacobian H2) const { + SOn result = derived().inverse() * g; + if (H1) *H1 = -result.inverse().AdjointMap(); + if (H2) *H2 = SOn::IdentityJacobian(g.rows()); + return result; +} + +} // namespace gtsam diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 4f4fcd157..64718ed5f 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -188,64 +188,12 @@ class SO : public LieGroup, internal::DimensionSO(N)> { * -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; - } + static Matrix Hat(const Vector& xi); /** * 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; - } - } + static Vector Vee(const Matrix& X); // Chart at origin struct ChartAtOrigin { @@ -253,21 +201,12 @@ class SO : public LieGroup, internal::DimensionSO(N)> { * 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()); - } + static SO Retract(const TangentVector& xi, ChartJacobian H = boost::none); + /** * 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); - } + static TangentVector Local(const SO& R, ChartJacobian H = boost::none); }; // Return dynamic identity DxD Jacobian for given SO(n) @@ -300,6 +239,9 @@ class SO : public LieGroup, internal::DimensionSO(N)> { throw std::runtime_error("SO::Logmap only implemented for SO3 and SO4."); } + // template > + static Matrix3 LogmapDerivative(const Vector3& omega); + // inverse with optional derivative using LieGroup, internal::DimensionSO(N)>::inverse; @@ -313,39 +255,26 @@ class SO : public LieGroup, internal::DimensionSO(N)> { * 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; - } + boost::none) const; /// @} }; +using SOn = SO; + /* * Fully specialize compose and between, because the derivative is unknowable by * the LieGroup implementations, who return a fixed-size matrix for H2. */ -using SOn = SO; +using DynamicJacobian = OptionalJacobian; + +template <> +SOn LieGroup::compose(const SOn& g, DynamicJacobian H1, + DynamicJacobian H2) const; + +template <> +SOn LieGroup::between(const SOn& g, DynamicJacobian H1, + DynamicJacobian H2) const; /* * Define the traits. internal::LieGroup provides both Lie group and Testable @@ -358,3 +287,5 @@ template struct traits> : public internal::LieGroup> {}; } // namespace gtsam + +#include "SOn-inl.h" \ No newline at end of file diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index 30f20b495..1e4aee927 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -15,7 +15,6 @@ * @author Frank Dellaert **/ -#include #include #include @@ -36,27 +35,6 @@ #include #include -namespace gtsam { -using DynamicJacobian = OptionalJacobian; - -template <> -SOn LieGroup::compose(const SOn& g, DynamicJacobian H1, - DynamicJacobian H2) const { - if (H1) *H1 = g.inverse().AdjointMap(); - if (H2) *H2 = SOn::IdentityJacobian(g.rows()); - return derived() * g; -} - -template <> -SOn LieGroup::between(const SOn& g, DynamicJacobian H1, - DynamicJacobian H2) const { - SOn result = derived().inverse() * g; - if (H1) *H1 = -result.inverse().AdjointMap(); - if (H2) *H2 = SOn::IdentityJacobian(g.rows()); - return result; -} -} // namespace gtsam - using namespace std; using namespace gtsam; @@ -146,420 +124,6 @@ TEST(SOn, vec) { CHECK(assert_equal(H, actualH)); } -//****************************************************************************** -// SO4 -//****************************************************************************** - -TEST(SO4, Identity) { - const SO4 R; - EXPECT_LONGS_EQUAL(4, R.rows()); - EXPECT_LONGS_EQUAL(6, SO4::dimension); - EXPECT_LONGS_EQUAL(6, SO4::Dim()); - EXPECT_LONGS_EQUAL(6, R.dim()); -} - -//****************************************************************************** -TEST(SO4, Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); -} - -//****************************************************************************** -SO4 I4; -Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished(); -SO4 Q1 = SO4::Expmap(v1); -Vector6 v2 = (Vector(6) << 0.00, 0.00, 0.00, 0.01, 0.02, 0.03).finished(); -SO4 Q2 = SO4::Expmap(v2); -Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished(); -SO4 Q3 = SO4::Expmap(v3); - -//****************************************************************************** -TEST(SO4, Random) { - boost::mt19937 rng(42); - auto Q = SO4::Random(rng); - EXPECT_LONGS_EQUAL(4, Q.matrix().rows()); -} -//****************************************************************************** -TEST(SO4, Expmap) { - // If we do exponential map in SO(3) subgroup, topleft should be equal to R1. - auto R1 = SO3::Expmap(v1.tail<3>()).matrix(); - EXPECT((Q1.matrix().topLeftCorner<3, 3>().isApprox(R1))); - - // Same here - auto R2 = SO3::Expmap(v2.tail<3>()).matrix(); - EXPECT((Q2.matrix().topLeftCorner<3, 3>().isApprox(R2))); - - // Check commutative subgroups - for (size_t i = 0; i < 6; i++) { - Vector6 xi = Vector6::Zero(); - xi[i] = 2; - SO4 Q1 = SO4::Expmap(xi); - xi[i] = 3; - SO4 Q2 = SO4::Expmap(xi); - EXPECT(assert_equal(Q1 * Q2, Q2 * Q1)); - } -} - -//****************************************************************************** -TEST(SO4, Cayley) { - CHECK(assert_equal(I4.retract(v1 / 100), SO4::Expmap(v1 / 100))); - CHECK(assert_equal(I4.retract(v2 / 100), SO4::Expmap(v2 / 100))); -} - -//****************************************************************************** -TEST(SO4, Retract) { - auto v = Vector6::Zero(); - SO4 actual = traits::Retract(I4, v); - EXPECT(assert_equal(I4, actual)); -} - -//****************************************************************************** -TEST(SO4, Local) { - auto v0 = Vector6::Zero(); - Vector6 actual = traits::Local(I4, I4); - EXPECT(assert_equal((Vector)v0, actual)); -} - -//****************************************************************************** -TEST(SO4, Invariants) { - EXPECT(check_group_invariants(I4, I4)); - EXPECT(check_group_invariants(I4, Q1)); - EXPECT(check_group_invariants(Q2, I4)); - EXPECT(check_group_invariants(Q2, Q1)); - EXPECT(check_group_invariants(Q1, Q2)); - - EXPECT(check_manifold_invariants(I4, I4)); - EXPECT(check_manifold_invariants(I4, Q1)); - EXPECT(check_manifold_invariants(Q2, I4)); - EXPECT(check_manifold_invariants(Q2, Q1)); - EXPECT(check_manifold_invariants(Q1, Q2)); -} - -//****************************************************************************** -TEST(SO4, compose) { - SO4 expected = Q1 * Q2; - Matrix actualH1, actualH2; - SO4 actual = Q1.compose(Q2, actualH1, actualH2); - CHECK(assert_equal(expected, actual)); - - Matrix numericalH1 = - numericalDerivative21(testing::compose, Q1, Q2, 1e-2); - CHECK(assert_equal(numericalH1, actualH1)); - - Matrix numericalH2 = - numericalDerivative22(testing::compose, Q1, Q2, 1e-2); - CHECK(assert_equal(numericalH2, actualH2)); -} - -//****************************************************************************** -TEST(SO4, vec) { - using Vector16 = SO4::VectorN2; - const Vector16 expected = Eigen::Map(Q2.matrix().data()); - Matrix actualH; - const Vector16 actual = Q2.vec(actualH); - CHECK(assert_equal(expected, actual)); - boost::function f = [](const SO4& Q) { - return Q.vec(); - }; - const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5); - CHECK(assert_equal(numericalH, actualH)); -} - -// /* ************************************************************************* -// */ TEST(SO4, topLeft) { -// const Matrix3 expected = Q3.topLeftCorner<3, 3>(); -// Matrix actualH; -// const Matrix3 actual = Q3.topLeft(actualH); -// CHECK(assert_equal(expected, actual)); -// boost::function f = [](const SO4& Q3) { -// return Q3.topLeft(); -// }; -// const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); -// CHECK(assert_equal(numericalH, actualH)); -// } - -// /* ************************************************************************* -// */ TEST(SO4, stiefel) { -// const Matrix43 expected = Q3.leftCols<3>(); -// Matrix actualH; -// const Matrix43 actual = Q3.stiefel(actualH); -// CHECK(assert_equal(expected, actual)); -// boost::function f = [](const SO4& Q3) { -// return Q3.stiefel(); -// }; -// const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); -// CHECK(assert_equal(numericalH, actualH)); -// } - -//****************************************************************************** -// SO3 -//****************************************************************************** - -TEST(SO3, Identity) { - const SO3 R; - EXPECT_LONGS_EQUAL(3, R.rows()); - EXPECT_LONGS_EQUAL(3, SO3::dimension); - EXPECT_LONGS_EQUAL(3, SO3::Dim()); - EXPECT_LONGS_EQUAL(3, R.dim()); -} - -//****************************************************************************** -TEST(SO3, Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); -} - -//****************************************************************************** -TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } - -//****************************************************************************** -SO3 I3; -Vector3 z_axis(0, 0, 1); -SO3 R1(Eigen::AngleAxisd(0.1, z_axis)); -SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); - -//****************************************************************************** -// TEST(SO3, Logmap) { -// Vector3 expected(0, 0, 0.1); -// Vector3 actual = SO3::Logmap(R1.between(R2)); -// EXPECT(assert_equal((Vector)expected, actual)); -// } - -//****************************************************************************** -TEST(SO3, Expmap) { - Vector3 v(0, 0, 0.1); - SO3 actual = R1 * SO3::Expmap(v); - EXPECT(assert_equal(R2, actual)); -} - -//****************************************************************************** -TEST(SO3, Invariants) { - EXPECT(check_group_invariants(I3, I3)); - EXPECT(check_group_invariants(I3, R1)); - EXPECT(check_group_invariants(R2, I3)); - EXPECT(check_group_invariants(R2, R1)); - - EXPECT(check_manifold_invariants(I3, I3)); - EXPECT(check_manifold_invariants(I3, R1)); - EXPECT(check_manifold_invariants(R2, I3)); - EXPECT(check_manifold_invariants(R2, R1)); -} - -//****************************************************************************** -// TEST(SO3, LieGroupDerivatives) { -// CHECK_LIE_GROUP_DERIVATIVES(I3, I3); -// CHECK_LIE_GROUP_DERIVATIVES(I3, R2); -// CHECK_LIE_GROUP_DERIVATIVES(R2, I3); -// CHECK_LIE_GROUP_DERIVATIVES(R2, R1); -// } - -//****************************************************************************** -// TEST(SO3, ChartDerivatives) { -// CHECK_CHART_DERIVATIVES(I3, I3); -// CHECK_CHART_DERIVATIVES(I3, R2); -// CHECK_CHART_DERIVATIVES(R2, I3); -// CHECK_CHART_DERIVATIVES(R2, R1); -// } - -// //****************************************************************************** -// namespace exmap_derivative { -// static const Vector3 w(0.1, 0.27, -0.2); -// } -// // Left trivialized Derivative of exp(w) wrpt w: -// // How does exp(w) change when w changes? -// // We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 -// // => y = log (exp(-w) * exp(w+dw)) -// Vector3 testDexpL(const Vector3& dw) { -// using exmap_derivative::w; -// return SO3::Logmap(SO3::Expmap(-w) * SO3::Expmap(w + dw)); -// } - -// TEST(SO3, ExpmapDerivative) { -// using exmap_derivative::w; -// const Matrix actualDexpL = SO3::ExpmapDerivative(w); -// const Matrix expectedDexpL = -// numericalDerivative11(testDexpL, Vector3::Zero(), -// 1e-2); -// EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-7)); - -// const Matrix actualDexpInvL = SO3::LogmapDerivative(w); -// EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-7)); -// } - -// //****************************************************************************** -// TEST(SO3, ExpmapDerivative2) { -// const Vector3 theta(0.1, 0, 0.1); -// const Matrix Jexpected = numericalDerivative11( -// boost::bind(&SO3::Expmap, _1, boost::none), theta); - -// CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); -// CHECK(assert_equal(Matrix3(Jexpected.transpose()), -// SO3::ExpmapDerivative(-theta))); -// } - -// //****************************************************************************** -// TEST(SO3, ExpmapDerivative3) { -// const Vector3 theta(10, 20, 30); -// const Matrix Jexpected = numericalDerivative11( -// boost::bind(&SO3::Expmap, _1, boost::none), theta); - -// CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); -// CHECK(assert_equal(Matrix3(Jexpected.transpose()), -// SO3::ExpmapDerivative(-theta))); -// } - -// //****************************************************************************** -// TEST(SO3, ExpmapDerivative4) { -// // Iserles05an (Lie-group Methods) says: -// // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) -// // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t) -// // where A(t): R -> so(3) is a trajectory in the tangent space of SO(3) -// // and dexp[A] is a linear map from 3*3 to 3*3 derivatives of se(3) -// // Hence, the above matrix equation is typed: 3*3 = SO(3) * linear_map(3*3) - -// // In GTSAM, we don't work with the skew-symmetric matrices A directly, but -// // with 3-vectors. -// // omega is easy: d Expmap(w(t)) / dt = ExmapDerivative[w(t)] * w'(t) - -// // Let's verify the above formula. - -// auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; -// auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; - -// // We define a function R -// auto R = [w](double t) { return SO3::Expmap(w(t)); }; - -// for (double t = -2.0; t < 2.0; t += 0.3) { -// const Matrix expected = numericalDerivative11(R, t); -// const Matrix actual = SO3::ExpmapDerivative(w(t)) * w_dot(t); -// CHECK(assert_equal(expected, actual, 1e-7)); -// } -// } - -// //****************************************************************************** -// TEST(SO3, ExpmapDerivative5) { -// auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; -// auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; - -// // Now define R as mapping local coordinates to neighborhood around R0. -// const SO3 R0 = SO3::Expmap(Vector3(0.1, 0.4, 0.2)); -// auto R = [R0, w](double t) { return R0.expmap(w(t)); }; - -// for (double t = -2.0; t < 2.0; t += 0.3) { -// const Matrix expected = numericalDerivative11(R, t); -// const Matrix actual = SO3::ExpmapDerivative(w(t)) * w_dot(t); -// CHECK(assert_equal(expected, actual, 1e-7)); -// } -// } - -// //****************************************************************************** -// TEST(SO3, ExpmapDerivative6) { -// const Vector3 thetahat(0.1, 0, 0.1); -// const Matrix Jexpected = numericalDerivative11( -// boost::bind(&SO3::Expmap, _1, boost::none), thetahat); -// Matrix3 Jactual; -// SO3::Expmap(thetahat, Jactual); -// EXPECT(assert_equal(Jexpected, Jactual)); -// } - -// /* ************************************************************************* -// */ -// TEST(SO3, LogmapDerivative) { -// const Vector3 thetahat(0.1, 0, 0.1); -// const SO3 R = SO3::Expmap(thetahat); // some rotation -// const Matrix Jexpected = numericalDerivative11( -// boost::bind(&SO3::Logmap, _1, boost::none), R); -// const Matrix3 Jactual = SO3::LogmapDerivative(thetahat); -// EXPECT(assert_equal(Jexpected, Jactual)); -// } - -// //****************************************************************************** -// TEST(SO3, JacobianLogmap) { -// const Vector3 thetahat(0.1, 0, 0.1); -// const SO3 R = SO3::Expmap(thetahat); // some rotation -// const Matrix Jexpected = numericalDerivative11( -// boost::bind(&SO3::Logmap, _1, boost::none), R); -// Matrix3 Jactual; -// SO3::Logmap(R, Jactual); -// EXPECT(assert_equal(Jexpected, Jactual)); -// } - -//****************************************************************************** -TEST(SO3, ApplyDexp) { - Matrix aH1, aH2; - for (bool nearZeroApprox : {true, false}) { - boost::function f = - [=](const Vector3& omega, const Vector3& 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)}) { - 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), - local.applyDexp(v, aH1, aH2))); - EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); - EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); - EXPECT(assert_equal(local.dexp(), aH2)); - } - } - } -} - -//****************************************************************************** -TEST(SO3, ApplyInvDexp) { - Matrix aH1, aH2; - for (bool nearZeroApprox : {true, false}) { - boost::function f = - [=](const Vector3& omega, const Vector3& 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)}) { - 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)}) { - EXPECT(assert_equal(Vector3(invDexp * v), - local.applyInvDexp(v, aH1, aH2))); - EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); - EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); - EXPECT(assert_equal(invDexp, aH2)); - } - } - } -} - -//****************************************************************************** -TEST(SO3, vec) { - const Vector9 expected = Eigen::Map(R2.matrix().data()); - Matrix actualH; - const Vector9 actual = R2.vec(actualH); - CHECK(assert_equal(expected, actual)); - boost::function f = [](const SO3& Q) { return Q.vec(); }; - const Matrix numericalH = numericalDerivative11(f, R2, 1e-5); - CHECK(assert_equal(numericalH, actualH)); -} - -// //****************************************************************************** -// TEST(Matrix, compose) { -// Matrix3 M; -// M << 1, 2, 3, 4, 5, 6, 7, 8, 9; -// SO3 R = SO3::Expmap(Vector3(1, 2, 3)); -// const Matrix3 expected = M * R.matrix(); -// Matrix actualH; -// const Matrix3 actual = sot::compose(M, R, actualH); -// CHECK(assert_equal(expected, actual)); -// boost::function f = [R](const Matrix3& M) { -// return sot::compose(M, R); -// }; -// Matrix numericalH = numericalDerivative11(f, M, 1e-2); -// CHECK(assert_equal(numericalH, actualH)); -// } - //****************************************************************************** int main() { TestResult tr; From 095071cf36ea99b8c897ef7bbf3093c6aee2835c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 May 2019 15:37:38 -0400 Subject: [PATCH 20/47] Rescued some specializations for SO3 --- gtsam/geometry/SOt.cpp | 171 +++++++++++++++++++++-------------------- gtsam/geometry/SOt.h | 70 ++++++----------- 2 files changed, 111 insertions(+), 130 deletions(-) diff --git a/gtsam/geometry/SOt.cpp b/gtsam/geometry/SOt.cpp index 64be9acbc..9249715e8 100644 --- a/gtsam/geometry/SOt.cpp +++ b/gtsam/geometry/SOt.cpp @@ -133,12 +133,12 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, } // 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 = @@ -146,7 +146,7 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, // 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) @@ -170,6 +170,7 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, // return xi; // } +//****************************************************************************** template <> SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { if (H) { @@ -185,97 +186,101 @@ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { // return sot::DexpFunctor(omega).dexp(); // } -/* ************************************************************************* */ -// Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { -// using std::sin; -// using std::sqrt; +//****************************************************************************** +/* 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. -// // 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); + logmap( Rhat * expmap(omega) ) \approx logmap(Rhat) + Jrinv * omega -// // Get trace(R) -// const double tr = R.trace(); + where Jrinv = LogmapDerivative(omega). This maps a perturbation on the + manifold (expmap(omega)) to a perturbation in the tangent space (Jrinv * + omega) + */ +template <> +Matrix3 SO3::LogmapDerivative(const Vector3& omega) { + using std::cos; + using std::sin; -// Vector3 omega; + double theta2 = omega.dot(omega); + if (theta2 <= std::numeric_limits::epsilon()) return I_3x3; + double theta = std::sqrt(theta2); // rotation angle -// // 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); -// } + // element of Lie algebra so(3): W = omega^ + const Matrix3 W = Hat(omega); + return I_3x3 + 0.5 * W + + (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * + W * W; +} -// if (H) *H = LogmapDerivative(omega); -// return omega; -// } +//****************************************************************************** +template <> +Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { + using std::sin; + using std::sqrt; -/* ************************************************************************* */ -// Matrix3 SO3::LogmapDerivative(const Vector3& omega) { -// using std::cos; -// using std::sin; + // note switch to base 1 + const Matrix3& R = Q.matrix(); + 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); -// 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; -// } + // Get trace(R) + const double tr = R.trace(); -/* ************************************************************************* */ -// static Vector9 vec(const SO3& R) { return Eigen::Map(R.data()); } + Vector3 omega; -// static const std::vector G({SO3::Hat(Vector3::Unit(0)), -// SO3::Hat(Vector3::Unit(1)), -// SO3::Hat(Vector3::Unit(2))}); + // 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); + } -// static const Matrix93 P = -// (Matrix93() << vec(G[0]), vec(G[1]), vec(G[2])).finished(); + if (H) *H = SO3::LogmapDerivative(omega); + return omega; +} -/* ************************************************************************* */ -// 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); -// }; +//****************************************************************************** +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); +} +//****************************************************************************** } // end namespace gtsam diff --git a/gtsam/geometry/SOt.h b/gtsam/geometry/SOt.h index 0fa8f5776..8d7632497 100644 --- a/gtsam/geometry/SOt.h +++ b/gtsam/geometry/SOt.h @@ -42,7 +42,10 @@ using SO3 = SO<3>; // static Matrix3 Hat(const Vector3 &xi); ///< make skew symmetric matrix // static Vector3 Vee(const Matrix3 &X); ///< inverse of Hat -//****************************************************************************** +/** + * Exponential map at identity - create a rotation from canonical coordinates + * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula + */ template <> SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H); @@ -51,12 +54,6 @@ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H); // 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); @@ -64,49 +61,28 @@ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H); * 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); +Vector3 SO3::Logmap(const SO3& R, ChartJacobian H); + +template <> +Matrix3 SO3::AdjointMap() const { + return matrix_; } +// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap +template <> +SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) { + return Expmap(omega, H); +} + +template <> +Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H) { + return Logmap(R, H); +} + +template <> +Vector9 SO3::vec(OptionalJacobian<9, 3> H) const; + // private: // /** Serialization function */ From e93149babb163d7dfa919223392ab24557778c17 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 May 2019 15:37:54 -0400 Subject: [PATCH 21/47] Moved all tests to their own test files --- gtsam/geometry/tests/testSO3.cpp | 51 +++++++++++++------ gtsam/geometry/tests/testSO4.cpp | 85 ++++++++++++++++++-------------- 2 files changed, 83 insertions(+), 53 deletions(-) diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 94c130f9f..83916bd1b 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -15,14 +15,23 @@ * @author Frank Dellaert **/ -#include -#include -#include #include +#include +#include +#include using namespace std; using namespace gtsam; +//****************************************************************************** +TEST(SO3, Identity) { + const SO3 R; + EXPECT_LONGS_EQUAL(3, R.rows()); + EXPECT_LONGS_EQUAL(3, SO3::dimension); + EXPECT_LONGS_EQUAL(3, SO3::Dim()); + EXPECT_LONGS_EQUAL(3, R.dim()); +} + //****************************************************************************** TEST(SO3, Concept) { BOOST_CONCEPT_ASSERT((IsGroup)); @@ -50,7 +59,21 @@ TEST(SO3, Local) { TEST(SO3, Retract) { Vector3 v(0, 0, 0.1); SO3 actual = traits::Retract(R1, v); - EXPECT(actual.isApprox(R2)); + EXPECT(assert_equal(R2, actual)); +} + +//****************************************************************************** +TEST(SO3, Logmap) { + Vector3 expected(0, 0, 0.1); + Vector3 actual = SO3::Logmap(R1.between(R2)); + EXPECT(assert_equal((Vector)expected, actual)); +} + +//****************************************************************************** +TEST(SO3, Expmap) { + Vector3 v(0, 0, 0.1); + SO3 actual = R1 * SO3::Expmap(v); + EXPECT(assert_equal(R2, actual)); } //****************************************************************************** @@ -231,17 +254,17 @@ TEST(SO3, JacobianLogmap) { EXPECT(assert_equal(Jexpected, Jactual)); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, ApplyDexp) { Matrix aH1, aH2; 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), @@ -254,17 +277,17 @@ TEST(SO3, ApplyDexp) { } } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, ApplyInvDexp) { Matrix aH1, aH2; 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)}) { @@ -278,15 +301,13 @@ TEST(SO3, ApplyInvDexp) { } } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, vec) { - const Vector9 expected = Eigen::Map(R2.data()); + const Vector9 expected = Eigen::Map(R2.matrix().data()); Matrix actualH; const Vector9 actual = R2.vec(actualH); CHECK(assert_equal(expected, actual)); - boost::function f = [](const SO3& Q) { - return Q.vec(); - }; + boost::function f = [](const SO3& Q) { return Q.vec(); }; const Matrix numericalH = numericalDerivative11(f, R2, 1e-5); CHECK(assert_equal(numericalH, actualH)); } diff --git a/gtsam/geometry/tests/testSO4.cpp b/gtsam/geometry/tests/testSO4.cpp index 037280136..29ae2d50f 100644 --- a/gtsam/geometry/tests/testSO4.cpp +++ b/gtsam/geometry/tests/testSO4.cpp @@ -19,10 +19,9 @@ #include #include #include -#include +#include #include - #include #include @@ -30,6 +29,16 @@ using namespace std; using namespace gtsam; +//****************************************************************************** + +TEST(SO4, Identity) { + const SO4 R; + EXPECT_LONGS_EQUAL(4, R.rows()); + EXPECT_LONGS_EQUAL(6, SO4::dimension); + EXPECT_LONGS_EQUAL(6, SO4::Dim()); + EXPECT_LONGS_EQUAL(6, R.dim()); +} + //****************************************************************************** TEST(SO4, Concept) { BOOST_CONCEPT_ASSERT((IsGroup)); @@ -39,9 +48,9 @@ TEST(SO4, Concept) { //****************************************************************************** SO4 id; -Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished(); +Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished(); SO4 Q1 = SO4::Expmap(v1); -Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.00, 0.00, 0.00).finished(); +Vector6 v2 = (Vector(6) << 0.00, 0.00, 0.00, 0.01, 0.02, 0.03).finished(); SO4 Q2 = SO4::Expmap(v2); Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished(); SO4 Q3 = SO4::Expmap(v3); @@ -55,12 +64,12 @@ TEST(SO4, Random) { //****************************************************************************** TEST(SO4, Expmap) { // If we do exponential map in SO(3) subgroup, topleft should be equal to R1. - auto R1 = SO3::Expmap(v1.head<3>()); - EXPECT((Q1.topLeft().isApprox(R1))); + auto R1 = SO3::Expmap(v1.tail<3>()).matrix(); + EXPECT((Q1.matrix().topLeftCorner<3, 3>().isApprox(R1))); // Same here - auto R2 = SO3::Expmap(v2.head<3>()); - EXPECT((Q2.topLeft().isApprox(R2))); + auto R2 = SO3::Expmap(v2.tail<3>()).matrix(); + EXPECT((Q2.matrix().topLeftCorner<3, 3>().isApprox(R2))); // Check commutative subgroups for (size_t i = 0; i < 6; i++) { @@ -69,7 +78,7 @@ TEST(SO4, Expmap) { SO4 Q1 = SO4::Expmap(xi); xi[i] = 3; SO4 Q2 = SO4::Expmap(xi); - EXPECT(((Q1 * Q2).isApprox(Q2 * Q1))); + EXPECT(assert_equal(Q1 * Q2, Q2 * Q1)); } } @@ -83,7 +92,7 @@ TEST(SO4, Cayley) { TEST(SO4, Retract) { auto v = Vector6::Zero(); SO4 actual = traits::Retract(id, v); - EXPECT(actual.isApprox(id)); + EXPECT(assert_equal(id, actual)); } //****************************************************************************** @@ -108,7 +117,7 @@ TEST(SO4, Invariants) { EXPECT(check_manifold_invariants(Q1, Q2)); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO4, compose) { SO4 expected = Q1 * Q2; Matrix actualH1, actualH2; @@ -124,10 +133,10 @@ TEST(SO4, compose) { CHECK(assert_equal(numericalH2, actualH2)); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO4, vec) { - using Vector16 = SO4::Vector16; - const Vector16 expected = Eigen::Map(Q2.data()); + using Vector16 = SO4::VectorN2; + const Vector16 expected = Eigen::Map(Q2.matrix().data()); Matrix actualH; const Vector16 actual = Q2.vec(actualH); CHECK(assert_equal(expected, actual)); @@ -138,31 +147,31 @@ TEST(SO4, vec) { CHECK(assert_equal(numericalH, actualH)); } -/* ************************************************************************* */ -TEST(SO4, topLeft) { - const Matrix3 expected = Q3.topLeftCorner<3, 3>(); - Matrix actualH; - const Matrix3 actual = Q3.topLeft(actualH); - CHECK(assert_equal(expected, actual)); - boost::function f = [](const SO4& Q3) { - return Q3.topLeft(); - }; - const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); - CHECK(assert_equal(numericalH, actualH)); -} +// /* ************************************************************************* +// */ TEST(SO4, topLeft) { +// const Matrix3 expected = Q3.topLeftCorner<3, 3>(); +// Matrix actualH; +// const Matrix3 actual = Q3.topLeft(actualH); +// CHECK(assert_equal(expected, actual)); +// boost::function f = [](const SO4& Q3) { +// return Q3.topLeft(); +// }; +// const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); +// CHECK(assert_equal(numericalH, actualH)); +// } -/* ************************************************************************* */ -TEST(SO4, stiefel) { - const Matrix43 expected = Q3.leftCols<3>(); - Matrix actualH; - const Matrix43 actual = Q3.stiefel(actualH); - CHECK(assert_equal(expected, actual)); - boost::function f = [](const SO4& Q3) { - return Q3.stiefel(); - }; - const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); - CHECK(assert_equal(numericalH, actualH)); -} +// /* ************************************************************************* +// */ TEST(SO4, stiefel) { +// const Matrix43 expected = Q3.leftCols<3>(); +// Matrix actualH; +// const Matrix43 actual = Q3.stiefel(actualH); +// CHECK(assert_equal(expected, actual)); +// boost::function f = [](const SO4& Q3) { +// return Q3.stiefel(); +// }; +// const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); +// CHECK(assert_equal(numericalH, actualH)); +// } //****************************************************************************** int main() { From f7ad80673c0a2263812aa2f47ae8509269f3b583 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 May 2019 16:37:34 -0400 Subject: [PATCH 22/47] Moved things to inl --- gtsam/geometry/SOn-inl.h | 26 ++++++++++++++++++++++++++ gtsam/geometry/SOn.h | 23 ++++++++++------------- 2 files changed, 36 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/SOn-inl.h b/gtsam/geometry/SOn-inl.h index 26dc2229b..826efe1ee 100644 --- a/gtsam/geometry/SOn-inl.h +++ b/gtsam/geometry/SOn-inl.h @@ -97,6 +97,32 @@ typename SO::TangentVector SO::ChartAtOrigin::Local(const SO& R, return -2 * Vee(X); } +template +typename SO::MatrixDD SO::AdjointMap() const { + throw std::runtime_error( + "SO::AdjointMap only implemented for SO3 and SO4."); +} + +template +SO SO::Expmap(const TangentVector& omega, ChartJacobian H) { + throw std::runtime_error("SO::Expmap only implemented for SO3 and SO4."); +} + +template +typename SO::MatrixDD SO::ExpmapDerivative(const TangentVector& omega) { + throw std::runtime_error("SO::ExpmapDerivative only implemented for SO3."); +} + +template +typename SO::TangentVector SO::Logmap(const SO& R, ChartJacobian H) { + throw std::runtime_error("SO::Logmap only implemented for SO3 and SO4."); +} + +template +typename SO::MatrixDD SO::LogmapDerivative(const TangentVector& omega) { + throw std::runtime_error("O::LogmapDerivative only implemented for SO3."); +} + template typename SO::VectorN2 SO::vec( OptionalJacobian H) const { diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 64718ed5f..45fe43519 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -220,27 +220,24 @@ class SO : public LieGroup, internal::DimensionSO(N)> { /// @name Lie Group /// @{ - MatrixDD AdjointMap() const { - throw std::runtime_error( - "SO::AdjointMap only implemented for SO3 and SO4."); - } + /// Adjoint map + MatrixDD AdjointMap() const; /** * 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."); - } + static SO Expmap(const TangentVector& omega, ChartJacobian H = boost::none); + + /// Derivative of Expmap, currently only defined for SO3 + static MatrixDD ExpmapDerivative(const TangentVector& omega); /** * 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."); - } + static TangentVector Logmap(const SO& R, ChartJacobian H = boost::none); - // template > - static Matrix3 LogmapDerivative(const Vector3& omega); + /// Derivative of Logmap, currently only defined for SO3 + static MatrixDD LogmapDerivative(const TangentVector& omega); // inverse with optional derivative using LieGroup, internal::DimensionSO(N)>::inverse; @@ -288,4 +285,4 @@ struct traits> : public internal::LieGroup> {}; } // namespace gtsam -#include "SOn-inl.h" \ No newline at end of file +#include "SOn-inl.h" From d376d0158d87cb5159963f21e536fbc4dbb9217f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 May 2019 16:38:05 -0400 Subject: [PATCH 23/47] All tests for SO3 now uncommented --- gtsam/geometry/SOn-inl.h | 2 +- gtsam/geometry/SOt.cpp | 12 ++++++++---- gtsam/geometry/SOt.h | 22 +++++++++++++--------- gtsam/geometry/tests/testSO3.cpp | 14 +++++++------- 4 files changed, 29 insertions(+), 21 deletions(-) diff --git a/gtsam/geometry/SOn-inl.h b/gtsam/geometry/SOn-inl.h index 826efe1ee..d1c9efe29 100644 --- a/gtsam/geometry/SOn-inl.h +++ b/gtsam/geometry/SOn-inl.h @@ -115,7 +115,7 @@ typename SO::MatrixDD SO::ExpmapDerivative(const TangentVector& omega) { template typename SO::TangentVector SO::Logmap(const SO& R, ChartJacobian H) { - throw std::runtime_error("SO::Logmap only implemented for SO3 and SO4."); + throw std::runtime_error("SO::Logmap only implemented for SO3."); } template diff --git a/gtsam/geometry/SOt.cpp b/gtsam/geometry/SOt.cpp index 9249715e8..bf259041d 100644 --- a/gtsam/geometry/SOt.cpp +++ b/gtsam/geometry/SOt.cpp @@ -182,9 +182,10 @@ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { } } -// Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { -// return sot::DexpFunctor(omega).dexp(); -// } +template <> +Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { + return sot::DexpFunctor(omega).dexp(); +} //****************************************************************************** /* Right Jacobian for Log map in SO(3) - equation (10.86) and following @@ -254,19 +255,22 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); } - if (H) *H = SO3::LogmapDerivative(omega); + if (H) *H = LogmapDerivative(omega); return omega; } //****************************************************************************** +// local vectorize static Vector9 vec3(const Matrix3& R) { return Eigen::Map(R.data()); } +// so<3> generators static const std::vector G3({SO3::Hat(Vector3::Unit(0)), SO3::Hat(Vector3::Unit(1)), SO3::Hat(Vector3::Unit(2))}); +// vectorized generators static const Matrix93 P3 = (Matrix93() << vec3(G3[0]), vec3(G3[1]), vec3(G3[2])).finished(); diff --git a/gtsam/geometry/SOt.h b/gtsam/geometry/SOt.h index 8d7632497..4b62d92e3 100644 --- a/gtsam/geometry/SOt.h +++ b/gtsam/geometry/SOt.h @@ -42,6 +42,15 @@ using SO3 = SO<3>; // static Matrix3 Hat(const Vector3 &xi); ///< make skew symmetric matrix // static Vector3 Vee(const Matrix3 &X); ///< inverse of Hat +// Below are all declarations of SO<3> specializations. +// They are *defined* in SO3.cpp. + +/// Adjoint map +template <> +Matrix3 SO3::AdjointMap() const { + return matrix_; +} + /** * Exponential map at identity - create a rotation from canonical coordinates * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula @@ -49,13 +58,9 @@ using SO3 = SO<3>; template <> SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H); -// template<> -// Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { -// return sot::DexpFunctor(omega).dexp(); -// } - /// Derivative of Expmap -// static Matrix3 ExpmapDerivative(const Vector3& omega); +template <> +Matrix3 SO3::ExpmapDerivative(const Vector3& omega); /** * Log map at identity - returns the canonical coordinates @@ -64,10 +69,9 @@ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H); template <> Vector3 SO3::Logmap(const SO3& R, ChartJacobian H); +/// Derivative of Logmap template <> -Matrix3 SO3::AdjointMap() const { - return matrix_; -} +Matrix3 SO3::LogmapDerivative(const Vector3& omega); // Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap template <> diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 83916bd1b..d6ca68ccf 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -157,7 +157,7 @@ TEST(SO3, ExpmapDerivative) { EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-7)); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, ExpmapDerivative2) { const Vector3 theta(0.1, 0, 0.1); const Matrix Jexpected = numericalDerivative11( @@ -168,7 +168,7 @@ TEST(SO3, ExpmapDerivative2) { SO3::ExpmapDerivative(-theta))); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, ExpmapDerivative3) { const Vector3 theta(10, 20, 30); const Matrix Jexpected = numericalDerivative11( @@ -179,7 +179,7 @@ TEST(SO3, ExpmapDerivative3) { SO3::ExpmapDerivative(-theta))); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, ExpmapDerivative4) { // Iserles05an (Lie-group Methods) says: // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) @@ -207,7 +207,7 @@ TEST(SO3, ExpmapDerivative4) { } } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, ExpmapDerivative5) { auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; @@ -223,7 +223,7 @@ TEST(SO3, ExpmapDerivative5) { } } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, ExpmapDerivative6) { const Vector3 thetahat(0.1, 0, 0.1); const Matrix Jexpected = numericalDerivative11( @@ -233,7 +233,7 @@ TEST(SO3, ExpmapDerivative6) { EXPECT(assert_equal(Jexpected, Jactual)); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, LogmapDerivative) { const Vector3 thetahat(0.1, 0, 0.1); const SO3 R = SO3::Expmap(thetahat); // some rotation @@ -243,7 +243,7 @@ TEST(SO3, LogmapDerivative) { EXPECT(assert_equal(Jexpected, Jactual)); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, JacobianLogmap) { const Vector3 thetahat(0.1, 0, 0.1); const SO3 R = SO3::Expmap(thetahat); // some rotation From ab1be9c4decb3138e9aeee46815c55a66e706b41 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 May 2019 18:45:16 -0400 Subject: [PATCH 24/47] Specialized Hat/Vee --- gtsam/geometry/SOn-inl.h | 58 +++++----------------------------------- gtsam/geometry/SOn.cpp | 58 ++++++++++++++++++++++++++++++++++++++++ gtsam/geometry/SOn.h | 24 ++++++++++++++--- 3 files changed, 84 insertions(+), 56 deletions(-) diff --git a/gtsam/geometry/SOn-inl.h b/gtsam/geometry/SOn-inl.h index d1c9efe29..1c05eac47 100644 --- a/gtsam/geometry/SOn-inl.h +++ b/gtsam/geometry/SOn-inl.h @@ -22,62 +22,16 @@ namespace gtsam { +// Implementation for N>5 just uses dynamic version template -Matrix SO::Hat(const Vector& xi) { - size_t n = AmbientDim(xi.size()); - if (n < 2) throw std::invalid_argument("SO::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; +typename SO::MatrixNN SO::Hat(const TangentVector& xi) { + return SOn::Hat(xi); } +// Implementation for N>5 just uses dynamic version template -Vector SO::Vee(const Matrix& X) { - const size_t n = X.rows(); - if (n < 2) throw std::invalid_argument("SO::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; - } +typename SO::TangentVector SO::Vee(const MatrixNN& X) { + return SOn::Vee(X); } template diff --git a/gtsam/geometry/SOn.cpp b/gtsam/geometry/SOn.cpp index 67b780db8..06d24472e 100644 --- a/gtsam/geometry/SOn.cpp +++ b/gtsam/geometry/SOn.cpp @@ -20,6 +20,64 @@ namespace gtsam { +template <> +Matrix SOn::Hat(const Vector& xi) { + size_t n = AmbientDim(xi.size()); + if (n < 2) throw std::invalid_argument("SO::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; +} + +template <> +Vector SOn::Vee(const Matrix& X) { + const size_t n = X.rows(); + if (n < 2) throw std::invalid_argument("SO::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; + } +} + template <> SOn LieGroup::compose(const SOn& g, DynamicJacobian H1, DynamicJacobian H2) const { diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 45fe43519..56cdeb3fe 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -83,6 +83,10 @@ class SO : public LieGroup, internal::DimensionSO(N)> { template explicit SO(const Eigen::MatrixBase& R) : matrix_(R.eval()) {} + /// Construct dynamic SO(n) from Fixed SO + template > + explicit SO(const SO& R) : matrix_(R.matrix()) {} + /// Constructor from AngleAxisd template > SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {} @@ -188,12 +192,12 @@ class SO : public LieGroup, internal::DimensionSO(N)> { * -d c -b a 0 * This scheme behaves exactly as expected for SO(2) and SO(3). */ - static Matrix Hat(const Vector& xi); + static MatrixNN Hat(const TangentVector& xi); /** * Inverse of Hat. See note about xi element order in Hat. */ - static Vector Vee(const Matrix& X); + static TangentVector Vee(const MatrixNN& X); // Chart at origin struct ChartAtOrigin { @@ -259,8 +263,20 @@ class SO : public LieGroup, internal::DimensionSO(N)> { using SOn = SO; /* - * Fully specialize compose and between, because the derivative is unknowable by - * the LieGroup implementations, who return a fixed-size matrix for H2. + * Specialize dynamic Hat and Vee, because recursion depends on dynamic nature. + * The definition is in SOn.cpp. Fixed-size SO3 and SO4 have their own version, + * and implementation for other fixed N is in SOn-inl.h. + */ + +template <> +Matrix SOn::Hat(const Vector& xi); + +template <> +Vector SOn::Vee(const Matrix& X); + +/* + * Specialize dynamic compose and between, because the derivative is unknowable + * by the LieGroup implementations, who return a fixed-size matrix for H2. */ using DynamicJacobian = OptionalJacobian; From a765886e5aca67815c0d0ed22aa82a94fea265d6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 May 2019 18:45:41 -0400 Subject: [PATCH 25/47] Hat and Vee for SO4 --- gtsam/geometry/SO4.cpp | 331 +++++++++++++++---------------- gtsam/geometry/SO4.h | 130 +++--------- gtsam/geometry/tests/testSO4.cpp | 92 +++++---- 3 files changed, 247 insertions(+), 306 deletions(-) diff --git a/gtsam/geometry/SO4.cpp b/gtsam/geometry/SO4.cpp index 1f265ecba..93c5f7f8e 100644 --- a/gtsam/geometry/SO4.cpp +++ b/gtsam/geometry/SO4.cpp @@ -24,20 +24,23 @@ #include #include +#include #include #include +#include using namespace std; namespace gtsam { -// /* ************************************************************************* */ -// static Vector3 randomOmega(boost::mt19937 &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; @@ -45,185 +48,179 @@ namespace gtsam { // return SO4::Expmap(delta); // } -// /* ************************************************************************* */ -// void SO4::print(const string &s) const { cout << s << *this << endl; } +//****************************************************************************** +template <> +Matrix4 SO4::Hat(const Vector6& xi) { + // skew symmetric matrix X = xi^ + // Unlike Luca, makes upper-left the SO(3) subgroup. + Matrix4 Y = Z_4x4; + Y(0, 1) = -xi(5); + Y(0, 2) = +xi(4); + Y(1, 2) = -xi(3); + Y(0, 3) = -xi(2); + Y(1, 3) = +xi(1); + Y(2, 3) = -xi(0); + return Y - Y.transpose(); +} -// /* ************************************************************************* */ -// bool SO4::equals(const SO4 &R, double tol) const { -// return equal_with_abs_tol(*this, R, tol); -// } +//****************************************************************************** +template <> +Vector6 SO4::Vee(const Matrix4& X) { + Vector6 xi; + xi(5) = -X(0, 1); + xi(4) = +X(0, 2); + xi(3) = -X(1, 2); + xi(2) = -X(0, 3); + xi(1) = +X(1, 3); + xi(0) = -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 */ +template <> +SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) { + using namespace std; + if (H) throw std::runtime_error("SO4::Expmap Jacobian"); -// //****************************************************************************** -// /* 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 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(); + } +} -// // 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(); -// } -// } +//****************************************************************************** +// local vectorize +static SO4::VectorN2 vec4(const Matrix4& Q) { + return Eigen::Map(Q.data()); +} -// //****************************************************************************** -// static SO4::VectorN2 vec4(const Matrix4& Q) { -// return Eigen::Map(Q.data()); -// } +// so<4> generators +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 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))}); +// vectorized generators +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 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 <> -// 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); +} -// //****************************************************************************** -// 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); -// } +///****************************************************************************** +template <> +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 SO4((I_4x4 + X) * (I_4x4 - X).inverse()); +} +//****************************************************************************** +template <> +Vector6 SO4::ChartAtOrigin::Local(const SO4& Q, ChartJacobian H) { + if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian"); + const Matrix4& R = Q.matrix(); + const Matrix4 X = (I_4x4 - R) * (I_4x4 + R).inverse(); + return -2 * Vee(X); +} -// //****************************************************************************** -// Vector6 SO4::Logmap(const SO4 &Q, ChartJacobian H) { -// if (H) throw std::runtime_error("SO4::Logmap Jacobian"); -// throw std::runtime_error("SO4::Logmap"); -// } +//****************************************************************************** +Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H) { + const Matrix4& R = Q.matrix(); + const Matrix3 M = R.topLeftCorner<3, 3>(); + if (H) { + const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2), + q = R.topRightCorner<3, 1>(); + *H << Z_3x1, Z_3x1, q, Z_3x1, -m3, m2, // + Z_3x1, -q, Z_3x1, m3, Z_3x1, -m1, // + q, Z_3x1, Z_3x1, -m2, m1, Z_3x1; + } + return M; +} -// /* ************************************************************************* */ -// 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(); -// } +//****************************************************************************** +Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H) { + const Matrix4& R = Q.matrix(); + const Matrix43 M = R.leftCols<3>(); + if (H) { + const auto &m1 = R.col(0), m2 = R.col(1), m3 = R.col(2), q = R.col(3); + *H << Z_4x1, Z_4x1, q, Z_4x1, -m3, m2, // + Z_4x1, -q, Z_4x1, m3, Z_4x1, -m1, // + q, Z_4x1, Z_4x1, -m2, m1, Z_4x1; + } + 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); -// } - -// /* ************************************************************************* */ -// 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 aed0d482d..a92cdfc5c 100644 --- a/gtsam/geometry/SO4.h +++ b/gtsam/geometry/SO4.h @@ -38,120 +38,40 @@ using SO4 = SO<4>; // /// Random SO(4) element (no big claims about uniformity) // static SO4 Random(boost::mt19937 &rng); -// 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; +// Below are all declarations of SO<4> specializations. +// They are *defined* in SO4.cpp. -//****************************************************************************** -/* 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"); +Matrix4 SO4::Hat(const TangentVector& 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()); - }); - - // 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; -} +Vector6 SO4::Vee(const Matrix4& X); -//****************************************************************************** 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); -} +SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H); -// /// Vectorize -// Vector16 vec(OptionalJacobian<16, 6> H = boost::none) const; +template <> +Matrix6 SO4::AdjointMap() 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; +template <> +SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) 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; +template <> +SO4 SO4::ChartAtOrigin::Retract(const Vector6& omega, ChartJacobian H); + +template <> +Vector6 SO4::ChartAtOrigin::Local(const SO4& R, ChartJacobian H); + +/** + * Project to top-left 3*3 matrix. Note this is *not* in general \in SO(3). + */ +Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H = boost::none); + +/** + * Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) + * -> S \in St(3,4). + */ +Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H = boost::none); // private: // /** Serialization function */ diff --git a/gtsam/geometry/tests/testSO4.cpp b/gtsam/geometry/tests/testSO4.cpp index 29ae2d50f..d343e9397 100644 --- a/gtsam/geometry/tests/testSO4.cpp +++ b/gtsam/geometry/tests/testSO4.cpp @@ -19,8 +19,8 @@ #include #include #include -#include #include +#include #include @@ -30,7 +30,6 @@ using namespace std; using namespace gtsam; //****************************************************************************** - TEST(SO4, Identity) { const SO4 R; EXPECT_LONGS_EQUAL(4, R.rows()); @@ -83,13 +82,32 @@ TEST(SO4, Expmap) { } //****************************************************************************** -TEST(SO4, Cayley) { - CHECK(assert_equal(id.retract(v1 / 100), SO4::Expmap(v1 / 100))); - CHECK(assert_equal(id.retract(v2 / 100), SO4::Expmap(v2 / 100))); +TEST(SO4, Hat) { + // Check that Hat specialization is equal to dynamic version + EXPECT(assert_equal(SO4::Hat(v1), SOn::Hat(v1))); + EXPECT(assert_equal(SO4::Hat(v2), SOn::Hat(v2))); + EXPECT(assert_equal(SO4::Hat(v3), SOn::Hat(v3))); +} + +//****************************************************************************** +TEST(SO4, Vee) { + // Check that Hat specialization is equal to dynamic version + auto X1 = SOn::Hat(v1), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3); + EXPECT(assert_equal(SO4::Vee(X1), SOn::Vee(X1))); + EXPECT(assert_equal(SO4::Vee(X2), SOn::Vee(X2))); + EXPECT(assert_equal(SO4::Vee(X3), SOn::Vee(X3))); } //****************************************************************************** TEST(SO4, Retract) { + // Check that Cayley yields the same as Expmap for small values + EXPECT(assert_equal(id.retract(v1 / 100), SO4::Expmap(v1 / 100))); + EXPECT(assert_equal(id.retract(v2 / 100), SO4::Expmap(v2 / 100))); + + // Check that Cayley is identical to dynamic version + EXPECT(assert_equal(SOn(id.retract(v1 / 100)), SOn(4).retract(v1 / 100))); + EXPECT(assert_equal(SOn(id.retract(v2 / 100)), SOn(4).retract(v2 / 100))); + auto v = Vector6::Zero(); SO4 actual = traits::Retract(id, v); EXPECT(assert_equal(id, actual)); @@ -97,6 +115,12 @@ TEST(SO4, Retract) { //****************************************************************************** TEST(SO4, Local) { + // Check that Cayley is identical to dynamic version + EXPECT( + assert_equal(id.localCoordinates(Q1), SOn(4).localCoordinates(SOn(Q1)))); + EXPECT( + assert_equal(id.localCoordinates(Q2), SOn(4).localCoordinates(SOn(Q2)))); + auto v0 = Vector6::Zero(); Vector6 actual = traits::Local(id, id); EXPECT(assert_equal((Vector)v0, actual)); @@ -122,15 +146,15 @@ TEST(SO4, compose) { SO4 expected = Q1 * Q2; Matrix actualH1, actualH2; SO4 actual = Q1.compose(Q2, actualH1, actualH2); - CHECK(assert_equal(expected, actual)); + EXPECT(assert_equal(expected, actual)); Matrix numericalH1 = numericalDerivative21(testing::compose, Q1, Q2, 1e-2); - CHECK(assert_equal(numericalH1, actualH1)); + EXPECT(assert_equal(numericalH1, actualH1)); Matrix numericalH2 = numericalDerivative22(testing::compose, Q1, Q2, 1e-2); - CHECK(assert_equal(numericalH2, actualH2)); + EXPECT(assert_equal(numericalH2, actualH2)); } //****************************************************************************** @@ -139,39 +163,39 @@ TEST(SO4, vec) { const Vector16 expected = Eigen::Map(Q2.matrix().data()); Matrix actualH; const Vector16 actual = Q2.vec(actualH); - CHECK(assert_equal(expected, actual)); + EXPECT(assert_equal(expected, actual)); boost::function f = [](const SO4& Q) { return Q.vec(); }; const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5); - CHECK(assert_equal(numericalH, actualH)); + EXPECT(assert_equal(numericalH, actualH)); } -// /* ************************************************************************* -// */ TEST(SO4, topLeft) { -// const Matrix3 expected = Q3.topLeftCorner<3, 3>(); -// Matrix actualH; -// const Matrix3 actual = Q3.topLeft(actualH); -// CHECK(assert_equal(expected, actual)); -// boost::function f = [](const SO4& Q3) { -// return Q3.topLeft(); -// }; -// const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); -// CHECK(assert_equal(numericalH, actualH)); -// } +//****************************************************************************** +TEST(SO4, topLeft) { + const Matrix3 expected = Q3.matrix().topLeftCorner<3, 3>(); + Matrix actualH; + const Matrix3 actual = topLeft(Q3, actualH); + EXPECT(assert_equal(expected, actual)); + boost::function f = [](const SO4& Q3) { + return topLeft(Q3); + }; + const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); + EXPECT(assert_equal(numericalH, actualH)); +} -// /* ************************************************************************* -// */ TEST(SO4, stiefel) { -// const Matrix43 expected = Q3.leftCols<3>(); -// Matrix actualH; -// const Matrix43 actual = Q3.stiefel(actualH); -// CHECK(assert_equal(expected, actual)); -// boost::function f = [](const SO4& Q3) { -// return Q3.stiefel(); -// }; -// const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); -// CHECK(assert_equal(numericalH, actualH)); -// } +//****************************************************************************** +TEST(SO4, stiefel) { + const Matrix43 expected = Q3.matrix().leftCols<3>(); + Matrix actualH; + const Matrix43 actual = stiefel(Q3, actualH); + EXPECT(assert_equal(expected, actual)); + boost::function f = [](const SO4& Q3) { + return stiefel(Q3); + }; + const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); + EXPECT(assert_equal(numericalH, actualH)); +} //****************************************************************************** int main() { From dea6b995e53980fbb0066fdb4fc2ac7a274dee1a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 May 2019 18:51:32 -0400 Subject: [PATCH 26/47] Hat and Vee for SO3 --- gtsam/geometry/SOt.cpp | 27 ++++++++++++++++++--------- gtsam/geometry/SOt.h | 9 ++++++--- gtsam/geometry/tests/testSO3.cpp | 19 ++++++++++++++++++- 3 files changed, 42 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/SOt.cpp b/gtsam/geometry/SOt.cpp index bf259041d..a5e8f457d 100644 --- a/gtsam/geometry/SOt.cpp +++ b/gtsam/geometry/SOt.cpp @@ -159,16 +159,25 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, // } //****************************************************************************** -// Matrix3 SO3::Hat(const Vector3& xi) { return skewSymmetric(xi); } +template <> +Matrix3 SO3::Hat(const Vector3& xi) { + // skew symmetric matrix X = xi^ + Matrix3 Y = Z_3x3; + Y(0, 1) = -xi(2); + Y(0, 2) = +xi(1); + Y(1, 2) = -xi(0); + return Y - Y.transpose(); +} -// /* ************************************************************************* -// */ 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 <> +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 <> diff --git a/gtsam/geometry/SOt.h b/gtsam/geometry/SOt.h index 4b62d92e3..52d000c93 100644 --- a/gtsam/geometry/SOt.h +++ b/gtsam/geometry/SOt.h @@ -39,12 +39,15 @@ using SO3 = SO<3>; // /// 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 - // Below are all declarations of SO<3> specializations. // They are *defined* in SO3.cpp. +template <> +Matrix3 SO3::Hat(const Vector3& xi); ///< make skew symmetric matrix + +template <> +Vector3 SO3::Vee(const Matrix3& X); ///< inverse of Hat + /// Adjoint map template <> Matrix3 SO3::AdjointMap() const { diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index d6ca68ccf..f7a936a74 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -44,10 +44,27 @@ TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } //****************************************************************************** SO3 id; -Vector3 z_axis(0, 0, 1); +Vector3 z_axis(0, 0, 1), v2(1, 2, 0), v3(1, 2, 3); SO3 R1(Eigen::AngleAxisd(0.1, z_axis)); SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); +//****************************************************************************** +TEST(SO3, Hat) { + // Check that Hat specialization is equal to dynamic version + EXPECT(assert_equal(SO3::Hat(z_axis), SOn::Hat(z_axis))); + EXPECT(assert_equal(SO3::Hat(v2), SOn::Hat(v2))); + EXPECT(assert_equal(SO3::Hat(v3), SOn::Hat(v3))); +} + +//****************************************************************************** +TEST(SO3, Vee) { + // Check that Hat specialization is equal to dynamic version + auto X1 = SOn::Hat(z_axis), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3); + EXPECT(assert_equal(SO3::Vee(X1), SOn::Vee(X1))); + EXPECT(assert_equal(SO3::Vee(X2), SOn::Vee(X2))); + EXPECT(assert_equal(SO3::Vee(X3), SOn::Vee(X3))); +} + //****************************************************************************** TEST(SO3, Local) { Vector3 expected(0, 0, 0.1); From 3193af9698c7c6007a732718489abb507a02aa62 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 May 2019 19:13:26 -0400 Subject: [PATCH 27/47] Three more constructors for SO3 --- gtsam/geometry/SO4.h | 4 +-- gtsam/geometry/SOn.h | 12 +++++++++ gtsam/geometry/SOt.cpp | 42 +++++++++++++++++--------------- gtsam/geometry/SOt.h | 15 +++++++----- gtsam/geometry/tests/testSO3.cpp | 31 ++++++++++++++++++++++- 5 files changed, 76 insertions(+), 28 deletions(-) diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h index a92cdfc5c..49b8b5183 100644 --- a/gtsam/geometry/SO4.h +++ b/gtsam/geometry/SO4.h @@ -104,9 +104,9 @@ Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H = boost::none); */ template <> -struct traits : Testable, internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; template <> -struct traits : Testable, internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; } // end namespace gtsam diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 56cdeb3fe..f5696d765 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -27,6 +27,7 @@ #include #include +#include namespace gtsam { @@ -91,6 +92,17 @@ class SO : public LieGroup, internal::DimensionSO(N)> { template > SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {} + /// Constructor from axis and angle. Only defined for SO3 + static SO AxisAngle(const Vector3& axis, double theta); + + /// Named constructor that finds SO(n) matrix closest to M in Frobenius norm, + /// currently only defined for SO3. + static SO ClosestTo(const MatrixNN& M); + + /// Named constructor that finds chordal mean = argmin_R \sum sqr(|R-R_i|_F), + /// currently only defined for SO3. + static SO ChordalMean(const std::vector& rotations); + /// Random SO(n) element (no big claims about uniformity) template > static SO Random(boost::mt19937& rng, size_t n = 0) { diff --git a/gtsam/geometry/SOt.cpp b/gtsam/geometry/SOt.cpp index a5e8f457d..9fc3aac42 100644 --- a/gtsam/geometry/SOt.cpp +++ b/gtsam/geometry/SOt.cpp @@ -134,29 +134,33 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, } // namespace sot //****************************************************************************** -// SO3 SO3::AxisAngle(const Vector3& axis, double theta) { -// return sot::ExpmapFunctor(axis, theta).expmap(); -// } +template <> +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(); -// } +template <> +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 SO3(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); -// } +template <> +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.matrix(); + } + return ClosestTo(C_e); +} //****************************************************************************** template <> diff --git a/gtsam/geometry/SOt.h b/gtsam/geometry/SOt.h index 52d000c93..f94d5cc98 100644 --- a/gtsam/geometry/SOt.h +++ b/gtsam/geometry/SOt.h @@ -33,15 +33,18 @@ 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); - // Below are all declarations of SO<3> specializations. // They are *defined* in SO3.cpp. +template <> +SO3 SO3::AxisAngle(const Vector3& axis, double theta); + +template <> +SO3 SO3::ClosestTo(const Matrix3& M); + +template <> +SO3 SO3::ChordalMean(const std::vector& rotations); + template <> Matrix3 SO3::Hat(const Vector3& xi); ///< make skew symmetric matrix diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index f7a936a74..3d68fda7f 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -40,7 +40,30 @@ TEST(SO3, Concept) { } //****************************************************************************** -TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } +TEST(SO3, Constructors) { + const Vector3 axis(0, 0, 1); + const double angle = 2.5; + SO3 Q(Eigen::AngleAxisd(angle, axis)); + SO3 R = SO3::AxisAngle(axis, angle); + EXPECT(assert_equal(Q, R)); +} + +/* ************************************************************************* */ +TEST(SO3, ClosestTo) { + // Example top-left of SO(4) matrix not quite on SO(3) manifold + Matrix3 M; + M << 0.79067393, 0.6051136, -0.0930814, // + 0.4155925, -0.64214347, -0.64324489, // + -0.44948549, 0.47046326, -0.75917576; + + Matrix expected(3, 3); + expected << 0.790687, 0.605096, -0.0931312, // + 0.415746, -0.642355, -0.643844, // + -0.449411, 0.47036, -0.759468; + + auto actual = SO3::ClosestTo(3 * M); + EXPECT(assert_equal(expected, actual.matrix(), 1e-6)); +} //****************************************************************************** SO3 id; @@ -48,6 +71,12 @@ Vector3 z_axis(0, 0, 1), v2(1, 2, 0), v3(1, 2, 3); SO3 R1(Eigen::AngleAxisd(0.1, z_axis)); SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); +/* ************************************************************************* */ +TEST(SO3, ChordalMean) { + std::vector rotations = {R1, R1.inverse()}; + EXPECT(assert_equal(SO3(), SO3::ChordalMean(rotations))); +} + //****************************************************************************** TEST(SO3, Hat) { // Check that Hat specialization is equal to dynamic version From 8eacdcbe586abfcc4244d21b80b6171ef02b1f42 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 May 2019 19:27:06 -0400 Subject: [PATCH 28/47] Serialization --- gtsam/geometry/SO4.h | 57 ++++++++++++++++++++++---------------------- gtsam/geometry/SOn.h | 4 ++++ gtsam/geometry/SOt.h | 31 +++++++++++------------- 3 files changed, 46 insertions(+), 46 deletions(-) diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h index 49b8b5183..0cc3bb9fe 100644 --- a/gtsam/geometry/SO4.h +++ b/gtsam/geometry/SO4.h @@ -42,13 +42,13 @@ using SO4 = SO<4>; // They are *defined* in SO4.cpp. template <> -Matrix4 SO4::Hat(const TangentVector& xi); +Matrix4 SO4::Hat(const TangentVector &xi); template <> -Vector6 SO4::Vee(const Matrix4& X); +Vector6 SO4::Vee(const Matrix4 &X); template <> -SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H); +SO4 SO4::Expmap(const Vector6 &xi, ChartJacobian H); template <> Matrix6 SO4::AdjointMap() const; @@ -57,47 +57,46 @@ template <> SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const; template <> -SO4 SO4::ChartAtOrigin::Retract(const Vector6& omega, ChartJacobian H); +SO4 SO4::ChartAtOrigin::Retract(const Vector6 &omega, ChartJacobian H); template <> -Vector6 SO4::ChartAtOrigin::Local(const SO4& R, ChartJacobian H); +Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H); /** * Project to top-left 3*3 matrix. Note this is *not* in general \in SO(3). */ -Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H = boost::none); +Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = boost::none); /** * Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) * -> S \in St(3,4). */ -Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H = boost::none); +Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = boost::none); -// 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)); +/** Serialization function */ +template +void serialize(Archive &ar, SO4 &Q, const unsigned int /*version*/) { + Matrix4 &M = Q.matrix_; + ar &boost::serialization::make_nvp("Q11", M(0, 0)); + ar &boost::serialization::make_nvp("Q12", M(0, 1)); + ar &boost::serialization::make_nvp("Q13", M(0, 2)); + ar &boost::serialization::make_nvp("Q14", M(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("Q21", M(1, 0)); + ar &boost::serialization::make_nvp("Q22", M(1, 1)); + ar &boost::serialization::make_nvp("Q23", M(1, 2)); + ar &boost::serialization::make_nvp("Q24", M(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("Q31", M(2, 0)); + ar &boost::serialization::make_nvp("Q32", M(2, 1)); + ar &boost::serialization::make_nvp("Q33", M(2, 2)); + ar &boost::serialization::make_nvp("Q34", M(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)); -// } + ar &boost::serialization::make_nvp("Q41", M(3, 0)); + ar &boost::serialization::make_nvp("Q42", M(3, 1)); + ar &boost::serialization::make_nvp("Q43", M(3, 2)); + ar &boost::serialization::make_nvp("Q44", M(3, 3)); +} /* * Define the traits. internal::LieGroup provides both Lie group and Testable diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index f5696d765..c42107afa 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -270,6 +270,10 @@ class SO : public LieGroup, internal::DimensionSO(N)> { VectorN2 vec(OptionalJacobian H = boost::none) const; /// @} + + template + friend void serialize(Archive& ar, SO& R, const unsigned int /*version*/); + friend class boost::serialization::access; }; using SOn = SO; diff --git a/gtsam/geometry/SOt.h b/gtsam/geometry/SOt.h index f94d5cc98..9eedbcc67 100644 --- a/gtsam/geometry/SOt.h +++ b/gtsam/geometry/SOt.h @@ -93,23 +93,20 @@ Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H) { template <> Vector9 SO3::vec(OptionalJacobian<9, 3> H) const; -// 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)); -// } +/** Serialization function */ +template +void serialize(Archive& ar, SO3& R, const unsigned int /*version*/) { + Matrix3& M = R.matrix_; + ar& boost::serialization::make_nvp("R11", M(0, 0)); + ar& boost::serialization::make_nvp("R12", M(0, 1)); + ar& boost::serialization::make_nvp("R13", M(0, 2)); + ar& boost::serialization::make_nvp("R21", M(1, 0)); + ar& boost::serialization::make_nvp("R22", M(1, 1)); + ar& boost::serialization::make_nvp("R23", M(1, 2)); + ar& boost::serialization::make_nvp("R31", M(2, 0)); + ar& boost::serialization::make_nvp("R32", M(2, 1)); + ar& boost::serialization::make_nvp("R33", M(2, 2)); +} namespace sot { From 06952cd83ba05cdfc729e3007504decd45a647d9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 7 May 2019 10:48:55 -0400 Subject: [PATCH 29/47] Full enchilada: SO3 is now SO<3> --- gtsam/geometry/Rot3.h | 72 ++--- gtsam/geometry/Rot3M.cpp | 37 ++- gtsam/geometry/SO3.cpp | 195 +++++++------ gtsam/geometry/SO3.h | 206 ++++++-------- gtsam/geometry/SOn.h | 3 +- gtsam/geometry/SOt.cpp | 303 --------------------- gtsam/geometry/SOt.h | 187 ------------- gtsam/geometry/tests/testSO3.cpp | 12 +- gtsam/geometry/tests/testSO4.cpp | 2 +- gtsam/geometry/tests/testSOn.cpp | 2 +- gtsam/navigation/TangentPreintegration.cpp | 24 +- gtsam/slam/tests/testKarcherMeanFactor.cpp | 2 +- 12 files changed, 276 insertions(+), 769 deletions(-) delete mode 100644 gtsam/geometry/SOt.cpp delete mode 100644 gtsam/geometry/SOt.h diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index d4f1301e9..f3a6f08cd 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -61,7 +61,7 @@ namespace gtsam { /** Internal Eigen Quaternion */ gtsam::Quaternion quaternion_; #else - Matrix3 rot_; + SO3 rot_; #endif public: @@ -92,26 +92,33 @@ namespace gtsam { * allow assignment/construction from a generic matrix. * See: http://stackoverflow.com/questions/27094132/cannot-understand-if-this-is-circular-dependency-or-clang#tab-top */ - template - inline explicit Rot3(const Eigen::MatrixBase& R) { - #ifdef GTSAM_USE_QUATERNIONS - quaternion_=Matrix3(R); - #else - rot_ = R; - #endif + template +#ifdef GTSAM_USE_QUATERNIONS + explicit Rot3(const Eigen::MatrixBase& R) : quaternion_(R) { } +#else + explicit Rot3(const Eigen::MatrixBase& R) : rot_(R) { + } +#endif /** * Constructor from a rotation matrix * Overload version for Matrix3 to avoid casting in quaternion mode. */ - inline explicit Rot3(const Matrix3& R) { - #ifdef GTSAM_USE_QUATERNIONS - quaternion_=R; - #else - rot_ = R; - #endif - } +#ifdef GTSAM_USE_QUATERNIONS + explicit Rot3(const Matrix3& R) : quaternion_(R) {} +#else + explicit Rot3(const Matrix3& R) : rot_(R) {} +#endif + + /** + * Constructor from an SO3 instance + */ +#ifdef GTSAM_USE_QUATERNIONS + explicit Rot3(const SO3& R) : quaternion_(R.matrix()) {} +#else + explicit Rot3(const SO3& R) : rot_(R) {} +#endif /** Constructor from a quaternion. This can also be called using a plain * Vector, due to implicit conversion from Vector to Quaternion @@ -486,28 +493,27 @@ namespace gtsam { /// @} #endif - private: - + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) - { + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { #ifndef GTSAM_USE_QUATERNIONS - ar & boost::serialization::make_nvp("rot11", rot_(0,0)); - ar & boost::serialization::make_nvp("rot12", rot_(0,1)); - ar & boost::serialization::make_nvp("rot13", rot_(0,2)); - ar & boost::serialization::make_nvp("rot21", rot_(1,0)); - ar & boost::serialization::make_nvp("rot22", rot_(1,1)); - ar & boost::serialization::make_nvp("rot23", rot_(1,2)); - ar & boost::serialization::make_nvp("rot31", rot_(2,0)); - ar & boost::serialization::make_nvp("rot32", rot_(2,1)); - ar & boost::serialization::make_nvp("rot33", rot_(2,2)); + Matrix3& M = rot_.matrix_; + ar& boost::serialization::make_nvp("rot11", M(0, 0)); + ar& boost::serialization::make_nvp("rot12", M(0, 1)); + ar& boost::serialization::make_nvp("rot13", M(0, 2)); + ar& boost::serialization::make_nvp("rot21", M(1, 0)); + ar& boost::serialization::make_nvp("rot22", M(1, 1)); + ar& boost::serialization::make_nvp("rot23", M(1, 2)); + ar& boost::serialization::make_nvp("rot31", M(2, 0)); + ar& boost::serialization::make_nvp("rot32", M(2, 1)); + ar& boost::serialization::make_nvp("rot33", M(2, 2)); #else - ar & boost::serialization::make_nvp("w", quaternion_.w()); - ar & boost::serialization::make_nvp("x", quaternion_.x()); - ar & boost::serialization::make_nvp("y", quaternion_.y()); - ar & boost::serialization::make_nvp("z", quaternion_.z()); + ar& boost::serialization::make_nvp("w", quaternion_.w()); + ar& boost::serialization::make_nvp("x", quaternion_.x()); + ar& boost::serialization::make_nvp("y", quaternion_.y()); + ar& boost::serialization::make_nvp("z", quaternion_.z()); #endif } diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 529c64973..d38d33bf1 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -36,18 +36,17 @@ Rot3::Rot3() : rot_(I_3x3) {} /* ************************************************************************* */ Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { - rot_.col(0) = col1; - rot_.col(1) = col2; - rot_.col(2) = col3; + Matrix3 R; + R << col1, col2, col3; + rot_ = SO3(R); } /* ************************************************************************* */ -Rot3::Rot3(double R11, double R12, double R13, - double R21, double R22, double R23, - double R31, double R32, double R33) { - rot_ << R11, R12, R13, - R21, R22, R23, - R31, R32, R33; +Rot3::Rot3(double R11, double R12, double R13, double R21, double R22, + double R23, double R31, double R32, double R33) { + Matrix3 R; + R << R11, R12, R13, R21, R22, R23, R31, R32, R33; + rot_ = SO3(R); } /* ************************************************************************* */ @@ -107,21 +106,21 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) { /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { - return Rot3(Matrix3(rot_*R2.rot_)); + return Rot3(rot_*R2.rot_); } /* ************************************************************************* */ // TODO const Eigen::Transpose Rot3::transpose() const { Matrix3 Rot3::transpose() const { - return rot_.transpose(); + return rot_.matrix().transpose(); } /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { - if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); - if (H2) *H2 = rot_; - return rot_ * p; + if (H1) *H1 = rot_.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z()); + if (H2) *H2 = rot_.matrix(); + return rot_.matrix() * p; } /* ************************************************************************* */ @@ -178,21 +177,21 @@ Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) { /* ************************************************************************* */ Matrix3 Rot3::matrix() const { - return rot_; + return rot_.matrix(); } /* ************************************************************************* */ -Point3 Rot3::r1() const { return Point3(rot_.col(0)); } +Point3 Rot3::r1() const { return Point3(rot_.matrix().col(0)); } /* ************************************************************************* */ -Point3 Rot3::r2() const { return Point3(rot_.col(1)); } +Point3 Rot3::r2() const { return Point3(rot_.matrix().col(1)); } /* ************************************************************************* */ -Point3 Rot3::r3() const { return Point3(rot_.col(2)); } +Point3 Rot3::r3() const { return Point3(rot_.matrix().col(2)); } /* ************************************************************************* */ gtsam::Quaternion Rot3::toQuaternion() const { - return gtsam::Quaternion(rot_); + return gtsam::Quaternion(rot_.matrix()); } /* ************************************************************************* */ diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index a509bcf74..31d63d312 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -18,22 +18,23 @@ * @date December 2014 */ -#include #include +#include #include #include -#include #include +#include namespace gtsam { -/* ************************************************************************* */ +//****************************************************************************** namespace so3 { -Matrix99 Dcompose(const SO3& R) { +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); @@ -47,7 +48,8 @@ Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) { } void ExpmapFunctor::init(bool nearZeroApprox) { - nearZero = nearZeroApprox || (theta2 <= std::numeric_limits::epsilon()); + nearZero = + nearZeroApprox || (theta2 <= std::numeric_limits::epsilon()); if (!nearZero) { sin_theta = std::sin(theta); const double s2 = std::sin(theta / 2.0); @@ -79,9 +81,9 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApp SO3 ExpmapFunctor::expmap() const { if (nearZero) - return I_3x3 + W; + return SO3(I_3x3 + W); else - return I_3x3 + sin_theta * K + one_minus_cos * KK; + return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK); } DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) @@ -121,7 +123,7 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, if (H1) { Matrix3 D_dexpv_omega; applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping - *H1 = -invDexp* D_dexpv_omega; + *H1 = -invDexp * D_dexpv_omega; } if (H2) *H2 = invDexp; return c; @@ -129,72 +131,117 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, } // namespace so3 -/* ************************************************************************* */ +//****************************************************************************** +template <> SO3 SO3::AxisAngle(const Vector3& axis, double theta) { return so3::ExpmapFunctor(axis, theta).expmap(); } -/* ************************************************************************* */ +//****************************************************************************** +template <> 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(); + return SO3(U * Vector3(1, 1, det).asDiagonal() * V.transpose()); } -/* ************************************************************************* */ +//****************************************************************************** +template <> SO3 SO3::ChordalMean(const std::vector& rotations) { - // See Hartley13ijcv: - // Cost function C(R) = \sum sqr(|R-R_i|_F) + // 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}; + Matrix3 C_e{Z_3x3}; for (const auto& R_i : rotations) { - C_e += R_i; + C_e += R_i.matrix(); } return ClosestTo(C_e); } -/* ************************************************************************* */ -void SO3::print(const std::string& s) const { - std::cout << s << *this << std::endl; - } - //****************************************************************************** - 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 <> +Matrix3 SO3::Hat(const Vector3& xi) { + // skew symmetric matrix X = xi^ + Matrix3 Y = Z_3x3; + Y(0, 1) = -xi(2); + Y(0, 2) = +xi(1); + Y(1, 2) = -xi(0); + return Y - Y.transpose(); } -/* ************************************************************************* */ +//****************************************************************************** +template <> +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 <> +Matrix3 SO3::AdjointMap() const { + return matrix_; +} + +//****************************************************************************** +template <> SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { if (H) { so3::DexpFunctor impl(omega); *H = impl.dexp(); return impl.expmap(); - } else + } else { return so3::ExpmapFunctor(omega).expmap(); + } } +template <> Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { return so3::DexpFunctor(omega).dexp(); } -/* ************************************************************************* */ -Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { - using std::sqrt; +//****************************************************************************** +/* 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) + */ +template <> +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 + + // element of Lie algebra so(3): W = omega^ + const Matrix3 W = Hat(omega); + return I_3x3 + 0.5 * W + + (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * + W * W; +} + +//****************************************************************************** +template <> +Vector3 SO3::Logmap(const SO3& Q, 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); + const Matrix3& R = Q.matrix(); + 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(); @@ -213,7 +260,7 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { 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 + 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)); @@ -225,53 +272,49 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); } - if(H) *H = LogmapDerivative(omega); + if (H) *H = LogmapDerivative(omega); return omega; } -/* ************************************************************************* */ -Matrix3 SO3::LogmapDerivative(const Vector3& omega) { - using std::cos; - using std::sin; +//****************************************************************************** +// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap - 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; +template <> +SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) { + return Expmap(omega, H); } -/* ************************************************************************* */ -static Vector9 vec(const SO3& R) { return Eigen::Map(R.data()); } +template <> +Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H) { + return Logmap(R, H); +} -static const std::vector G({SO3::Hat(Vector3::Unit(0)), - SO3::Hat(Vector3::Unit(1)), - SO3::Hat(Vector3::Unit(2))}); +//****************************************************************************** +// local vectorize +static Vector9 vec3(const Matrix3& R) { + return Eigen::Map(R.data()); +} -static const Matrix93 P = - (Matrix93() << vec(G[0]), vec(G[1]), vec(G[2])).finished(); +// so<3> generators +static const std::vector G3({SO3::Hat(Vector3::Unit(0)), + SO3::Hat(Vector3::Unit(1)), + SO3::Hat(Vector3::Unit(2))}); -/* ************************************************************************* */ +// vectorized generators +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 SO3& R = *this; + const Matrix3& R = matrix_; 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); + // 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::vec(R); -}; - -/* ************************************************************************* */ - -} // end namespace gtsam + return gtsam::vec3(R); +} +//****************************************************************************** +} // end namespace gtsam diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index f89e2f59a..2b84dafba 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -20,146 +20,92 @@ #pragma once +#include + #include #include #include #include +#include namespace gtsam { +using SO3 = SO<3>; + +// Below are all declarations of SO<3> specializations. +// They are *defined* in SO3.cpp. + +template <> +SO3 SO3::AxisAngle(const Vector3& axis, double theta); + +template <> +SO3 SO3::ClosestTo(const Matrix3& M); + +template <> +SO3 SO3::ChordalMean(const std::vector& rotations); + +template <> +Matrix3 SO3::Hat(const Vector3& xi); ///< make skew symmetric matrix + +template <> +Vector3 SO3::Vee(const Matrix3& X); ///< inverse of Hat + +/// Adjoint map +template <> +Matrix3 SO3::AdjointMap() const; + /** - * True SO(3), i.e., 3*3 matrix subgroup - * We guarantee (all but first) constructors only generate from sub-manifold. - * However, round-off errors in repeated composition could move off it... + * Exponential map at identity - create a rotation from canonical coordinates + * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula */ -class SO3 : public Matrix3, public LieGroup { - public: - enum { N = 3 }; - enum { dimension = 3 }; +template <> +SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H); - /// @name Constructors - /// @{ +/// Derivative of Expmap +template <> +Matrix3 SO3::ExpmapDerivative(const Vector3& omega); - /// Default constructor creates identity - SO3() : Matrix3(I_3x3) {} +/** + * Log map at identity - returns the canonical coordinates + * \f$ [R_x,R_y,R_z] \f$ of this rotation + */ +template <> +Vector3 SO3::Logmap(const SO3& R, ChartJacobian H); - /// Constructor from Eigen Matrix - template - SO3(const MatrixBase& R) : Matrix3(R.eval()) {} +/// Derivative of Logmap +template <> +Matrix3 SO3::LogmapDerivative(const Vector3& omega); - /// Constructor from AngleAxisd - SO3(const Eigen::AngleAxisd& angleAxis) : Matrix3(angleAxis) {} +// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap +template <> +SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H); - /// Static, named constructor. TODO(dellaert): think about relation with above - GTSAM_EXPORT static SO3 AxisAngle(const Vector3& axis, double theta); +template <> +Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H); - /// Static, named constructor that finds SO(3) matrix closest to M in Frobenius norm. - static SO3 ClosestTo(const Matrix3& M); +template <> +Vector9 SO3::vec(OptionalJacobian<9, 3> H) const; - /// Static, named constructor that finds chordal mean = argmin_R \sum sqr(|R-R_i|_F). - static SO3 ChordalMean(const std::vector& rotations); - - /// @} - /// @name Testable - /// @{ - - GTSAM_EXPORT void print(const std::string& s) const; - - bool equals(const SO3 & R, double tol) const { - return equal_with_abs_tol(*this, R, tol); - } - - /// @} - /// @name Group - /// @{ - - /// identity rotation for group operation - static SO3 identity() { - return I_3x3; - } - - /// inverse of a rotation = transpose - SO3 inverse() const { - return this->transpose(); - } - - /// @} - /// @name Lie Group - /// @{ - - static Matrix3 Hat(const Vector3 &xi); ///< make skew symmetric matrix - static Vector3 Vee(const Matrix3 &X); ///< inverse of Hat - - /** - * Exponential map at identity - create a rotation from canonical coordinates - * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula - */ - GTSAM_EXPORT static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none); - - /// Derivative of Expmap - GTSAM_EXPORT 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 - */ - GTSAM_EXPORT static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none); - - /// Derivative of Logmap - GTSAM_EXPORT 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); - } - }; - - using LieGroup::inverse; - - /// @} - /// @name Other methods - /// @{ - - /// Vectorize - Vector9 vec(OptionalJacobian<9, 3> H = boost::none) const; - - /// Return matrix (for wrapper) - const Matrix3& 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("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)); - } -}; +/** Serialization function */ +template +void serialize(Archive& ar, SO3& R, const unsigned int /*version*/) { + Matrix3& M = R.matrix_; + ar& boost::serialization::make_nvp("R11", M(0, 0)); + ar& boost::serialization::make_nvp("R12", M(0, 1)); + ar& boost::serialization::make_nvp("R13", M(0, 2)); + ar& boost::serialization::make_nvp("R21", M(1, 0)); + ar& boost::serialization::make_nvp("R22", M(1, 1)); + ar& boost::serialization::make_nvp("R23", M(1, 2)); + ar& boost::serialization::make_nvp("R31", M(2, 0)); + ar& boost::serialization::make_nvp("R32", M(2, 1)); + ar& boost::serialization::make_nvp("R33", M(2, 2)); +} namespace so3 { -/** - * Compose general matrix with an SO(3) element. +/** + * 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, @@ -184,7 +130,7 @@ class GTSAM_EXPORT ExpmapFunctor { public: /// Constructor with element of Lie algebra so(3) - ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false); + explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false); /// Constructor with axis-angle ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false); @@ -201,7 +147,7 @@ class DexpFunctor : public ExpmapFunctor { public: /// Constructor with element of Lie algebra so(3) - GTSAM_EXPORT DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); + GTSAM_EXPORT 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, @@ -222,12 +168,14 @@ class DexpFunctor : public ExpmapFunctor { }; } // namespace so3 -template<> -struct traits : public internal::LieGroup { -}; +/* + * Define the traits. internal::LieGroup provides both Lie group and Testable + */ -template<> -struct traits : public internal::LieGroup { -}; -} // end namespace gtsam +template <> +struct traits : public internal::LieGroup {}; +template <> +struct traits : public internal::LieGroup {}; + +} // end namespace gtsam diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index c42107afa..511a03a6f 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -272,8 +272,9 @@ class SO : public LieGroup, internal::DimensionSO(N)> { /// @} template - friend void serialize(Archive& ar, SO& R, const unsigned int /*version*/); + friend void serialize(Archive&, SO&, const unsigned int); friend class boost::serialization::access; + friend class Rot3; // for serialize }; using SOn = SO; diff --git a/gtsam/geometry/SOt.cpp b/gtsam/geometry/SOt.cpp deleted file mode 100644 index 9fc3aac42..000000000 --- a/gtsam/geometry/SOt.cpp +++ /dev/null @@ -1,303 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - -//****************************************************************************** -template <> -SO3 SO3::AxisAngle(const Vector3& axis, double theta) { - return sot::ExpmapFunctor(axis, theta).expmap(); -} - -//****************************************************************************** -template <> -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 SO3(U * Vector3(1, 1, det).asDiagonal() * V.transpose()); -} - -//****************************************************************************** -template <> -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.matrix(); - } - return ClosestTo(C_e); -} - -//****************************************************************************** -template <> -Matrix3 SO3::Hat(const Vector3& xi) { - // skew symmetric matrix X = xi^ - Matrix3 Y = Z_3x3; - Y(0, 1) = -xi(2); - Y(0, 2) = +xi(1); - Y(1, 2) = -xi(0); - return Y - Y.transpose(); -} - -//****************************************************************************** -template <> -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(); - } -} - -template <> -Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { - return sot::DexpFunctor(omega).dexp(); -} - -//****************************************************************************** -/* 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) - */ -template <> -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 - - // element of Lie algebra so(3): W = omega^ - const Matrix3 W = Hat(omega); - return I_3x3 + 0.5 * W + - (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * - W * W; -} - -//****************************************************************************** -template <> -Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { - using std::sin; - using std::sqrt; - - // note switch to base 1 - const Matrix3& R = Q.matrix(); - 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; -} - -//****************************************************************************** -// local vectorize -static Vector9 vec3(const Matrix3& R) { - return Eigen::Map(R.data()); -} - -// so<3> generators -static const std::vector G3({SO3::Hat(Vector3::Unit(0)), - SO3::Hat(Vector3::Unit(1)), - SO3::Hat(Vector3::Unit(2))}); - -// vectorized generators -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); -} -//****************************************************************************** - -} // end namespace gtsam diff --git a/gtsam/geometry/SOt.h b/gtsam/geometry/SOt.h deleted file mode 100644 index 9eedbcc67..000000000 --- a/gtsam/geometry/SOt.h +++ /dev/null @@ -1,187 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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>; - -// Below are all declarations of SO<3> specializations. -// They are *defined* in SO3.cpp. - -template <> -SO3 SO3::AxisAngle(const Vector3& axis, double theta); - -template <> -SO3 SO3::ClosestTo(const Matrix3& M); - -template <> -SO3 SO3::ChordalMean(const std::vector& rotations); - -template <> -Matrix3 SO3::Hat(const Vector3& xi); ///< make skew symmetric matrix - -template <> -Vector3 SO3::Vee(const Matrix3& X); ///< inverse of Hat - -/// Adjoint map -template <> -Matrix3 SO3::AdjointMap() const { - return matrix_; -} - -/** - * Exponential map at identity - create a rotation from canonical coordinates - * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula - */ -template <> -SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H); - -/// Derivative of Expmap -template <> -Matrix3 SO3::ExpmapDerivative(const Vector3& omega); - -/** - * Log map at identity - returns the canonical coordinates - * \f$ [R_x,R_y,R_z] \f$ of this rotation - */ -template <> -Vector3 SO3::Logmap(const SO3& R, ChartJacobian H); - -/// Derivative of Logmap -template <> -Matrix3 SO3::LogmapDerivative(const Vector3& omega); - -// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap -template <> -SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) { - return Expmap(omega, H); -} - -template <> -Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H) { - return Logmap(R, H); -} - -template <> -Vector9 SO3::vec(OptionalJacobian<9, 3> H) const; - -/** Serialization function */ -template -void serialize(Archive& ar, SO3& R, const unsigned int /*version*/) { - Matrix3& M = R.matrix_; - ar& boost::serialization::make_nvp("R11", M(0, 0)); - ar& boost::serialization::make_nvp("R12", M(0, 1)); - ar& boost::serialization::make_nvp("R13", M(0, 2)); - ar& boost::serialization::make_nvp("R21", M(1, 0)); - ar& boost::serialization::make_nvp("R22", M(1, 1)); - ar& boost::serialization::make_nvp("R23", M(1, 2)); - ar& boost::serialization::make_nvp("R31", M(2, 0)); - ar& boost::serialization::make_nvp("R32", M(2, 1)); - ar& boost::serialization::make_nvp("R33", M(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/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 3d68fda7f..e3dd5cff1 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include using namespace std; using namespace gtsam; @@ -152,7 +152,7 @@ TEST(SO3, ChartDerivatives) { } /* ************************************************************************* */ -TEST(SO3, Expmap) { +TEST(SO3, ExpmapFunctor) { Vector axis = Vector3(0., 1., 0.); // rotation around Y double angle = 3.14 / 4.0; Matrix expected(3,3); @@ -306,11 +306,11 @@ TEST(SO3, ApplyDexp) { for (bool nearZeroApprox : {true, false}) { boost::function f = [=](const Vector3& omega, const Vector3& v) { - return sot::DexpFunctor(omega, nearZeroApprox).applyDexp(v); + return so3::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)}) { - sot::DexpFunctor local(omega, nearZeroApprox); + so3::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), @@ -329,11 +329,11 @@ TEST(SO3, ApplyInvDexp) { for (bool nearZeroApprox : {true, false}) { boost::function f = [=](const Vector3& omega, const Vector3& v) { - return sot::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v); + return so3::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)}) { - sot::DexpFunctor local(omega, nearZeroApprox); + so3::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)}) { diff --git a/gtsam/geometry/tests/testSO4.cpp b/gtsam/geometry/tests/testSO4.cpp index d343e9397..2c6342c38 100644 --- a/gtsam/geometry/tests/testSO4.cpp +++ b/gtsam/geometry/tests/testSO4.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index 1e4aee927..603caa4b4 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -16,7 +16,7 @@ **/ #include -#include +#include #include #include diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp index 4229d4f0c..55efdb151 100644 --- a/gtsam/navigation/TangentPreintegration.cpp +++ b/gtsam/navigation/TangentPreintegration.cpp @@ -68,30 +68,30 @@ Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body, Matrix3 w_tangent_H_theta, invH; const Vector3 w_tangent = // angular velocity mapped back to tangent space local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0); - const SO3 R = local.expmap(); + const Rot3 R(local.expmap()); const Vector3 a_nav = R * a_body; const double dt22 = 0.5 * dt * dt; Vector9 preintegratedPlus; - preintegratedPlus << // new preintegrated vector: - theta + w_tangent * dt, // theta - position + velocity * dt + a_nav * dt22, // position - velocity + a_nav * dt; // velocity + preintegratedPlus << // new preintegrated vector: + theta + w_tangent * dt, // theta + position + velocity * dt + a_nav * dt22, // position + velocity + a_nav * dt; // velocity if (A) { // Exact derivative of R*a with respect to theta: - const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp(); + const Matrix3 a_nav_H_theta = R.matrix() * skewSymmetric(-a_body) * local.dexp(); A->setIdentity(); - A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta - A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta... - A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity - A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta + A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta + A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta... + A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity + A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta } if (B) { B->block<3, 3>(0, 0) = Z_3x3; - B->block<3, 3>(3, 0) = R * dt22; - B->block<3, 3>(6, 0) = R * dt; + B->block<3, 3>(3, 0) = R.matrix() * dt22; + B->block<3, 3>(6, 0) = R.matrix() * dt; } if (C) { C->block<3, 3>(0, 0) = invH * dt; diff --git a/gtsam/slam/tests/testKarcherMeanFactor.cpp b/gtsam/slam/tests/testKarcherMeanFactor.cpp index bcc20c62a..a3e52e64d 100644 --- a/gtsam/slam/tests/testKarcherMeanFactor.cpp +++ b/gtsam/slam/tests/testKarcherMeanFactor.cpp @@ -98,7 +98,7 @@ TEST(KarcherMean, FactorSO4) { FindKarcherMean({result.at(1), result.at(2)}); EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal((Matrix)(Q * Q * Q).matrix(), - result.at(1).between(result.at(2)))); + result.at(1).between(result.at(2)).matrix())); } /* ************************************************************************* */ From 39d1f58eaeadcce21b7011eb3cb515c85f43fe4e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 8 May 2019 18:41:53 -0400 Subject: [PATCH 30/47] More asserts/tests --- gtsam/geometry/SOn.h | 5 ++++- gtsam/geometry/tests/testSO3.cpp | 1 + gtsam/geometry/tests/testSO4.cpp | 1 + gtsam/geometry/tests/testSOn.cpp | 33 +++++++++++++++++++++++++++++--- 4 files changed, 36 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 511a03a6f..fca229682 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -151,7 +151,10 @@ class SO : public LieGroup, internal::DimensionSO(N)> { /// @{ /// Multiplication - SO operator*(const SO& other) const { return SO(matrix_ * other.matrix_); } + SO operator*(const SO& other) const { + assert(dim() == other.dim()); + return SO(matrix_ * other.matrix_); + } /// SO identity for N >= 2 template > diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index e3dd5cff1..3c5b947ba 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -30,6 +30,7 @@ TEST(SO3, Identity) { EXPECT_LONGS_EQUAL(3, SO3::dimension); EXPECT_LONGS_EQUAL(3, SO3::Dim()); EXPECT_LONGS_EQUAL(3, R.dim()); + EXPECT_LONGS_EQUAL(3, traits::GetDimension(R)); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testSO4.cpp b/gtsam/geometry/tests/testSO4.cpp index 2c6342c38..594da01f6 100644 --- a/gtsam/geometry/tests/testSO4.cpp +++ b/gtsam/geometry/tests/testSO4.cpp @@ -36,6 +36,7 @@ TEST(SO4, Identity) { EXPECT_LONGS_EQUAL(6, SO4::dimension); EXPECT_LONGS_EQUAL(6, SO4::Dim()); EXPECT_LONGS_EQUAL(6, R.dim()); + EXPECT_LONGS_EQUAL(6, traits::GetDimension(R)); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index 603caa4b4..cc89f76fe 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -15,8 +15,9 @@ * @author Frank Dellaert **/ -#include #include +#include +#include #include #include @@ -45,6 +46,7 @@ TEST(SOn, SO5) { EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension); EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim()); EXPECT_LONGS_EQUAL(10, R.dim()); + EXPECT_LONGS_EQUAL(10, traits::GetDimension(R)); } //****************************************************************************** @@ -54,6 +56,14 @@ TEST(SOn, Concept) { BOOST_CONCEPT_ASSERT((IsLieGroup)); } +//****************************************************************************** +TEST(SOn, CopyConstructor) { + const auto R = SOn(5); + const auto B(R); + EXPECT_LONGS_EQUAL(5, B.rows()); + EXPECT_LONGS_EQUAL(10, B.dim()); +} + //****************************************************************************** TEST(SOn, Values) { const auto R = SOn(5); @@ -62,6 +72,7 @@ TEST(SOn, Values) { values.insert(key, R); const auto B = values.at(key); EXPECT_LONGS_EQUAL(5, B.rows()); + EXPECT_LONGS_EQUAL(10, B.dim()); } //****************************************************************************** @@ -104,9 +115,25 @@ TEST(SOn, HatVee) { //****************************************************************************** TEST(SOn, RetractLocal) { + Vector6 v1 = (Vector(6) << 0, 0, 0, 1, 0, 0).finished() / 10000; + Vector6 v2 = (Vector(6) << 0, 0, 0, 1, 2, 3).finished() / 10000; + Vector6 v3 = (Vector(6) << 3, 2, 1, 1, 2, 3).finished() / 10000; + + // Check that Cayley yields the same as Expmap for small values + SOn id(4); + EXPECT(assert_equal(id.retract(v1), SOn(SO4::Expmap(v1)))); + EXPECT(assert_equal(id.retract(v2), SOn(SO4::Expmap(v2)))); + EXPECT(assert_equal(id.retract(v3), SOn(SO4::Expmap(v3)))); + + // Same for SO3: + SOn I3(3); + EXPECT( + assert_equal(I3.retract(v1.tail<3>()), SOn(SO3::Expmap(v1.tail<3>())))); + EXPECT( + assert_equal(I3.retract(v2.tail<3>()), SOn(SO3::Expmap(v2.tail<3>())))); + // If we do expmap in SO(3) subgroup, topleft should be equal to R1. - Vector6 v1 = (Vector(6) << 0, 0, 0, 0.01, 0, 0).finished(); - Matrix R1 = SO3::Retract(v1.tail<3>()).matrix(); + Matrix R1 = SO3().retract(v1.tail<3>()).matrix(); SOn Q1 = SOn::Retract(v1); CHECK(assert_equal(R1, Q1.matrix().block(0, 0, 3, 3), 1e-7)); CHECK(assert_equal(v1, SOn::ChartAtOrigin::Local(Q1), 1e-7)); From f5b57ce59f6520980282c9fc2b0660957eb4d096 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 14 May 2019 20:43:44 -0400 Subject: [PATCH 31/47] Python tests --- cython/gtsam/tests/test_SO4.py | 14 ++++---- cython/gtsam/tests/test_SOn.py | 59 ++++++++++++++++++++++++++++++++++ gtsam.h | 19 ++++++++++- gtsam/geometry/SOn.h | 11 +++++-- 4 files changed, 93 insertions(+), 10 deletions(-) create mode 100644 cython/gtsam/tests/test_SOn.py diff --git a/cython/gtsam/tests/test_SO4.py b/cython/gtsam/tests/test_SO4.py index 3b30a8e89..648bd1710 100644 --- a/cython/gtsam/tests/test_SO4.py +++ b/cython/gtsam/tests/test_SO4.py @@ -14,10 +14,10 @@ import unittest import numpy as np from gtsam import SO4 -id = SO4() -v1 = np.array([0.1, 0, 0, 0, 0, 0]) +I4 = SO4() +v1 = np.array([0, 0, 0, .1, 0, 0]) +v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03]) Q1 = SO4.Expmap(v1) -v2 = np.array([0.01, 0.02, 0.03, 0, 0, 0]) Q2 = SO4.Expmap(v2) @@ -33,13 +33,13 @@ class TestSO4(unittest.TestCase): def test_retract(self): """Test retraction to manifold.""" v = np.zeros((6,), np.float) - actual = id.retract(v) - self.assertTrue(actual.equals(id, 1e-9)) + actual = I4.retract(v) + self.assertTrue(actual.equals(I4, 1e-9)) def test_local(self): """Check localCoordinates for trivial case.""" v0 = np.zeros((6,), np.float) - actual = id.localCoordinates(id) + actual = I4.localCoordinates(I4) np.testing.assert_array_almost_equal(actual, v0) def test_compose(self): @@ -51,7 +51,7 @@ class TestSO4(unittest.TestCase): def test_vec(self): """Check wrapping of vec.""" expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1]) - actual = id.vec() + actual = I4.vec() np.testing.assert_array_equal(actual, expected) diff --git a/cython/gtsam/tests/test_SOn.py b/cython/gtsam/tests/test_SOn.py new file mode 100644 index 000000000..7599363e2 --- /dev/null +++ b/cython/gtsam/tests/test_SOn.py @@ -0,0 +1,59 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Dynamic SO(n) unit tests. +Author: Frank Dellaert +""" +# pylint: disable=no-name-in-module, import-error +import unittest + +import numpy as np +from gtsam import SOn + +I4 = SOn(4) +v1 = np.array([0, 0, 0, .1, 0, 0]) +v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03]) +Q1 = I4.retract(v1) +Q2 = I4.retract(v2) + + +class TestSO4(unittest.TestCase): + """Test selected SOn methods.""" + + def test_constructor(self): + """Construct from matrix.""" + matrix = np.eye(4) + so4 = SOn.FromMatrix(matrix) + self.assertIsInstance(so4, SOn) + + def test_retract(self): + """Test retraction to manifold.""" + v = np.zeros((6,), np.float) + actual = I4.retract(v) + self.assertTrue(actual.equals(I4, 1e-9)) + + def test_local(self): + """Check localCoordinates for trivial case.""" + v0 = np.zeros((6,), np.float) + actual = I4.localCoordinates(I4) + np.testing.assert_array_almost_equal(actual, v0) + + def test_compose(self): + """Check compose works in subgroup.""" + expected = I4.retract(2*v1) + actual = Q1.compose(Q1) + self.assertTrue(actual.equals(expected, 1e-3)) # not exmap so only approximate + + def test_vec(self): + """Check wrapping of vec.""" + expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1]) + actual = I4.vec() + np.testing.assert_array_equal(actual, expected) + + +if __name__ == "__main__": + unittest.main() diff --git a/gtsam.h b/gtsam.h index efe36a595..e3f682187 100644 --- a/gtsam.h +++ b/gtsam.h @@ -542,6 +542,7 @@ class SO3 { // Standard Constructors SO3(); SO3(Matrix R); + static gtsam::SO3 FromMatrix(Matrix R); static gtsam::SO3 AxisAngle(const Vector axis, double theta); static gtsam::SO3 ClosestTo(const Matrix M); @@ -570,6 +571,7 @@ class SO4 { // Standard Constructors SO4(); SO4(Matrix R); + static gtsam::SO4 FromMatrix(Matrix R); // Testable void print(string s) const; @@ -595,7 +597,22 @@ class SO4 { class SOn { // Standard Constructors SOn(size_t n); - SOn(Matrix R); + static gtsam::SOn FromMatrix(Matrix R); + + // Testable + void print(string s) const; + bool equals(const gtsam::SOn& other, double tol) const; + + // Group + static gtsam::SOn identity(); + gtsam::SOn inverse() const; + gtsam::SOn between(const gtsam::SOn& Q) const; + gtsam::SOn compose(const gtsam::SOn& Q) const; + + // Manifold + gtsam::SOn retract(Vector v) const; + Vector localCoordinates(const gtsam::SOn& Q) const; + static gtsam::SOn Expmap(Vector v); // Other methods Vector vec() const; diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index fca229682..eea24f1a8 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -76,14 +76,21 @@ class SO : public LieGroup, internal::DimensionSO(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."); + // We allow for n=0 as the default constructor, needed for serialization, + // wrappers etc. matrix_ = Eigen::MatrixXd::Identity(n, n); } - /// Constructor from Eigen Matrix + /// Constructor from Eigen Matrix, dynamic version template explicit SO(const Eigen::MatrixBase& R) : matrix_(R.eval()) {} + /// Named constructor from Eigen Matrix + template + static SO FromMatrix(const Eigen::MatrixBase& R) { + return SO(R); + } + /// Construct dynamic SO(n) from Fixed SO template > explicit SO(const SO& R) : matrix_(R.matrix()) {} From a573658ba479551450da4fca3bf6fe2565e98bb2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Jun 2019 14:45:10 -0400 Subject: [PATCH 32/47] Updated timing script --- timing/timeRot3.cpp | 36 ++++++++++++++++++------------------ 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/timing/timeRot3.cpp b/timing/timeRot3.cpp index 00c02c250..3a6a156d5 100644 --- a/timing/timeRot3.cpp +++ b/timing/timeRot3.cpp @@ -23,33 +23,33 @@ using namespace std; using namespace gtsam; -#define TEST(TITLE,STATEMENT) \ - cout << endl << TITLE << endl;\ - timeLog = clock();\ - for(int i = 0; i < n; i++)\ - STATEMENT;\ - timeLog2 = clock();\ - seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;\ - cout << seconds << " seconds" << endl;\ - cout << ((double)n/seconds) << " calls/second" << endl; +#define TEST(TITLE, STATEMENT) \ + cout << endl << TITLE << endl; \ + timeLog = clock(); \ + for (int i = 0; i < n; i++) STATEMENT; \ + timeLog2 = clock(); \ + seconds = static_cast(timeLog2 - timeLog) / CLOCKS_PER_SEC; \ + cout << 1000 * seconds << " milliseconds" << endl; \ + cout << (1e9 * seconds / static_cast(n)) << " nanosecs/call" << endl; -int main() -{ - int n = 100000; long timeLog, timeLog2; double seconds; +int main() { + int n = 100000; + clock_t timeLog, timeLog2; + double seconds; // create a random direction: - double norm=sqrt(1.0+16.0+4.0); - double x=1.0/norm, y=4.0/norm, z=2.0/norm; + double norm = sqrt(1.0 + 16.0 + 4.0); + double x = 1.0 / norm, y = 4.0 / norm, z = 2.0 / norm; Vector v = (Vector(3) << x, y, z).finished(); Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2), R2 = R.retract(v); - TEST("Rodriguez formula given axis angle", Rot3::AxisAngle(v,0.001)) + TEST("Rodriguez formula given axis angle", Rot3::AxisAngle(v, 0.001)) TEST("Rodriguez formula given canonical coordinates", Rot3::Rodrigues(v)) - TEST("Expmap", R*Rot3::Expmap(v)) + TEST("Expmap", R * Rot3::Expmap(v)) TEST("Retract", R.retract(v)) TEST("Logmap", Rot3::Logmap(R.between(R2))) TEST("localCoordinates", R.localCoordinates(R2)) - TEST("Slow rotation matrix",Rot3::Rz(z)*Rot3::Ry(y)*Rot3::Rx(x)) - TEST("Fast Rotation matrix", Rot3::RzRyRx(x,y,z)) + TEST("Slow rotation matrix", Rot3::Rz(z) * Rot3::Ry(y) * Rot3::Rx(x)) + TEST("Fast Rotation matrix", Rot3::RzRyRx(x, y, z)) return 0; } From 9d23fe52e010d566e581b2e56e2ed97bd0ccf103 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Jun 2019 15:24:37 -0400 Subject: [PATCH 33/47] Fixed issue with quaternions and added "special" travis stage that checks quaternions path. --- .travis.yml | 15 +++++++++++---- gtsam/geometry/Rot3.h | 3 ++- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/.travis.yml b/.travis.yml index 1e2d6760a..d7863cfdd 100644 --- a/.travis.yml +++ b/.travis.yml @@ -27,6 +27,7 @@ install: stages: - compile - test + - special # Compile stage without building examples/tests to populate the caches. jobs: @@ -75,12 +76,18 @@ jobs: compiler: clang env: CMAKE_BUILD_TYPE=Release script: bash .travis.sh -b -# on Linux, with deprecated ON to make sure that path still compiles - - stage: compile +# on Linux, with deprecated ON to make sure that path still compiles/tests + - stage: special os: linux compiler: clang - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON - script: bash .travis.sh -b + env: CMAKE_BUILD_TYPE=Release GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON + script: bash .travis.sh -t +# on Linux, with quaternions ON to make sure that path still compiles/tests + - stage: special + os: linux + compiler: clang + env: CMAKE_BUILD_TYPE=Release GTSAM_BUILD_UNSTABLE=OFF GTSAM_USE_QUATERNIONS=ON + script: bash .travis.sh -t # Matrix configuration: os: diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index f3a6f08cd..ab43b2d42 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -94,7 +94,8 @@ namespace gtsam { */ template #ifdef GTSAM_USE_QUATERNIONS - explicit Rot3(const Eigen::MatrixBase& R) : quaternion_(R) { + explicit Rot3(const Eigen::MatrixBase& R) { + quaternion_ = Matrix3(R); } #else explicit Rot3(const Eigen::MatrixBase& R) : rot_(R) { From f1e8a20a5a9798f68083d1f9e06bb46cebb015a0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Jun 2019 15:34:41 -0400 Subject: [PATCH 34/47] Cleaned up some unused headers --- gtsam/geometry/SO3.h | 1 - gtsam/geometry/SO4.h | 1 - gtsam/geometry/SOn.h | 1 - gtsam/linear/tests/testGaussianBayesTree.cpp | 1 - 4 files changed, 4 deletions(-) diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 2b84dafba..eb6129ac8 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -26,7 +26,6 @@ #include #include -#include #include namespace gtsam { diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h index 0cc3bb9fe..c8e85b63f 100644 --- a/gtsam/geometry/SO4.h +++ b/gtsam/geometry/SO4.h @@ -28,7 +28,6 @@ #include -#include #include namespace gtsam { diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index eea24f1a8..529bcc1bd 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -25,7 +25,6 @@ #include #include -#include #include #include diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index ca3ebf91c..17dc6a425 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -25,7 +25,6 @@ using namespace boost::assign; #include #include -#include #include #include #include From 158b99f030d92eef71cc7d264b660f31da44880d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Jun 2019 15:39:44 -0400 Subject: [PATCH 35/47] Made sure GTSAM_USE_QUATERNIONS flag is set and used --- .travis.sh | 2 ++ .travis.yml | 1 + 2 files changed, 3 insertions(+) diff --git a/.travis.sh b/.travis.sh index 3cec20f53..1d71f263b 100755 --- a/.travis.sh +++ b/.travis.sh @@ -47,6 +47,7 @@ function build () -DCMAKE_BUILD_TYPE=$CMAKE_BUILD_TYPE \ -DGTSAM_BUILD_TESTS=OFF \ -DGTSAM_BUILD_UNSTABLE=$GTSAM_BUILD_UNSTABLE \ + -DGTSAM_USE_QUATERNIONS=$GTSAM_USE_QUATERNIONS \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=ON \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=$GTSAM_ALLOW_DEPRECATED_SINCE_V4 @@ -65,6 +66,7 @@ function test () -DCMAKE_BUILD_TYPE=$CMAKE_BUILD_TYPE \ -DGTSAM_BUILD_TESTS=ON \ -DGTSAM_BUILD_UNSTABLE=$GTSAM_BUILD_UNSTABLE \ + -DGTSAM_USE_QUATERNIONS=$GTSAM_USE_QUATERNIONS \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF diff --git a/.travis.yml b/.travis.yml index d7863cfdd..50553e081 100644 --- a/.travis.yml +++ b/.travis.yml @@ -101,6 +101,7 @@ env: - MAKEFLAGS="-j2" - CCACHE_SLOPPINESS=pch_defines,time_macros - GTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF + - GTSAM_USE_QUATERNIONS=OFF - GTSAM_BUILD_UNSTABLE=ON matrix: - CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF From 1e5973628e05ec849184b2cf32b960677f6ecaf6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Jun 2019 16:03:47 -0400 Subject: [PATCH 36/47] Worked around build snafu on Travis with enable_if_t (not in all boost versions, and only in std as of c++14) --- gtsam/geometry/SOn.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 529bcc1bd..a827b3a63 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -26,6 +26,7 @@ #include #include +#include #include namespace gtsam { @@ -58,11 +59,11 @@ class SO : public LieGroup, internal::DimensionSO(N)> { // 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; + using IsDynamic = typename std::enable_if::type; template - using IsFixed = boost::enable_if_t= 2, void>; + using IsFixed = typename std::enable_if= 2, void>::type; template - using IsSO3 = boost::enable_if_t; + using IsSO3 = typename std::enable_if::type; public: /// @name Constructors From e758089f13ece3412e386817ef47fd0a6eb33fd3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Jun 2019 16:25:53 -0400 Subject: [PATCH 37/47] Fixed test --- gtsam/geometry/tests/testSOn.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index cc89f76fe..384150c52 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -46,7 +46,7 @@ TEST(SOn, SO5) { EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension); EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim()); EXPECT_LONGS_EQUAL(10, R.dim()); - EXPECT_LONGS_EQUAL(10, traits::GetDimension(R)); + EXPECT_LONGS_EQUAL(-1, traits::GetDimension(R)); } //****************************************************************************** From 40af21914bac071a06004cb19ceaefabdef27503 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Jun 2019 19:32:45 -0400 Subject: [PATCH 38/47] Fixed issue with static vector being const --- gtsam/geometry/SO3.cpp | 13 +++++++------ gtsam/geometry/SO4.cpp | 2 +- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 31d63d312..de595239a 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -68,7 +68,8 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) } } -ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox) +ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, + bool nearZeroApprox) : theta2(angle * angle), theta(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; @@ -88,9 +89,9 @@ SO3 ExpmapFunctor::expmap() const { DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { - if (nearZero) + if (nearZero) { dexp_ = I_3x3 - 0.5 * W; - else { + } else { a = one_minus_cos / theta; b = 1.0 - sin_theta / theta; dexp_ = I_3x3 - a * K + b * KK; @@ -296,9 +297,9 @@ static Vector9 vec3(const Matrix3& R) { } // so<3> generators -static const std::vector G3({SO3::Hat(Vector3::Unit(0)), - SO3::Hat(Vector3::Unit(1)), - SO3::Hat(Vector3::Unit(2))}); +static std::vector G3({SO3::Hat(Vector3::Unit(0)), + SO3::Hat(Vector3::Unit(1)), + SO3::Hat(Vector3::Unit(2))}); // vectorized generators static const Matrix93 P3 = diff --git a/gtsam/geometry/SO4.cpp b/gtsam/geometry/SO4.cpp index 93c5f7f8e..5660e85d5 100644 --- a/gtsam/geometry/SO4.cpp +++ b/gtsam/geometry/SO4.cpp @@ -138,7 +138,7 @@ static SO4::VectorN2 vec4(const Matrix4& Q) { } // so<4> generators -static const std::vector G4( +static 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))}); From 41902efc79a8337a58d8e52d1eec04a2f2040387 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 15 Jun 2019 21:12:55 -0400 Subject: [PATCH 39/47] Another attempt at getting past Linux compiler --- gtsam/geometry/SO3.cpp | 6 +++--- gtsam/geometry/SO4.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index de595239a..7ea9dfb07 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -297,9 +297,9 @@ static Vector9 vec3(const Matrix3& R) { } // so<3> generators -static std::vector G3({SO3::Hat(Vector3::Unit(0)), - SO3::Hat(Vector3::Unit(1)), - SO3::Hat(Vector3::Unit(2))}); +static std::vector G3({SO3::Hat(Vector3::Unit(0)), + SO3::Hat(Vector3::Unit(1)), + SO3::Hat(Vector3::Unit(2))}); // vectorized generators static const Matrix93 P3 = diff --git a/gtsam/geometry/SO4.cpp b/gtsam/geometry/SO4.cpp index 5660e85d5..93fb05386 100644 --- a/gtsam/geometry/SO4.cpp +++ b/gtsam/geometry/SO4.cpp @@ -138,7 +138,7 @@ static SO4::VectorN2 vec4(const Matrix4& Q) { } // so<4> generators -static std::vector G4( +static 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))}); From 78af8894f20ee09f4f98a019181516b360790375 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 18 Nov 2019 11:27:08 -0500 Subject: [PATCH 40/47] Fix alignment on SO --- gtsam/geometry/SOn.h | 3 +++ gtsam/linear/GaussianBayesNet.cpp | 4 ++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index a827b3a63..06031b093 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -53,6 +53,9 @@ class SO : public LieGroup, internal::DimensionSO(N)> { using VectorN2 = Eigen::Matrix; using MatrixDD = Eigen::Matrix; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + protected: MatrixNN matrix_; ///< Rotation matrix diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index bc96452b9..4118d8f71 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -194,9 +194,9 @@ namespace gtsam { if(cg->get_model()) { Vector diag = cg->R().diagonal(); cg->get_model()->whitenInPlace(diag); - logDet += diag.unaryExpr(ptr_fun(log)).sum(); + logDet += diag.unaryExpr([](double c){return log(c);}).sum(); } else { - logDet += cg->R().diagonal().unaryExpr(ptr_fun(log)).sum(); + logDet += cg->R().diagonal().unaryExpr([](double c){return log(c);}).sum(); } } return logDet; From 520bb970f381d6ab2284749ca5efcf3363eda0f9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 28 Nov 2019 17:35:46 -0800 Subject: [PATCH 41/47] Addressed Duy's comments --- gtsam/geometry/SOn-inl.h | 7 ++++++- gtsam/geometry/SOn.h | 10 ++++------ gtsam/geometry/tests/testRot3.cpp | 1 - gtsam/geometry/tests/testSO3.cpp | 7 ++++--- gtsam/geometry/tests/testSOn.cpp | 11 +++++++++++ gtsam/slam/KarcherMeanFactor.h | 10 +++++----- 6 files changed, 30 insertions(+), 16 deletions(-) diff --git a/gtsam/geometry/SOn-inl.h b/gtsam/geometry/SOn-inl.h index 1c05eac47..344822c1f 100644 --- a/gtsam/geometry/SOn-inl.h +++ b/gtsam/geometry/SOn-inl.h @@ -36,15 +36,18 @@ typename SO::TangentVector SO::Vee(const MatrixNN& X) { template SO SO::ChartAtOrigin::Retract(const TangentVector& xi, ChartJacobian H) { + if (H) throw std::runtime_error("SO::Retract jacobian not implemented."); const Matrix X = Hat(xi / 2.0); size_t n = AmbientDim(xi.size()); const auto I = Eigen::MatrixXd::Identity(n, n); + // https://pdfs.semanticscholar.org/6165/0347b2ccac34b5f423081d1ce4dbc4d09475.pdf return SO((I + X) * (I - X).inverse()); } template typename SO::TangentVector SO::ChartAtOrigin::Local(const SO& R, ChartJacobian H) { + if (H) throw std::runtime_error("SO::Local jacobian not implemented."); const size_t n = R.rows(); const auto I = Eigen::MatrixXd::Identity(n, n); const Matrix X = (I - R.matrix_) * (I + R.matrix_).inverse(); @@ -87,9 +90,11 @@ typename SO::VectorN2 SO::vec( VectorN2 X(n2); X << Eigen::Map(matrix_.data(), n2, 1); - // If requested, calculate H as (I \oplus Q) * P + // If requested, calculate H as (I \oplus Q) * P, + // where Q is the N*N rotation matrix, and P is calculated below. if (H) { // Calculate P matrix of vectorized generators + // TODO(duy): Should we refactor this as the jacobian of Hat? const size_t d = dim(); Matrix P(n2, d); for (size_t j = 0; j < d; j++) { diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 06031b093..7954c4d6c 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -11,8 +11,7 @@ /** * @file SOn.h - * @brief n*n matrix representation of SO(n), template on N, which can be - * Eigen::Dynamic + * @brief N*N matrix representation of SO(N). N can be Eigen::Dynamic * @author Frank Dellaert * @date March 2019 */ @@ -43,7 +42,7 @@ constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; } /** * Manifold of special orthogonal rotation matrices SO. - * Template paramater N can be a fizxed integer or can be Eigen::Dynamic + * Template paramater N can be a fixed integer or can be Eigen::Dynamic */ template class SO : public LieGroup, internal::DimensionSO(N)> { @@ -53,7 +52,6 @@ class SO : public LieGroup, internal::DimensionSO(N)> { using VectorN2 = Eigen::Matrix; using MatrixDD = Eigen::Matrix; - public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW protected: @@ -100,7 +98,7 @@ class SO : public LieGroup, internal::DimensionSO(N)> { /// Constructor from AngleAxisd template > - SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {} + explicit SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {} /// Constructor from axis and angle. Only defined for SO3 static SO AxisAngle(const Vector3& axis, double theta); @@ -117,7 +115,7 @@ class SO : public LieGroup, internal::DimensionSO(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! + // TODO(frank): 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); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 0365bc659..c95b85f21 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -647,7 +647,6 @@ TEST(Rot3 , ChartDerivatives) { /* ************************************************************************* */ TEST(Rot3, ClosestTo) { - // Example top-left of SO(4) matrix not quite on SO(3) manifold Matrix3 M; M << 0.79067393, 0.6051136, -0.0930814, // 0.4155925, -0.64214347, -0.64324489, // diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 3c5b947ba..b2c781b35 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -15,10 +15,12 @@ * @author Frank Dellaert **/ -#include +#include + #include #include -#include + +#include using namespace std; using namespace gtsam; @@ -51,7 +53,6 @@ TEST(SO3, Constructors) { /* ************************************************************************* */ TEST(SO3, ClosestTo) { - // Example top-left of SO(4) matrix not quite on SO(3) manifold Matrix3 M; M << 0.79067393, 0.6051136, -0.0930814, // 0.4155925, -0.64214347, -0.64324489, // diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index 384150c52..4e5f12c0c 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -39,6 +39,17 @@ using namespace std; using namespace gtsam; +//****************************************************************************** +// Test dhynamic with n=0 +TEST(SOn, SO0) { + const auto R = SOn(0); + EXPECT_LONGS_EQUAL(0, R.rows()); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim()); + EXPECT_LONGS_EQUAL(0, R.dim()); + EXPECT_LONGS_EQUAL(-1, traits::GetDimension(R)); +} + //****************************************************************************** TEST(SOn, SO5) { const auto R = SOn(5); diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h index d0f2f57db..c098c9665 100644 --- a/gtsam/slam/KarcherMeanFactor.h +++ b/gtsam/slam/KarcherMeanFactor.h @@ -12,7 +12,7 @@ /* * @file KarcherMeanFactor.h * @author Frank Dellaert - * @date March 2019ƒ + * @date March 2019 */ #pragma once @@ -26,18 +26,18 @@ namespace gtsam { /** * Optimize for the Karcher mean, minimizing the geodesic distance to each of - * the given rotations, ,by constructing a factor graph out of simple + * the given rotations, by constructing a factor graph out of simple * PriorFactors. */ template T FindKarcherMean(const std::vector& rotations); /** - * The KarcherMeanFactor creates a constraint on all SO(3) variables with + * The KarcherMeanFactor creates a constraint on all SO(n) variables with * given keys that the Karcher mean (see above) will stay the same. Note the * mean itself is irrelevant to the constraint and is not a parameter: the * constraint is implemented as enforcing that the sum of local updates is - * equal to zero, hence creating a rank-3 constraint. Note it is implemented as + * equal to zero, hence creating a rank-dim constraint. Note it is implemented as * a soft constraint, as typically it is used to fix a gauge freedom. * */ template @@ -67,4 +67,4 @@ class KarcherMeanFactor : public NonlinearFactor { } }; // \KarcherMeanFactor -} // namespace gtsam \ No newline at end of file +} // namespace gtsam From 522dbc00f66d54770e93ed21cd4a39aedae493b4 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 3 Dec 2019 13:51:37 -0500 Subject: [PATCH 42/47] Use aligned_allocator for static allocation --- gtsam/geometry/SO4.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/SO4.cpp b/gtsam/geometry/SO4.cpp index 93fb05386..dd15807c3 100644 --- a/gtsam/geometry/SO4.cpp +++ b/gtsam/geometry/SO4.cpp @@ -138,7 +138,7 @@ static SO4::VectorN2 vec4(const Matrix4& Q) { } // so<4> generators -static std::vector G4( +static 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))}); From 1ab21c0ed3810a229d6cfc62937c5b611df8f156 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 4 Dec 2019 15:53:57 -0500 Subject: [PATCH 43/47] Workaround for upstream boost issue #119 --- gtsam/inference/BayesTreeCliqueBase.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 317ba1c44..cd8157a26 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -166,7 +166,9 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(conditional_); - ar & BOOST_SERIALIZATION_NVP(parent_); + if (parent_) { // TODO(fan): Workaround for boost/serialization #119 + ar & BOOST_SERIALIZATION_NVP(parent_); + } ar & BOOST_SERIALIZATION_NVP(children); } From 1b4b292a6ed21363882920f6b247174d7672dc42 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 6 Dec 2019 14:08:04 -0500 Subject: [PATCH 44/47] Refine workaround --- gtsam/inference/BayesTreeCliqueBase.h | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index cd8157a26..6266e961c 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -86,6 +86,8 @@ namespace gtsam { FastVector children; int problemSize_; + bool is_root = false; + /// Fill the elimination result produced during elimination. Here this just stores the /// conditional and ignores the remaining factor, but this is overridden in ISAM2Clique /// to also cache the remaining factor. @@ -165,8 +167,12 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + if(!parent_.lock()) { + is_root = true; + } + ar & BOOST_SERIALIZATION_NVP(is_root); ar & BOOST_SERIALIZATION_NVP(conditional_); - if (parent_) { // TODO(fan): Workaround for boost/serialization #119 + if (!is_root) { // TODO(fan): Workaround for boost/serialization #119 ar & BOOST_SERIALIZATION_NVP(parent_); } ar & BOOST_SERIALIZATION_NVP(children); From 6d0705c5dc41031bc3f4a143027dbe09f8d1c48e Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 8 Dec 2019 00:50:30 -0500 Subject: [PATCH 45/47] Workaround for boost serialization bug for NULL shared_ptrs version < 1.66.0 --- gtsam/linear/JacobianFactor.h | 42 ++++++++++++++++++++++++++++++----- 1 file changed, 37 insertions(+), 5 deletions(-) diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index c8d77c554..53ce4b9ca 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -25,6 +25,8 @@ #include #include +#include +#include namespace gtsam { @@ -406,13 +408,41 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(Ab_); - ar & BOOST_SERIALIZATION_NVP(model_); + void save(ARCHIVE & ar, const unsigned int version) const { + // TODO(fan): This is a hack for Boost < 1.66 + // We really need to introduce proper versioning in the archives + // As otherwise this will not read objects serialized by older + // versions of GTSAM + ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar << BOOST_SERIALIZATION_NVP(Ab_); + bool model_null = false; + if(model_.get() == nullptr) { + model_null = true; + ar << boost::serialization::make_nvp("model_null", model_null); + } else { + ar << boost::serialization::make_nvp("model_null", model_null); + ar << BOOST_SERIALIZATION_NVP(model_); + } } - }; // JacobianFactor + template + void load(ARCHIVE & ar, const unsigned int version) { + // invoke serialization of the base class + ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar >> BOOST_SERIALIZATION_NVP(Ab_); + if (version < 1) { + ar >> BOOST_SERIALIZATION_NVP(model_); + } else { + bool model_null; + ar >> BOOST_SERIALIZATION_NVP(model_null); + if (!model_null) { + ar >> BOOST_SERIALIZATION_NVP(model_); + } + } + } + + BOOST_SERIALIZATION_SPLIT_MEMBER() + }; // JacobianFactor /// traits template<> struct traits : public Testable { @@ -420,6 +450,8 @@ struct traits : public Testable { } // \ namespace gtsam +BOOST_CLASS_VERSION(gtsam::JacobianFactor, 1) + #include From b156a6498e67e06dafc4015ccc7235fd18bfa99b Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 10 Dec 2019 12:19:38 -0500 Subject: [PATCH 46/47] Fix KarcherMeanFactor --- gtsam/slam/KarcherMeanFactor-inl.h | 14 ++++++++++++-- gtsam/slam/KarcherMeanFactor.h | 5 ++++- gtsam/slam/tests/testKarcherMeanFactor.cpp | 4 +++- 3 files changed, 19 insertions(+), 4 deletions(-) diff --git a/gtsam/slam/KarcherMeanFactor-inl.h b/gtsam/slam/KarcherMeanFactor-inl.h index cfe071ee5..8ff61d796 100644 --- a/gtsam/slam/KarcherMeanFactor-inl.h +++ b/gtsam/slam/KarcherMeanFactor-inl.h @@ -26,8 +26,8 @@ using namespace std; namespace gtsam { -template -T FindKarcherMean(const vector& rotations) { +template +T FindKarcherMeanImpl(const vector& rotations) { // Cost function C(R) = \sum PriorFactor(R_i)::error(R) // No closed form solution. NonlinearFactorGraph graph; @@ -41,6 +41,16 @@ T FindKarcherMean(const vector& rotations) { return result.at(kKey); } +template +T FindKarcherMean(const vector& rotations) { + return FindKarcherMeanImpl(rotations); +} + +template +T FindKarcherMean(std::initializer_list&& rotations) { + return FindKarcherMeanImpl(std::vector >(rotations)); +} + template template KarcherMeanFactor::KarcherMeanFactor(const CONTAINER& keys, int d) diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h index c098c9665..f62cb8904 100644 --- a/gtsam/slam/KarcherMeanFactor.h +++ b/gtsam/slam/KarcherMeanFactor.h @@ -29,8 +29,11 @@ namespace gtsam { * the given rotations, by constructing a factor graph out of simple * PriorFactors. */ +template > +T FindKarcherMean(const std::vector& rotations); + template -T FindKarcherMean(const std::vector& rotations); +T FindKarcherMean(std::initializer_list&& rotations); /** * The KarcherMeanFactor creates a constraint on all SO(n) variables with diff --git a/gtsam/slam/tests/testKarcherMeanFactor.cpp b/gtsam/slam/tests/testKarcherMeanFactor.cpp index a3e52e64d..64cdb1682 100644 --- a/gtsam/slam/tests/testKarcherMeanFactor.cpp +++ b/gtsam/slam/tests/testKarcherMeanFactor.cpp @@ -91,7 +91,9 @@ TEST(KarcherMean, FactorSO4) { Values initial; initial.insert(1, Q.inverse()); initial.insert(2, Q); - const auto expected = FindKarcherMean({Q, Q.inverse()}); + + std::vector > rotations = {Q, Q.inverse()}; + const auto expected = FindKarcherMean(rotations); auto result = GaussNewtonOptimizer(graph, initial).optimize(); const auto actual = From 4d256eae0fd134cd1ca236a5e15fee9d07959e50 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 10 Dec 2019 15:10:32 -0500 Subject: [PATCH 47/47] Additional Fixes for Rot3 --- gtsam/slam/KarcherMeanFactor-inl.h | 10 ++++++++-- gtsam/slam/KarcherMeanFactor.h | 4 ++-- gtsam/slam/tests/testKarcherMeanFactor.cpp | 2 +- 3 files changed, 11 insertions(+), 5 deletions(-) diff --git a/gtsam/slam/KarcherMeanFactor-inl.h b/gtsam/slam/KarcherMeanFactor-inl.h index 8ff61d796..076b0ff0c 100644 --- a/gtsam/slam/KarcherMeanFactor-inl.h +++ b/gtsam/slam/KarcherMeanFactor-inl.h @@ -41,8 +41,14 @@ T FindKarcherMeanImpl(const vector& rotations) { return result.at(kKey); } -template -T FindKarcherMean(const vector& rotations) { +template ::value >::type > +T FindKarcherMean(const std::vector& rotations) { + return FindKarcherMeanImpl(rotations); +} + +template +T FindKarcherMean(const std::vector>& rotations) { return FindKarcherMeanImpl(rotations); } diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h index f62cb8904..54b3930d4 100644 --- a/gtsam/slam/KarcherMeanFactor.h +++ b/gtsam/slam/KarcherMeanFactor.h @@ -29,8 +29,8 @@ namespace gtsam { * the given rotations, by constructing a factor graph out of simple * PriorFactors. */ -template > -T FindKarcherMean(const std::vector& rotations); +template +T FindKarcherMean(const std::vector>& rotations); template T FindKarcherMean(std::initializer_list&& rotations); diff --git a/gtsam/slam/tests/testKarcherMeanFactor.cpp b/gtsam/slam/tests/testKarcherMeanFactor.cpp index 64cdb1682..c129b8fa3 100644 --- a/gtsam/slam/tests/testKarcherMeanFactor.cpp +++ b/gtsam/slam/tests/testKarcherMeanFactor.cpp @@ -75,7 +75,7 @@ static const SO4 Q = SO4::Expmap((Vector6() << 1, 2, 3, 4, 5, 6).finished()); /* ************************************************************************* */ TEST(KarcherMean, FindSO4) { - std::vector rotations = {Q, Q.inverse()}; + std::vector> rotations = {Q, Q.inverse()}; auto expected = SO4(); //::ChordalMean(rotations); auto actual = FindKarcherMean(rotations); EXPECT(assert_equal(expected, actual));