From 74c588aee919f1f73013797ae323c81b70bcbdc9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 26 Dec 2014 16:11:53 +0100 Subject: [PATCH] Re-factored scalar traits and fixed Matrix traits (bug in RowMajor, compile issues for dynamic types) --- gtsam/base/VectorSpace.h | 320 ++++++++++++++++++-------------- gtsam/base/tests/testMatrix.cpp | 134 +++++++------ gtsam/base/tests/testVector.cpp | 60 +++--- tests/testManifold.cpp | 80 ++++---- 4 files changed, 328 insertions(+), 266 deletions(-) diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index ffeec3030..2e0b93fc0 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -24,12 +24,8 @@ namespace internal { template struct VectorSpaceImpl { - typedef vector_space_tag structure_category; - /// @name Group /// @{ - typedef additive_group_tag group_flavor; - static Class Identity() { return Class::identity();} static Class Compose(const Class& v1, const Class& v2) { return v1+v2;} static Class Between(const Class& v1, const Class& v2) { return v2-v1;} static Class Inverse(const Class& m) { return -m;} @@ -54,7 +50,7 @@ struct VectorSpaceImpl { ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { if (H1) *H1 = Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); - return origin + Class(v); + return origin + (Class)v; } /// @} @@ -100,8 +96,6 @@ struct VectorSpaceImpl { /// @name Group /// @{ - typedef additive_group_tag group_flavor; - static Class Identity() { return Class::identity();} static Class Compose(const Class& v1, const Class& v2) { return v1+v2;} static Class Between(const Class& v1, const Class& v2) { return v2-v1;} static Class Inverse(const Class& m) { return -m;} @@ -180,6 +174,12 @@ struct VectorSpace: Testable, VectorSpaceImpl { typedef vector_space_tag structure_category; + /// @name Group + /// @{ + typedef additive_group_tag group_flavor; + static Class Identity() { return Class::identity();} + /// @} + /// @name Manifold /// @{ enum { dimension = Class::dimension}; @@ -192,96 +192,62 @@ struct VectorSpace: Testable, VectorSpaceImpl { /// To use this for your gtsam type, define: /// template<> struct traits : public ScalarTraits { }; template -struct ScalarTraits { +struct ScalarTraits : VectorSpaceImpl { typedef vector_space_tag structure_category; + /// @name Testable + /// @{ + static void Print(Scalar m, const std::string& str = "") { + gtsam::print(m, str); + } + static bool Equals(Scalar v1, Scalar v2, double tol = 1e-8) { + return std::abs(v1 - v2) < tol; + } + /// @} + /// @name Group /// @{ typedef additive_group_tag group_flavor; static Scalar Identity() { return 0;} - static Scalar Compose(const Scalar& v1, const Scalar& v2) { return v1+v2;} - static Scalar Between(const Scalar& v1, const Scalar& v2) { return v2-v1;} - static Scalar Inverse(const Scalar& m) { return -m;} /// @} - // Typedefs required by all manifold types. + /// @name Manifold + /// @{ typedef Scalar ManifoldType; enum { dimension = 1 }; typedef Eigen::Matrix TangentVector; typedef OptionalJacobian<1, 1> ChartJacobian; - // For Testable - static void Print(Scalar m, const std::string& str = "") { - gtsam::print(m, str); - } - static bool Equals(Scalar v1, Scalar v2, double tol = 1e-8) { - return fabs(v1 - v2) < tol; - } - - static TangentVector Local(Scalar origin, Scalar other) { + static TangentVector Local(Scalar origin, Scalar other, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) (*H1)[0] = -1.0; + if (H2) (*H2)[0] = 1.0; TangentVector result; result(0) = other - origin; return result; } - static Scalar Retract(Scalar origin, const TangentVector& v) { - return origin + v[0]; - } - - static TangentVector Local(Scalar origin, Scalar other, ChartJacobian Horigin, - ChartJacobian Hother = boost::none) { - if (Horigin) (*Horigin)[0] = -1.0; - if (Hother) (*Hother)[0] = 1.0; - return Local(origin, other); - } - static Scalar Retract(Scalar origin, const TangentVector& v, - ChartJacobian Horigin, ChartJacobian Hv = boost::none) { - if (Horigin) (*Horigin)[0] = 1.0; - if (Hv) (*Hv)[0] = 1.0; - return origin + v[0]; - } - - static int GetDimension(Scalar m) {return 1;} - - static Scalar Compose(Scalar v1, Scalar v2, ChartJacobian H1, - ChartJacobian H2 = boost::none) { + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { if (H1) (*H1)[0] = 1.0; if (H2) (*H2)[0] = 1.0; - return v1 + v2; + return origin + v[0]; } + /// @} - static Scalar Between(Scalar v1, Scalar v2, ChartJacobian H1, - ChartJacobian H2 = boost::none) { - if (H1) (*H1)[0] = -1.0; - if (H2) (*H2)[0] = 1.0; - return v2 - v1; - } - - static Scalar Inverse(Scalar m, ChartJacobian H) { - if (H) (*H)[0] = -1; - return -m; - } - - static TangentVector Logmap(Scalar m) { - return Local(0, m); - } - static Scalar Expmap(const TangentVector& v) { - return v[0]; - } - - static TangentVector Logmap(Scalar m, ChartJacobian Hm) { - if (Hm) - (*Hm)[0] = 1.0; + /// @name Lie Group + /// @{ + static TangentVector Logmap(Scalar m, ChartJacobian H = boost::none) { + if (H) (*H)[0] = 1.0; return Local(0, m); } - static Scalar Expmap(const TangentVector& v, ChartJacobian Hv) { - if (Hv) - (*Hv)[0] = 1.0; + static Scalar Expmap(const TangentVector& v, ChartJacobian H = boost::none) { + if (H) (*H)[0] = 1.0; return v[0]; } + /// @} }; @@ -295,108 +261,190 @@ template<> struct traits_x : public internal::ScalarTraits { template<> struct traits_x : public internal::ScalarTraits { }; -// traits for any double Eigen matrix +// traits for any fixed double Eigen matrix template -struct traits_x > { -// BOOST_STATIC_ASSERT_MSG( -// M != Eigen::Dynamic && N != Eigen::Dynamic, -// "These traits are only valid on fixed-size types."); +struct traits_x > : + internal::VectorSpaceImpl< + Eigen::Matrix, M * N> { - // Typedefs required by all manifold types. typedef vector_space_tag structure_category; - enum { - dimension = ( - M == Eigen::Dynamic ? Eigen::Dynamic : - (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)) - }; - typedef Eigen::Matrix TangentVector; - typedef OptionalJacobian ChartJacobian; - typedef Eigen::Matrix ManifoldType; + typedef Eigen::Matrix Fixed; - static int GetDimension(const ManifoldType& m) { + /// @name Testable + /// @{ + static void Print(const Fixed& m, const std::string& str = "") { + gtsam::print(Eigen::MatrixXd(m), str); + } + static bool Equals(const Fixed& v1, const Fixed& v2, double tol = 1e-8) { + return equal_with_abs_tol(v1, v2, tol); + } + /// @} + + /// @name Group + /// @{ + typedef additive_group_tag group_flavor; + static Fixed Identity() { return Fixed::Zero();} + /// @} + + /// @name Manifold + /// @{ + enum { dimension = M*N}; + typedef Fixed ManifoldType; + typedef Eigen::Matrix TangentVector; + typedef Eigen::Matrix Jacobian; + typedef OptionalJacobian ChartJacobian; + + static TangentVector Local(Fixed origin, Fixed other, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) (*H1) = -Jacobian::Identity(); + if (H2) (*H2) = Jacobian::Identity(); + TangentVector result; + Eigen::Map(result.data()) = other - origin; + return result; + } + + static Fixed Retract(Fixed origin, const TangentVector& v, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) (*H1) = Jacobian::Identity(); + if (H2) (*H2) = Jacobian::Identity(); + return origin + Eigen::Map(v.data()); + } + /// @} + + /// @name Lie Group + /// @{ + static TangentVector Logmap(const Fixed& m, ChartJacobian H = boost::none) { + if (H) *H = Jacobian::Identity(); + TangentVector result; + Eigen::Map(result.data()) = m; + return result; + } + + static Fixed Expmap(const TangentVector& v, ChartJacobian H = boost::none) { + Fixed m; + m.setZero(); + if (H) *H = Jacobian::Identity(); + return m + Eigen::Map(v.data()); + } + /// @} +}; + + +namespace internal { + +// traits for dynamic Eigen matrices +template +struct DynamicTraits { + + typedef vector_space_tag structure_category; + typedef Eigen::Matrix Dynamic; + + /// @name Testable + /// @{ + static void Print(const Dynamic& m, const std::string& str = "") { + gtsam::print(Eigen::MatrixXd(m), str); + } + static bool Equals(const Dynamic& v1, const Dynamic& v2, + double tol = 1e-8) { + return equal_with_abs_tol(v1, v2, tol); + } + /// @} + + /// @name Group + /// @{ + typedef additive_group_tag group_flavor; + static Dynamic Identity() { + throw std::runtime_error("Identity not defined for dynamic types"); + } + /// @} + + /// @name Manifold + /// @{ + enum { dimension = Eigen::Dynamic }; + typedef Eigen::VectorXd TangentVector; + typedef Eigen::MatrixXd Jacobian; + typedef OptionalJacobian ChartJacobian; + typedef Dynamic ManifoldType; + + static int GetDimension(const Dynamic& m) { return m.rows() * m.cols(); } - static Eigen::Matrix Eye( - const ManifoldType& m) { + static Jacobian Eye(const Dynamic& m) { int dim = GetDimension(m); return Eigen::Matrix::Identity(dim, dim); } - // For Testable - static void Print(const ManifoldType& m, const std::string& str = "") { - gtsam::print(Eigen::MatrixXd(m), str); - } - static bool Equals(const ManifoldType& v1, const ManifoldType& v2, - double tol = 1e-8) { - return equal_with_abs_tol(v1, v2, tol); + static TangentVector Local(const Dynamic& m, const Dynamic& other, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = -Eye(m); + if (H2) *H2 = Eye(m); + TangentVector v(GetDimension(m)); + Eigen::Map(v.data(), m.rows(), m.cols()) = other - m; + return v; } - static TangentVector Local(const ManifoldType& origin, - const ManifoldType& other, ChartJacobian Horigin = boost::none, - ChartJacobian Hother = boost::none) { - if (Horigin) *Horigin = -Eye(origin); - if (Hother) *Hother = Eye(origin); - TangentVector result(GetDimension(origin)); - Eigen::Map >(result.data(), origin.rows(), - origin.cols()) = other - origin; + static Dynamic Retract(const Dynamic& m, const TangentVector& v, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = Eye(m); + if (H2) *H2 = Eye(m); + return m + Eigen::Map(v.data(), m.rows(), m.cols()); + } + /// @} + + /// @name Lie Group + /// @{ + static TangentVector Logmap(const Dynamic& m, ChartJacobian H = boost::none) { + if (H) *H = Eye(m); + TangentVector result(GetDimension(m)); + Eigen::Map(result.data(), m.cols(), m.rows()) = m; return result; } - static ManifoldType Retract(const ManifoldType& origin, - const TangentVector& v, ChartJacobian Horigin = boost::none, - ChartJacobian Hv = boost::none) { - if (Horigin) *Horigin = Eye(origin); - if (Hv) *Hv = Eye(origin); - return origin - + Eigen::Map >(v.data(), - origin.rows(), origin.cols()); + static Dynamic Expmap(const TangentVector& v, ChartJacobian H = boost::none) { + throw std::runtime_error("Expmap not defined for dynamic types"); } - static ManifoldType Compose(const ManifoldType& v1, const ManifoldType& v2, + static Dynamic Inverse(const Dynamic& m, ChartJacobian H = boost::none) { + if (H) *H = -Eye(m); + return -m; + } + + static Dynamic Compose(const Dynamic& v1, const Dynamic& v2, ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { if (H1) *H1 = Eye(v1); if (H2) *H2 = Eye(v1); return v1 + v2; } - static ManifoldType Between(const ManifoldType& v1, const ManifoldType& v2, + static Dynamic Between(const Dynamic& v1, const Dynamic& v2, ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { if (H1) *H1 = -Eye(v1); if (H2) *H2 = Eye(v1); return v2 - v1; } + /// @} - static ManifoldType Inverse(const ManifoldType& m, ChartJacobian H = - boost::none) { - if (H) *H = -Eye(m); - return -m; - } +}; - static ManifoldType Identity() { - //FIXME: this won't work for dynamic matrices, but where to get the size??? - return ManifoldType::Zero(); - } +} // \ internal - static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm = - boost::none) { - if (Hm) - *Hm = Eye(m); - TangentVector result(GetDimension(m)); - Eigen::Map >(result.data()) = m; - return result; - } +// traits for fully dynamic matrix +template +struct traits_x > : + public internal::DynamicTraits<-1, -1, Options, MaxRows, MaxCols> { +}; - //FIXME: this also does not work for dynamic matrices - static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv = - boost::none) { - ManifoldType m; - m.setZero(); - if (Hv) - *Hv = Eye(m); - return m + Eigen::Map >(v.data()); - } +// traits for dynamic column vector +template +struct traits_x > : + public internal::DynamicTraits<-1, 1, Options, MaxRows, MaxCols> { +}; +// traits for dynamic row vector +template +struct traits_x > : + public internal::DynamicTraits<1, -1, Options, MaxRows, MaxCols> { }; /// Vector Space concept diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 5b36d2b02..13d32794e 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -16,12 +16,14 @@ * @author Carlos Nieto **/ -#include -#include +#include +#include +#include #include #include #include -#include +#include +#include using namespace std; using namespace gtsam; @@ -30,7 +32,7 @@ static double inf = std::numeric_limits::infinity(); static const double tol = 1e-9; /* ************************************************************************* */ -TEST( matrix, constructor_data ) +TEST(Matrix, constructor_data ) { Matrix A = (Matrix(2, 2) << -5, 3, 0, -5).finished(); @@ -44,7 +46,7 @@ TEST( matrix, constructor_data ) } /* ************************************************************************* */ -TEST( matrix, Matrix_ ) +TEST(Matrix, Matrix_ ) { Matrix A = (Matrix(2, 2) << -5.0, 3.0, 0.0, -5.0).finished(); Matrix B(2, 2); @@ -74,7 +76,7 @@ namespace { } /* ************************************************************************* */ -TEST( matrix, special_comma_initializer) +TEST(Matrix, special_comma_initializer) { Matrix expected(2,2); expected(0,0) = 1; @@ -103,7 +105,7 @@ TEST( matrix, special_comma_initializer) } /* ************************************************************************* */ -TEST( matrix, col_major ) +TEST(Matrix, col_major ) { Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); const double * const a = &A(0, 0); @@ -114,7 +116,7 @@ TEST( matrix, col_major ) } /* ************************************************************************* */ -TEST( matrix, collect1 ) +TEST(Matrix, collect1 ) { Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished(); Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished(); @@ -131,7 +133,7 @@ TEST( matrix, collect1 ) } /* ************************************************************************* */ -TEST( matrix, collect2 ) +TEST(Matrix, collect2 ) { Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished(); Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished(); @@ -151,7 +153,7 @@ TEST( matrix, collect2 ) } /* ************************************************************************* */ -TEST( matrix, collect3 ) +TEST(Matrix, collect3 ) { Matrix A, B; A = eye(2, 3); @@ -168,7 +170,7 @@ TEST( matrix, collect3 ) } /* ************************************************************************* */ -TEST( matrix, stack ) +TEST(Matrix, stack ) { Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished(); Matrix B = (Matrix(3, 2) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished(); @@ -191,7 +193,7 @@ TEST( matrix, stack ) } /* ************************************************************************* */ -TEST( matrix, column ) +TEST(Matrix, column ) { Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., @@ -210,7 +212,7 @@ TEST( matrix, column ) } /* ************************************************************************* */ -TEST( matrix, insert_column ) +TEST(Matrix, insert_column ) { Matrix big = zeros(5, 6); Vector col = ones(5); @@ -229,7 +231,7 @@ TEST( matrix, insert_column ) } /* ************************************************************************* */ -TEST( matrix, insert_subcolumn ) +TEST(Matrix, insert_subcolumn ) { Matrix big = zeros(5, 6); Vector col1 = ones(2); @@ -252,7 +254,7 @@ TEST( matrix, insert_subcolumn ) } /* ************************************************************************* */ -TEST( matrix, row ) +TEST(Matrix, row ) { Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., @@ -271,7 +273,7 @@ TEST( matrix, row ) } /* ************************************************************************* */ -TEST( matrix, zeros ) +TEST(Matrix, zeros ) { Matrix A(2, 3); A(0, 0) = 0; @@ -287,7 +289,7 @@ TEST( matrix, zeros ) } /* ************************************************************************* */ -TEST( matrix, insert_sub ) +TEST(Matrix, insert_sub ) { Matrix big = zeros(5, 6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0).finished(); @@ -302,7 +304,7 @@ TEST( matrix, insert_sub ) } /* ************************************************************************* */ -TEST( matrix, diagMatrices ) +TEST(Matrix, diagMatrices ) { std::vector Hs; Hs.push_back(ones(3,3)); @@ -326,7 +328,7 @@ TEST( matrix, diagMatrices ) } /* ************************************************************************* */ -TEST( matrix, stream_read ) { +TEST(Matrix, stream_read ) { Matrix expected = (Matrix(3,4) << 1.1, 2.3, 4.2, 7.6, -0.3, -8e-2, 5.1, 9.0, @@ -346,7 +348,7 @@ TEST( matrix, stream_read ) { } /* ************************************************************************* */ -TEST( matrix, scale_columns ) +TEST(Matrix, scale_columns ) { Matrix A(3, 4); A(0, 0) = 1.; @@ -384,7 +386,7 @@ TEST( matrix, scale_columns ) } /* ************************************************************************* */ -TEST( matrix, scale_rows ) +TEST(Matrix, scale_rows ) { Matrix A(3, 4); A(0, 0) = 1.; @@ -422,7 +424,7 @@ TEST( matrix, scale_rows ) } /* ************************************************************************* */ -TEST( matrix, scale_rows_mask ) +TEST(Matrix, scale_rows_mask ) { Matrix A(3, 4); A(0, 0) = 1.; @@ -460,7 +462,7 @@ TEST( matrix, scale_rows_mask ) } /* ************************************************************************* */ -TEST( matrix, skewSymmetric ) +TEST(Matrix, skewSymmetric ) { double wx = 1, wy = 2, wz = 3; Matrix3 actual = skewSymmetric(wx,wy,wz); @@ -476,7 +478,7 @@ TEST( matrix, skewSymmetric ) /* ************************************************************************* */ -TEST( matrix, equal ) +TEST(Matrix, equal ) { Matrix A(4, 4); A(0, 0) = -1; @@ -506,7 +508,7 @@ TEST( matrix, equal ) } /* ************************************************************************* */ -TEST( matrix, equal_nan ) +TEST(Matrix, equal_nan ) { Matrix A(4, 4); A(0, 0) = -1; @@ -535,7 +537,7 @@ TEST( matrix, equal_nan ) } /* ************************************************************************* */ -TEST( matrix, addition ) +TEST(Matrix, addition ) { Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); @@ -544,7 +546,7 @@ TEST( matrix, addition ) } /* ************************************************************************* */ -TEST( matrix, addition_in_place ) +TEST(Matrix, addition_in_place ) { Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); @@ -554,7 +556,7 @@ TEST( matrix, addition_in_place ) } /* ************************************************************************* */ -TEST( matrix, subtraction ) +TEST(Matrix, subtraction ) { Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); @@ -563,7 +565,7 @@ TEST( matrix, subtraction ) } /* ************************************************************************* */ -TEST( matrix, subtraction_in_place ) +TEST(Matrix, subtraction_in_place ) { Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); @@ -573,7 +575,7 @@ TEST( matrix, subtraction_in_place ) } /* ************************************************************************* */ -TEST( matrix, multiplication ) +TEST(Matrix, multiplication ) { Matrix A(2, 2); A(0, 0) = -1; @@ -593,7 +595,7 @@ TEST( matrix, multiplication ) } /* ************************************************************************* */ -TEST( matrix, scalar_matrix_multiplication ) +TEST(Matrix, scalar_matrix_multiplication ) { Vector result(2); @@ -613,7 +615,7 @@ TEST( matrix, scalar_matrix_multiplication ) } /* ************************************************************************* */ -TEST( matrix, matrix_vector_multiplication ) +TEST(Matrix, matrix_vector_multiplication ) { Vector result(2); @@ -627,7 +629,7 @@ TEST( matrix, matrix_vector_multiplication ) } /* ************************************************************************* */ -TEST( matrix, nrRowsAndnrCols ) +TEST(Matrix, nrRowsAndnrCols ) { Matrix A(3, 6); LONGS_EQUAL( A.rows() , 3 ); @@ -635,7 +637,7 @@ TEST( matrix, nrRowsAndnrCols ) } /* ************************************************************************* */ -TEST( matrix, scalar_divide ) +TEST(Matrix, scalar_divide ) { Matrix A(2, 2); A(0, 0) = 10; @@ -653,7 +655,7 @@ TEST( matrix, scalar_divide ) } /* ************************************************************************* */ -TEST( matrix, zero_below_diagonal ) { +TEST(Matrix, zero_below_diagonal ) { Matrix A1 = (Matrix(3, 4) << 1.0, 2.0, 3.0, 4.0, 1.0, 2.0, 3.0, 4.0, @@ -708,7 +710,7 @@ TEST( matrix, zero_below_diagonal ) { } /* ************************************************************************* */ -TEST( matrix, inverse ) +TEST(Matrix, inverse ) { Matrix A(3, 3); A(0, 0) = 1; @@ -754,7 +756,7 @@ TEST( matrix, inverse ) } /* ************************************************************************* */ -TEST( matrix, inverse2 ) +TEST(Matrix, inverse2 ) { Matrix A(3, 3); A(0, 0) = 0; @@ -784,7 +786,7 @@ TEST( matrix, inverse2 ) } /* ************************************************************************* */ -TEST( matrix, backsubtitution ) +TEST(Matrix, backsubtitution ) { // TEST ONE 2x2 matrix U1*x=b1 Vector expected1 = Vector2(3.6250, -0.75); @@ -809,7 +811,7 @@ TEST( matrix, backsubtitution ) } /* ************************************************************************* */ -TEST( matrix, householder ) +TEST(Matrix, householder ) { // check in-place householder, with v vectors below diagonal @@ -838,7 +840,7 @@ TEST( matrix, householder ) } /* ************************************************************************* */ -TEST( matrix, householder_colMajor ) +TEST(Matrix, householder_colMajor ) { // check in-place householder, with v vectors below diagonal @@ -867,7 +869,7 @@ TEST( matrix, householder_colMajor ) } /* ************************************************************************* */ -TEST( matrix, eigen_QR ) +TEST(Matrix, eigen_QR ) { // use standard Eigen function to yield a non-in-place QR factorization @@ -898,7 +900,7 @@ TEST( matrix, eigen_QR ) // unit test for qr factorization (and hence householder) // This behaves the same as QR in matlab: [Q,R] = qr(A), except for signs /* ************************************************************************* */ -TEST( matrix, qr ) +TEST(Matrix, qr ) { Matrix A = (Matrix(6, 4) << -5, 0, 5, 0, 00, -5, 0, 5, 10, 0, 0, 0, 00, 10, 0, 0, 00, @@ -921,7 +923,7 @@ TEST( matrix, qr ) } /* ************************************************************************* */ -TEST( matrix, sub ) +TEST(Matrix, sub ) { Matrix A = (Matrix(4, 6) << -5, 0, 5, 0, 0, 0, 00, -5, 0, 5, 0, 0, 10, 0, 0, 0, -10, 0, 00, 10, 0, 0, 0, -10).finished(); @@ -933,7 +935,7 @@ TEST( matrix, sub ) } /* ************************************************************************* */ -TEST( matrix, trans ) +TEST(Matrix, trans ) { Matrix A = (Matrix(2, 2) << 1.0, 3.0, 2.0, 4.0).finished(); Matrix B = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); @@ -941,7 +943,7 @@ TEST( matrix, trans ) } /* ************************************************************************* */ -TEST( matrix, col_major_access ) +TEST(Matrix, col_major_access ) { Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); const double* a = &A(0, 0); @@ -949,7 +951,7 @@ TEST( matrix, col_major_access ) } /* ************************************************************************* */ -TEST( matrix, weighted_elimination ) +TEST(Matrix, weighted_elimination ) { // create a matrix to eliminate Matrix A = (Matrix(4, 6) << -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0., @@ -984,7 +986,7 @@ TEST( matrix, weighted_elimination ) } /* ************************************************************************* */ -TEST( matrix, inverse_square_root ) +TEST(Matrix, inverse_square_root ) { Matrix measurement_covariance = (Matrix(3, 3) << 0.25, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.01).finished(); @@ -1036,22 +1038,22 @@ Matrix expected = (Matrix(5, 5) << 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247).finished(); } -TEST( matrix, LLt ) +TEST(Matrix, LLt ) { EQUALITY(cholesky::expected, LLt(cholesky::M)); } -TEST( matrix, RtR ) +TEST(Matrix, RtR ) { EQUALITY(cholesky::expected.transpose(), RtR(cholesky::M)); } -TEST( matrix, cholesky_inverse ) +TEST(Matrix, cholesky_inverse ) { EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M)); } /* ************************************************************************* */ -TEST( matrix, multiplyAdd ) +TEST(Matrix, multiplyAdd ) { Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), @@ -1062,7 +1064,7 @@ TEST( matrix, multiplyAdd ) } /* ************************************************************************* */ -TEST( matrix, transposeMultiplyAdd ) +TEST(Matrix, transposeMultiplyAdd ) { Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), @@ -1073,7 +1075,7 @@ TEST( matrix, transposeMultiplyAdd ) } /* ************************************************************************* */ -TEST( matrix, linear_dependent ) +TEST(Matrix, linear_dependent ) { Matrix A = (Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished(); Matrix B = (Matrix(2, 3) << -1.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished(); @@ -1081,7 +1083,7 @@ TEST( matrix, linear_dependent ) } /* ************************************************************************* */ -TEST( matrix, linear_dependent2 ) +TEST(Matrix, linear_dependent2 ) { Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished(); Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished(); @@ -1089,7 +1091,7 @@ TEST( matrix, linear_dependent2 ) } /* ************************************************************************* */ -TEST( matrix, linear_dependent3 ) +TEST(Matrix, linear_dependent3 ) { Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished(); Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.1, 10.0, 12.0).finished(); @@ -1097,7 +1099,7 @@ TEST( matrix, linear_dependent3 ) } /* ************************************************************************* */ -TEST( matrix, svd1 ) +TEST(Matrix, svd1 ) { Vector v = Vector3(2., 1., 0.); Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1) @@ -1116,7 +1118,7 @@ static Matrix sampleA = (Matrix(3, 2) << 0.,-2., 0., 0., 3., 0.).finished(); static Matrix sampleAt = trans(sampleA); /* ************************************************************************* */ -TEST( matrix, svd2 ) +TEST(Matrix, svd2 ) { Matrix U, V; Vector s; @@ -1139,7 +1141,7 @@ TEST( matrix, svd2 ) } /* ************************************************************************* */ -TEST( matrix, svd3 ) +TEST(Matrix, svd3 ) { Matrix U, V; Vector s; @@ -1167,7 +1169,7 @@ TEST( matrix, svd3 ) } /* ************************************************************************* */ -TEST( matrix, svd4 ) +TEST(Matrix, svd4 ) { Matrix U, V; Vector s; @@ -1209,7 +1211,7 @@ TEST( matrix, svd4 ) } /* ************************************************************************* */ -TEST( matrix, DLT ) +TEST(Matrix, DLT ) { Matrix A = (Matrix(8, 9) << 0.21, -0.42, -10.71, 0.18, -0.36, -9.18, -0.61, 1.22, 31.11, @@ -1231,6 +1233,18 @@ TEST( matrix, DLT ) EXPECT(assert_equal(expected, actual, 1e-4)); } +//****************************************************************************** +TEST(Matrix , IsVectorSpace) { + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + typedef Eigen::Matrix RowMajor; + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + typedef Eigen::Matrix RowVector; + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 460ff6cd6..00e40039b 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -15,10 +15,12 @@ * @author Frank Dellaert **/ -#include +#include +#include +#include #include #include -#include +#include using namespace std; using namespace gtsam; @@ -40,7 +42,7 @@ namespace { } /* ************************************************************************* */ -TEST( TestVector, special_comma_initializer) +TEST(Vector, special_comma_initializer) { Vector expected(3); expected(0) = 1; @@ -68,7 +70,7 @@ TEST( TestVector, special_comma_initializer) } /* ************************************************************************* */ -TEST( TestVector, copy ) +TEST(Vector, copy ) { Vector a(2); a(0) = 10; a(1) = 20; double data[] = {10,20}; @@ -78,14 +80,14 @@ TEST( TestVector, copy ) } /* ************************************************************************* */ -TEST( TestVector, zero1 ) +TEST(Vector, zero1 ) { Vector v = Vector::Zero(2); EXPECT(zero(v)); } /* ************************************************************************* */ -TEST( TestVector, zero2 ) +TEST(Vector, zero2 ) { Vector a = zero(2); Vector b = Vector::Zero(2); @@ -94,7 +96,7 @@ TEST( TestVector, zero2 ) } /* ************************************************************************* */ -TEST( TestVector, scalar_multiply ) +TEST(Vector, scalar_multiply ) { Vector a(2); a(0) = 10; a(1) = 20; Vector b(2); b(0) = 1; b(1) = 2; @@ -102,7 +104,7 @@ TEST( TestVector, scalar_multiply ) } /* ************************************************************************* */ -TEST( TestVector, scalar_divide ) +TEST(Vector, scalar_divide ) { Vector a(2); a(0) = 10; a(1) = 20; Vector b(2); b(0) = 1; b(1) = 2; @@ -110,7 +112,7 @@ TEST( TestVector, scalar_divide ) } /* ************************************************************************* */ -TEST( TestVector, negate ) +TEST(Vector, negate ) { Vector a(2); a(0) = 10; a(1) = 20; Vector b(2); b(0) = -10; b(1) = -20; @@ -118,7 +120,7 @@ TEST( TestVector, negate ) } /* ************************************************************************* */ -TEST( TestVector, sub ) +TEST(Vector, sub ) { Vector a(6); a(0) = 10; a(1) = 20; a(2) = 3; @@ -134,7 +136,7 @@ TEST( TestVector, sub ) } /* ************************************************************************* */ -TEST( TestVector, subInsert ) +TEST(Vector, subInsert ) { Vector big = zero(6), small = ones(3); @@ -148,7 +150,7 @@ TEST( TestVector, subInsert ) } /* ************************************************************************* */ -TEST( TestVector, householder ) +TEST(Vector, householder ) { Vector x(4); x(0) = 3; x(1) = 1; x(2) = 5; x(3) = 1; @@ -163,7 +165,7 @@ TEST( TestVector, householder ) } /* ************************************************************************* */ -TEST( TestVector, concatVectors) +TEST(Vector, concatVectors) { Vector A(2); for(int i = 0; i < 2; i++) @@ -187,7 +189,7 @@ TEST( TestVector, concatVectors) } /* ************************************************************************* */ -TEST( TestVector, weightedPseudoinverse ) +TEST(Vector, weightedPseudoinverse ) { // column from a matrix Vector x(2); @@ -213,7 +215,7 @@ TEST( TestVector, weightedPseudoinverse ) } /* ************************************************************************* */ -TEST( TestVector, weightedPseudoinverse_constraint ) +TEST(Vector, weightedPseudoinverse_constraint ) { // column from a matrix Vector x(2); @@ -238,7 +240,7 @@ TEST( TestVector, weightedPseudoinverse_constraint ) } /* ************************************************************************* */ -TEST( TestVector, weightedPseudoinverse_nan ) +TEST(Vector, weightedPseudoinverse_nan ) { Vector a = (Vector(4) << 1., 0., 0., 0.).finished(); Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished(); @@ -252,7 +254,7 @@ TEST( TestVector, weightedPseudoinverse_nan ) } /* ************************************************************************* */ -TEST( TestVector, ediv ) +TEST(Vector, ediv ) { Vector a = Vector3(10., 20., 30.); Vector b = Vector3(2.0, 5.0, 6.0); @@ -263,7 +265,7 @@ TEST( TestVector, ediv ) } /* ************************************************************************* */ -TEST( TestVector, dot ) +TEST(Vector, dot ) { Vector a = Vector3(10., 20., 30.); Vector b = Vector3(2.0, 5.0, 6.0); @@ -271,7 +273,7 @@ TEST( TestVector, dot ) } /* ************************************************************************* */ -TEST( TestVector, axpy ) +TEST(Vector, axpy ) { Vector x = Vector3(10., 20., 30.); Vector y0 = Vector3(2.0, 5.0, 6.0); @@ -284,7 +286,7 @@ TEST( TestVector, axpy ) } /* ************************************************************************* */ -TEST( TestVector, equals ) +TEST(Vector, equals ) { Vector v1 = (Vector(1) << 0.0/std::numeric_limits::quiet_NaN()).finished(); //testing nan Vector v2 = (Vector(1) << 1.0).finished(); @@ -293,7 +295,7 @@ TEST( TestVector, equals ) } /* ************************************************************************* */ -TEST( TestVector, greater_than ) +TEST(Vector, greater_than ) { Vector v1 = Vector3(1.0, 2.0, 3.0), v2 = zero(3); @@ -302,14 +304,14 @@ TEST( TestVector, greater_than ) } /* ************************************************************************* */ -TEST( TestVector, reciprocal ) +TEST(Vector, reciprocal ) { Vector v = Vector3(1.0, 2.0, 4.0); EXPECT(assert_equal(Vector3(1.0, 0.5, 0.25),reciprocal(v))); } /* ************************************************************************* */ -TEST( TestVector, linear_dependent ) +TEST(Vector, linear_dependent ) { Vector v1 = Vector3(1.0, 2.0, 3.0); Vector v2 = Vector3(-2.0, -4.0, -6.0); @@ -317,7 +319,7 @@ TEST( TestVector, linear_dependent ) } /* ************************************************************************* */ -TEST( TestVector, linear_dependent2 ) +TEST(Vector, linear_dependent2 ) { Vector v1 = Vector3(0.0, 2.0, 0.0); Vector v2 = Vector3(0.0, -4.0, 0.0); @@ -325,13 +327,21 @@ TEST( TestVector, linear_dependent2 ) } /* ************************************************************************* */ -TEST( TestVector, linear_dependent3 ) +TEST(Vector, linear_dependent3 ) { Vector v1 = Vector3(0.0, 2.0, 0.0); Vector v2 = Vector3(0.1, -4.1, 0.0); EXPECT(!linear_dependent(v1, v2)); } +//****************************************************************************** +TEST(Vector, IsVectorSpace) { + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + typedef Eigen::Matrix RowVector; + BOOST_CONCEPT_ASSERT((IsVectorSpace)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index 70845b24f..6d27a250d 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -22,8 +22,8 @@ #include #include #include -#include -#include +#include +#include #include #undef CHECK @@ -39,34 +39,49 @@ using namespace gtsam; // The DefaultChart of Camera below is laid out like Snavely's 9-dim vector typedef PinholeCamera Camera; -/* ************************************************************************* */ -// is_manifold -TEST(Manifold, IsManifold) { - +//****************************************************************************** +TEST(Manifold, SomeManifoldsGTSAM) { //BOOST_CONCEPT_ASSERT((IsManifold)); // integer is not a manifold - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsManifold)); BOOST_CONCEPT_ASSERT((IsManifold)); - - // dynamic not working yet - //BOOST_CONCEPT_ASSERT((IsManifold)); - //BOOST_CONCEPT_ASSERT((IsManifold)); - + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsManifold)); } -/* ************************************************************************* */ +//****************************************************************************** +TEST(Manifold, SomeLieGroupsGTSAM) { + BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +TEST(Manifold, SomeVectorSpacesGTSAM) { + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); +} + +//****************************************************************************** // dimension TEST(Manifold, _dimension) { //using namespace traits; EXPECT_LONGS_EQUAL(2, traits_x::dimension); EXPECT_LONGS_EQUAL(8, traits_x::dimension); EXPECT_LONGS_EQUAL(1, traits_x::dimension); -// EXPECT_LONGS_EQUAL(Eigen::Dynamic, traits_x::dimension); -// EXPECT_LONGS_EQUAL(Eigen::Dynamic, traits_x::dimension); } -/* ************************************************************************* */ +//****************************************************************************** +TEST(Manifold, Identity) { + EXPECT_DOUBLES_EQUAL(0.0, traits_x::Identity(), 0.0); + EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits_x::Identity()))); + EXPECT(assert_equal(Pose3(), traits_x::Identity())); + EXPECT(assert_equal(Point2(), traits_x::Identity())); +} + +//****************************************************************************** // charts TEST(Manifold, DefaultChart) { @@ -134,35 +149,10 @@ TEST(Manifold, DefaultChart) { EXPECT(assert_equal(zero(3), traits_x::Local(R, R))); } -/* ************************************************************************* */ -// zero -//TEST(Manifold, _zero) { -// EXPECT(assert_equal(Pose3(), traits::zero::value())); -// Cal3Bundler cal(0, 0, 0); -// EXPECT(assert_equal(cal, traits::zero::value())); -// EXPECT(assert_equal(Camera(Pose3(), cal), traits::zero::value())); -// EXPECT(assert_equal(Point2(), traits::zero::value())); -// EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::zero::value()))); -// EXPECT_DOUBLES_EQUAL(0.0, traits::zero::value(), 0.0); -//} -// -/* ************************************************************************* */ -// identity -TEST(Manifold, _identity) { - EXPECT(assert_equal(Pose3(), traits_x::Identity())); -// BOOST_CONCEPT_ASSERT((IsLieGroup)); -// EXPECT(assert_equal(Cal3Bundler(), traits_x::Identity())); -// BOOST_CONCEPT_ASSERT((IsLieGroup)); -// EXPECT(assert_equal(Camera(), traits_x::Identity())); // not a lie group - EXPECT(assert_equal(Point2(), traits_x::Identity())); - EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits_x::Identity()))); - EXPECT_DOUBLES_EQUAL(0.0, traits_x::Identity(), 0.0); -} - -/* ************************************************************************* */ +//****************************************************************************** int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ +//******************************************************************************