From a0fc13fce8eda2ac5800af364ba13ce253c1e9c9 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 21 Jun 2012 01:20:25 +0000 Subject: [PATCH] Added LieMatrix to allow optimizing for matrices (treated as manifold objects) in gtsam --- gtsam/base/LieMatrix.cpp | 36 ++++++++ gtsam/base/LieMatrix.h | 139 +++++++++++++++++++++++++++++ gtsam/base/tests/testLieMatrix.cpp | 71 +++++++++++++++ 3 files changed, 246 insertions(+) create mode 100644 gtsam/base/LieMatrix.cpp create mode 100644 gtsam/base/LieMatrix.h create mode 100644 gtsam/base/tests/testLieMatrix.cpp diff --git a/gtsam/base/LieMatrix.cpp b/gtsam/base/LieMatrix.cpp new file mode 100644 index 000000000..332401689 --- /dev/null +++ b/gtsam/base/LieMatrix.cpp @@ -0,0 +1,36 @@ +/* ---------------------------------------------------------------------------- + + * 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 LieMatrix.cpp + * @brief A wrapper around Matrix providing Lie compatibility + * @author Richard Roberts and Alex Cunningham + */ + +#include + +namespace gtsam { + + /* ************************************************************************* */ + LieMatrix::LieMatrix(size_t m, size_t n, ...) + : Matrix(m,n) { + va_list ap; + va_start(ap, n); + for(size_t i = 0; i < m; ++i) { + for(size_t j = 0; j < n; ++j) { + double value = va_arg(ap, double); + (*this)(i,j) = value; + } + } + va_end(ap); + } + +} \ No newline at end of file diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h new file mode 100644 index 000000000..2ab15a054 --- /dev/null +++ b/gtsam/base/LieMatrix.h @@ -0,0 +1,139 @@ +/* ---------------------------------------------------------------------------- + + * 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 LieMatrix.h + * @brief A wrapper around Matrix providing Lie compatibility + * @author Richard Roberts and Alex Cunningham + */ + +#pragma once + +#include + +#include +#include +#include + +namespace gtsam { + +/** + * LieVector is a wrapper around vector to allow it to be a Lie type + */ +struct LieMatrix : public Matrix, public DerivedValue { + + /** default constructor - should be unnecessary */ + LieMatrix() {} + + /** initialize from a normal matrix */ + LieMatrix(const Matrix& v) : Matrix(v) {} + + /** initialize from a fixed size normal vector */ + template + LieMatrix(const Eigen::Matrix& v) : Matrix(v) {} + + /** constructor with size and initial data, row order ! */ + LieMatrix(size_t m, size_t n, const double* const data) : + Matrix(Eigen::Map(data, m, n)) {} + + /** Specify arguments directly, as in Matrix_() - always force these to be doubles */ + LieMatrix(size_t m, size_t n, ...); + + /** get the underlying vector */ + inline Matrix matrix() const { + return static_cast(*this); + } + + /** print @param s optional string naming the object */ + inline void print(const std::string& name="") const { + gtsam::print(matrix(), name); + } + + /** equality up to tolerance */ + inline bool equals(const LieMatrix& expected, double tol=1e-5) const { + return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol); + } + + // Manifold requirements + + /** Returns dimensionality of the tangent space */ + inline size_t dim() const { return this->size(); } + + /** Update the LieMatrix with a tangent space update. The elements of the + * tangent space vector correspond to the matrix entries arranged in + * *row-major* order. */ + inline LieMatrix retract(const Vector& v) const { + if(v.size() != this->rows() * this->cols()) + throw std::invalid_argument("LieMatrix::retract called with Vector of incorrect size"); + return LieMatrix(*this + Eigen::Map(&v(0), this->cols(), this->rows()).transpose()); + } + + /** @return the local coordinates of another object. The elements of the + * tangent space vector correspond to the matrix entries arranged in + * *row-major* order. */ + inline Vector localCoordinates(const LieMatrix& t2) const { + Vector result(this->rows() * this->cols()); + Eigen::Map(&result(0), this->cols(), this->rows()).transpose() = + (t2 - *this); + return result; + } + + // Group requirements + + /** identity - NOTE: no known size at compile time - so zero length */ + inline static LieMatrix identity() { + throw std::runtime_error("LieMatrix::identity(): Don't use this function"); + return LieMatrix(); + } + + // Note: Manually specifying the 'gtsam' namespace for the optional Matrix arguments + // This is a work-around for linux g++ 4.6.1 that incorrectly selects the Eigen::Matrix class + // instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector + // as the other geometry objects (Point3, Rot3, etc.) have this problem + /** compose with another object */ + inline LieMatrix compose(const LieMatrix& p, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = eye(dim()); + if(H2) *H2 = eye(p.dim()); + + return LieMatrix(*this + p); + } + + /** between operation */ + inline LieMatrix between(const LieMatrix& l2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -eye(dim()); + if(H2) *H2 = eye(l2.dim()); + return LieMatrix(l2 - *this); + } + + /** invert the object and yield a new one */ + inline LieMatrix inverse(boost::optional H=boost::none) const { + if(H) *H = -eye(dim()); + + return LieMatrix(-(*this)); + } + + // Lie functions + + /** Expmap around identity */ + static inline LieMatrix Expmap(const Vector& v) { + throw std::runtime_error("LieMatrix::Expmap(): Don't use this function"); + return LieMatrix(v); } + + /** Logmap around identity - just returns with default cast back */ + static inline Vector Logmap(const LieMatrix& p) { + return Eigen::Map(&p(0,0), p.dim()); } + +}; +} // \namespace gtsam diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp new file mode 100644 index 000000000..e4070d338 --- /dev/null +++ b/gtsam/base/tests/testLieMatrix.cpp @@ -0,0 +1,71 @@ +/* ---------------------------------------------------------------------------- + + * 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 testLieMatrix.cpp + * @author Richard Roberts + */ + +#include + +#include +#include +#include + +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(LieMatrix) +GTSAM_CONCEPT_LIE_INST(LieMatrix) + +/* ************************************************************************* */ +TEST( LieMatrix, construction ) { + Matrix m = Matrix_(2,2, 1.0, 2.0, 3.0, 4.0); + LieMatrix lie1(m), lie2(m); + + EXPECT(lie1.dim() == 4); + EXPECT(assert_equal(m, lie1.matrix())); + EXPECT(assert_equal(lie1, lie2)); +} + +/* ************************************************************************* */ +TEST( LieMatrix, other_constructors ) { + Matrix init = Matrix_(2,2, 10.0, 20.0, 30.0, 40.0); + LieMatrix exp(init); + LieMatrix a(2,2,10.0,20.0,30.0,40.0); + double data[] = {10,30,20,40}; + LieMatrix b(2,2,data); + EXPECT(assert_equal(exp, a)); + EXPECT(assert_equal(exp, b)); + EXPECT(assert_equal(b, a)); +} + +/* ************************************************************************* */ +TEST(LieMatrix, retract) { + LieMatrix init(2,2, 1.0,2.0,3.0,4.0); + init.print("init: "); + Vector update = Vector_(4, 3.0, 4.0, 6.0, 7.0); + + LieMatrix expected(2,2, 4.0, 6.0, 9.0, 11.0); + LieMatrix actual = init.retract(update); + + EXPECT(assert_equal(expected, actual)); + + LieMatrix expectedUpdate = update; + LieMatrix actualUpdate = init.localCoordinates(actual); + + EXPECT(assert_equal(expectedUpdate, actualUpdate)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + +