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