12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

LieMatrix loses its mojo (superfluous Lie/Manifold stuff)

release/4.3a0
dellaert 2014-12-22 14:24:47 +01:00
parent 242ebca494
commit 74ac79d588
3 changed files with 30 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -2570,6 +2570,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLieMatrix.run" path="build/gtsam/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testLieMatrix.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>

109
gtsam/base/LieMatrix.h
View File

@ -41,7 +41,7 @@ struct LieMatrix : public Matrix {
/// @{
enum { dimension = Eigen::Dynamic };
/** default constructor - should be unnecessary */
/** default constructor - only for serialize */
LieMatrix() {}
/** initialize from a normal matrix */
@ -83,111 +83,26 @@ struct LieMatrix : public Matrix {
}
/// @}
/// @name Manifold interface
/// @name VectorSpace requirements
/// @{
/** Returns dimensionality of the tangent space */
inline size_t dim() const { return size(); }
typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> RowMajor;
typedef const RowMajor ConstRowMajor;
/** Convert to vector, is done row-wise - TODO why? */
inline Vector vector() const {
Vector result(size());
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor> RowMajor;
Eigen::Map<RowMajor>(&result(0), rows(), cols()) = *this;
return result;
}
/** 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() != size())
throw std::invalid_argument("LieMatrix::retract called with Vector of incorrect size");
return LieMatrix(*this + Eigen::Map<ConstRowMajor>(&v(0), rows(), cols()));
}
inline LieMatrix retract(const Vector& v, OptionalJacobian<-1, -1> Horigin,
OptionalJacobian<-1, -1> Hv) const {
if (Horigin || Hv)
throw std::runtime_error("LieMatrix::retract derivative not implemented");
return retract(v);
}
/** @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(size());
Eigen::Map<RowMajor>(&result(0), rows(), cols()) = t2 - *this;
return result;
}
Vector localCoordinates(const LieMatrix& ts, OptionalJacobian<-1, -1> Horigin,
OptionalJacobian<-1, -1> Hother) const {
if (Horigin || Hother)
throw std::runtime_error("LieMatrix::localCoordinates derivative not implemented");
return localCoordinates(ts);
}
/// @}
/// @name Group interface
/// @{
/** 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,
OptionalJacobian<-1,-1> H1 = boost::none,
OptionalJacobian<-1,-1> 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,
OptionalJacobian<-1,-1> H1 = boost::none,
OptionalJacobian<-1,-1> 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(OptionalJacobian<-1,-1> H = boost::none) const {
if(H) *H = -eye(dim());
return LieMatrix(-(*this));
}
/// @}
/// @name Lie group interface
/// @{
/** Expmap around identity */
static inline LieMatrix Expmap(const Vector& v, OptionalJacobian<-1,-1> H = boost::none) {
throw std::runtime_error("LieMatrix::Expmap(): Don't use this function");
return LieMatrix(v); }
/** Logmap around identity */
static inline Vector Logmap(const LieMatrix& p, OptionalJacobian<-1,-1> H = boost::none) {
if (H) throw std::runtime_error("LieMatrix::Logmap derivative not implemented");
Vector result(p.size());
Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> >(
result.data(), p.rows(), p.cols()) = p;
return result;
}
/// @}
private:
@ -205,6 +120,18 @@ private:
template<>
struct traits_x<LieMatrix> : public internal::VectorSpace<LieMatrix> {};
struct traits_x<LieMatrix> : public internal::VectorSpace<LieMatrix> {
// Override Retract, as the default version does not know how to initialize
static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = Eye(origin);
if (H2) *H2 = Eye(origin);
typedef const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor> RowMajor;
return origin + Eigen::Map<RowMajor>(&v(0), origin.rows(), origin.cols());
}
};
} // \namespace gtsam

8
gtsam/base/tests/testLieMatrix.cpp
View File

@ -30,7 +30,7 @@ TEST( LieMatrix, construction ) {
Matrix m = (Matrix(2,2) << 1.0,2.0, 3.0,4.0).finished();
LieMatrix lie1(m), lie2(m);
EXPECT(lie1.dim() == 4);
EXPECT(traits_x<LieMatrix>::GetDimension(m) == 4);
EXPECT(assert_equal(m, lie1.matrix()));
EXPECT(assert_equal(lie1, lie2));
}
@ -50,17 +50,17 @@ TEST(LieMatrix, retract) {
Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0).finished();
LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0).finished());
LieMatrix actual = init.retract(update);
LieMatrix actual = traits_x<LieMatrix>::Retract(init,update);
EXPECT(assert_equal(expected, actual));
Vector expectedUpdate = update;
Vector actualUpdate = init.localCoordinates(actual);
Vector actualUpdate = traits_x<LieMatrix>::Local(init,actual);
EXPECT(assert_equal(expectedUpdate, actualUpdate));
Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4).finished();
Vector actualLogmap = LieMatrix::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0).finished()));
Vector actualLogmap = traits_x<LieMatrix>::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0).finished()));
EXPECT(assert_equal(expectedLogmap, actualLogmap));
}