From 9f765c749652183d33b8ebf3210702bdce17b4bd Mon Sep 17 00:00:00 2001 From: HannesSommer Date: Sun, 9 Nov 2014 10:27:23 +0100 Subject: [PATCH 1/4] micro cleanup --- tests/testManifold.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index b7ebfc4c8..c9451c6de 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -109,7 +109,7 @@ TEST(Manifold, _zero) { 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().eval()), Matrix(traits::zero::value()))); + EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::zero::value()))); EXPECT_DOUBLES_EQUAL(0.0, traits::zero::value(), 0.0); } From 6d04309dfb2e71174ff266b9c19d2ff9c23d2dc0 Mon Sep 17 00:00:00 2001 From: HannesSommer Date: Sun, 9 Nov 2014 17:41:19 +0100 Subject: [PATCH 2/4] * cleaned up and optimized a bit the Eigen matrices' DefaultChart * also added a few unit tests more for those chars --- gtsam/base/Manifold.h | 47 ++++++++++++++++++++++++++++++++++++------ tests/testManifold.cpp | 20 ++++++++++++++++++ 2 files changed, 61 insertions(+), 6 deletions(-) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 9dadaf9e6..fa509252d 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -242,21 +242,56 @@ struct DefaultChart { // Fixed size Eigen::Matrix type +namespace internal { + +template +struct Reshape { + //TODO replace this with Eigen's reshape function as soon as available. (There is a PR already pending : https://bitbucket.org/eigen/eigen/pull-request/41/reshape/diff) + typedef Eigen::Map > ReshapedType; + static inline ReshapedType reshape(const Eigen::Matrix & in) { + return in.data(); + } +}; + +template +struct Reshape { + typedef const Eigen::Matrix & ReshapedType; + static inline ReshapedType reshape(const Eigen::Matrix & in) { + return in; + } +}; + +template +struct Reshape { + typedef typename Eigen::Matrix::ConstTransposeReturnType ReshapedType; + static inline ReshapedType reshape(const Eigen::Matrix & in) { + return in.transpose(); + } +}; + +template +inline typename Reshape::ReshapedType reshape(const Eigen::Matrix & m){ + BOOST_STATIC_ASSERT(InM * InN == OutM * OutN); + return Reshape::reshape(m); +} + +} + template struct DefaultChart > { + /** + * This chart for the vector space of M x N matrices (represented by Eigen matrices) chooses as basis the one with respect to which the coordinates are exactly the matrix entries as laid out in memory (as determined by Options). + * Computing coordinates for a matrix is then simply a reshape to the row vector of appropriate size. + */ typedef Eigen::Matrix type; typedef type T; typedef Eigen::Matrix::value, 1> vector;BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), "DefaultChart has not been implemented yet for dynamically sized matrices"); static vector local(const T& origin, const T& other) { - T diff = other - origin; - Eigen::Map map(diff.data()); - return vector(map); - // Why is this function not : return other - origin; ?? what is the Eigen::Map used for? + return internal::reshape(other) - internal::reshape(origin); } static T retract(const T& origin, const vector& d) { - Eigen::Map map(d.data()); - return origin + map; + return origin + internal::reshape(d); } static int getDimension(const T&origin) { return origin.rows() * origin.cols(); diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index c9451c6de..6e720757a 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -76,6 +76,26 @@ TEST(Manifold, DefaultChart) { EXPECT(assert_equal(v2, chart2.local(Vector2(0, 0), Vector2(1, 0)))); EXPECT(chart2.retract(Vector2(0, 0), v2) == Vector2(1, 0)); + { + typedef Matrix2 ManifoldPoint; + ManifoldPoint m; + DefaultChart chart; + m << 1, 3, + 2, 4; + // m as per default is in column-major storage mode. So this yields a linear representation of (1, 2, 3, 4)! + EXPECT(assert_equal(Vector(Vector4(1, 2, 3, 4)), Vector(chart.local(ManifoldPoint::Zero(), m)))); + EXPECT(chart.retract(m, Vector4(1, 2, 3, 4)) == 2 * m); + } + + { + typedef Eigen::Matrix ManifoldPoint; + ManifoldPoint m; + DefaultChart chart; + m << 1, 2; + EXPECT(assert_equal(Vector(Vector2(1, 2)), Vector(chart.local(ManifoldPoint::Zero(), m)))); + EXPECT(chart.retract(m, Vector2(1, 2)) == 2 * m); + } + DefaultChart chart3; Vector v1(1); v1 << 1; From 00765d9bf3242bae64f768b302803b98f84ad7e8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 10 Nov 2014 15:30:53 +0100 Subject: [PATCH 3/4] Moved Reshape to matrix --- gtsam/base/Manifold.h | 39 ++------------------------------------- gtsam/base/Matrix.h | 34 ++++++++++++++++++++++++++++++++++ 2 files changed, 36 insertions(+), 37 deletions(-) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index fa509252d..fb926c630 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -242,41 +242,6 @@ struct DefaultChart { // Fixed size Eigen::Matrix type -namespace internal { - -template -struct Reshape { - //TODO replace this with Eigen's reshape function as soon as available. (There is a PR already pending : https://bitbucket.org/eigen/eigen/pull-request/41/reshape/diff) - typedef Eigen::Map > ReshapedType; - static inline ReshapedType reshape(const Eigen::Matrix & in) { - return in.data(); - } -}; - -template -struct Reshape { - typedef const Eigen::Matrix & ReshapedType; - static inline ReshapedType reshape(const Eigen::Matrix & in) { - return in; - } -}; - -template -struct Reshape { - typedef typename Eigen::Matrix::ConstTransposeReturnType ReshapedType; - static inline ReshapedType reshape(const Eigen::Matrix & in) { - return in.transpose(); - } -}; - -template -inline typename Reshape::ReshapedType reshape(const Eigen::Matrix & m){ - BOOST_STATIC_ASSERT(InM * InN == OutM * OutN); - return Reshape::reshape(m); -} - -} - template struct DefaultChart > { /** @@ -288,10 +253,10 @@ struct DefaultChart > { typedef Eigen::Matrix::value, 1> vector;BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), "DefaultChart has not been implemented yet for dynamically sized matrices"); static vector local(const T& origin, const T& other) { - return internal::reshape(other) - internal::reshape(origin); + return reshape(other) - reshape(origin); } static T retract(const T& origin, const vector& d) { - return origin + internal::reshape(d); + return origin + reshape(d); } static int getDimension(const T&origin) { return origin.rows() * origin.cols(); diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 1094306a9..eaa57c810 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -293,6 +293,40 @@ void zeroBelowDiagonal(MATRIX& A, size_t cols=0) { */ inline Matrix trans(const Matrix& A) { return A.transpose(); } +/// Reshape functor +template +struct Reshape { + //TODO replace this with Eigen's reshape function as soon as available. (There is a PR already pending : https://bitbucket.org/eigen/eigen/pull-request/41/reshape/diff) + typedef Eigen::Map > ReshapedType; + static inline ReshapedType reshape(const Eigen::Matrix & in) { + return in.data(); + } +}; + +/// Reshape specialization that does nothing as shape stays the same +template +struct Reshape { + typedef const Eigen::Matrix & ReshapedType; + static inline ReshapedType reshape(const Eigen::Matrix & in) { + return in; + } +}; + +/// Reshape specialization that does transpose +template +struct Reshape { + typedef typename Eigen::Matrix::ConstTransposeReturnType ReshapedType; + static inline ReshapedType reshape(const Eigen::Matrix & in) { + return in.transpose(); + } +}; + +template +inline typename Reshape::ReshapedType reshape(const Eigen::Matrix & m){ + BOOST_STATIC_ASSERT(InM * InN == OutM * OutN); + return Reshape::reshape(m); +} + /** * solve AX=B via in-place Lu factorization and backsubstitution * After calling, A contains LU, B the solved RHS vectors From fed2c8b6849205598ea97c4826260ac85ba8eabb Mon Sep 17 00:00:00 2001 From: HannesSommer Date: Mon, 10 Nov 2014 16:35:23 +0100 Subject: [PATCH 4/4] added missing square matrix specialization - without it, square to square cases would be ambiguous. --- gtsam/base/Matrix.h | 9 +++++++++ tests/testManifold.cpp | 9 +++++++++ 2 files changed, 18 insertions(+) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index eaa57c810..00caed44a 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -303,6 +303,15 @@ struct Reshape { } }; +/// Reshape specialization that does nothing as shape stays the same (needed to not be ambiguous for square input equals square output) +template +struct Reshape { + typedef const Eigen::Matrix & ReshapedType; + static inline ReshapedType reshape(const Eigen::Matrix & in) { + return in; + } +}; + /// Reshape specialization that does nothing as shape stays the same template struct Reshape { diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index 6e720757a..32f04225f 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -96,6 +96,15 @@ TEST(Manifold, DefaultChart) { EXPECT(chart.retract(m, Vector2(1, 2)) == 2 * m); } + { + typedef Eigen::Matrix ManifoldPoint; + ManifoldPoint m; + DefaultChart chart; + m << 1; + EXPECT(assert_equal(Vector(ManifoldPoint::Ones()), Vector(chart.local(ManifoldPoint::Zero(), m)))); + EXPECT(chart.retract(m, ManifoldPoint::Ones()) == 2 * m); + } + DefaultChart chart3; Vector v1(1); v1 << 1;