Added LieMatrix to allow optimizing for matrices (treated as manifold objects) in gtsam
Richard Roberts
parent
a9945f2265
commit
a0fc13fce8
|
|
@ -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 <gtsam/base/LieMatrix.h>
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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 <cstdarg>
|
||||
|
||||
#include <gtsam/base/Lie.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/DerivedValue.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* LieVector is a wrapper around vector to allow it to be a Lie type
|
||||
*/
|
||||
struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
|
||||
|
||||
/** 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<int M, int N>
|
||||
LieMatrix(const Eigen::Matrix<double, M, N>& 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<const Matrix>(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<Matrix>(*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<const Matrix>(&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<Matrix>(&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<gtsam::Matrix&> H1=boost::none,
|
||||
boost::optional<gtsam::Matrix&> 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<gtsam::Matrix&> H1=boost::none,
|
||||
boost::optional<gtsam::Matrix&> 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<gtsam::Matrix&> 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<const Vector>(&p(0,0), p.dim()); }
|
||||
|
||||
};
|
||||
} // \namespace gtsam
|
||||
|
|
@ -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 <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
#include <gtsam/base/LieMatrix.h>
|
||||
|
||||
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); }
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
||||
Loading…
Reference in New Issue