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;