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

Added Manifold and Lie Concept checks, fixed LieScalar and LieVector

release/4.3a0
Alex Cunningham 2011-11-04 21:44:24 +00:00
parent ccdd5b8fe6
commit b8c56f9047
8 changed files with 269 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

189
.cproject
View File

@ -375,6 +375,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -401,7 +409,6 @@
</target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -409,7 +416,6 @@
</target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -457,7 +463,6 @@
</target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -465,7 +470,6 @@
</target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -473,7 +477,6 @@
</target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -489,20 +492,11 @@
</target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -529,6 +523,7 @@
</target>
<target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -600,6 +595,7 @@
</target>
<target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testInference.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -607,6 +603,7 @@
</target>
<target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -614,6 +611,7 @@
</target>
<target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -621,6 +619,7 @@
</target>
<target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -628,6 +627,7 @@
</target>
<target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -721,15 +721,7 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
@ -737,14 +729,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -785,7 +769,15 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
@ -793,6 +785,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1107,7 +1107,6 @@
</target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1385,6 +1384,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build_retract" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1507,6 +1514,7 @@
</target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1546,6 +1554,7 @@
</target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1553,6 +1562,7 @@
</target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1800,6 +1810,7 @@
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1821,46 +1832,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="dist" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>dist</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1957,6 +1928,62 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="dist" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>dist</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1997,22 +2024,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
</buildTargets>
</storageModule>
</cproject>

49
gtsam/base/Lie.h
View File

@ -15,6 +15,10 @@
* @author Richard Roberts
* @author Alex Cunningham
*
* This concept check provides a specialization on the Manifold type,
* in which the Manifolds represented require an algebra and group structure.
* All Lie types must also be a Manifold.
*
* The necessary functions to implement for Lie are defined
* below with additional details as to the interface. The
* concept checking function in class Lie will check whether or not
@ -55,8 +59,7 @@
#pragma once
#include <string>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Manifold.h>
namespace gtsam {
@ -78,18 +81,15 @@ namespace gtsam {
inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));}
/**
* Base class for Lie group type
* This class uses the Curiously Recurring Template design pattern to allow for
* concept checking using a private function.
* Concept check class for Lie group type
*
* T is the derived Lie type, like Point2, Pose3, etc.
* T is the Lie type, like Point2, Pose3, etc.
*
* By convention, we use capital letters to designate a static function
*/
template <class T>
class Lie {
class LieConcept {
private:
/** concept checking function - implement the functions this demands */
static void concept_check(const T& t) {
@ -182,25 +182,16 @@ namespace gtsam {
return expm(xhat,K);
}
/**
* function wrappers for full versions of expmap/logmap
* these will default simple types to using the existing expmap/logmap,
* but more complex ones can be specialized to use improved versions
*
* TODO: replace this approach with a naming scheme that doesn't call
* non-expmap operations "expmap" - use same approach, but with "update"
*/
/** unary versions */
template<class T>
T ExpmapFull(const Vector& xi) { return T::Expmap(xi); }
template<class T>
Vector LogmapFull(const T& p) { return T::Logmap(p); }
/** binary versions */
template<class T>
T expmapFull(const T& t, const Vector& v) { return t.expmap(v); }
template<class T>
Vector logmapFull(const T& t, const T& p2) { return t.logmap(p2); }
} // namespace gtsam
/**
* Macros for using the ManifoldConcept
* - An instantiation for use inside unit tests
* - A typedef for use inside generic algorithms
*
* NOTE: intentionally not in the gtsam namespace to allow for classes not in
* the gtsam namespace to be more easily enforced as testable
*/
/// TODO: find better name for "INST" macro, something like "UNIT" or similar
#define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::LieConcept<T>;
#define GTSAM_CONCEPT_LIE_TYPE(T) typedef gtsam::LieConcept<T> _gtsam_LieConcept_##T;

24
gtsam/base/LieScalar.h
View File

@ -24,7 +24,7 @@ namespace gtsam {
/**
* LieScalar is a wrapper around double to allow it to be a Lie type
*/
struct LieScalar : public Lie<LieScalar> {
struct LieScalar {
/** default constructor - should be unnecessary */
LieScalar() {}
@ -72,6 +72,28 @@ namespace gtsam {
/** Logmap around identity - just returns with default cast back */
static inline Vector Logmap(const LieScalar& p) { return Vector_(1, p.d_); }
inline LieScalar between(const LieScalar& t2) const { return LieScalar(t2.value() - d_); }
/** compose with another object */
inline LieScalar compose(const LieScalar& t2) const { return LieScalar(t2.value() + d_); }
/** invert the object and yield a new one */
inline LieScalar inverse() const { return LieScalar(-d_); }
// Manifold requirements
inline LieScalar retract(const Vector& v) const { return expmap(v); }
/** expmap around identity */
inline static LieScalar Retract(const Vector& v) { return Expmap(v); }
/**
* Returns inverse retraction
*/
inline Vector unretract(const LieScalar& t2) const { return logmap(t2); }
/** Unretract around identity */
inline static Vector Unretract(const LieScalar& t) { return Logmap(t); }
private:
double d_;

17
gtsam/base/LieVector.h
View File

@ -25,7 +25,7 @@ namespace gtsam {
/**
* LieVector is a wrapper around vector to allow it to be a Lie type
*/
struct LieVector : public Vector, public Lie<LieVector> {
struct LieVector : public Vector {
/** default constructor - should be unnecessary */
LieVector() {}
@ -100,5 +100,20 @@ struct LieVector : public Vector, public Lie<LieVector> {
inline LieVector inverse() const {
return LieVector(-1.0 * vector());
}
// Manifold requirements
inline LieVector retract(const Vector& v) const { return expmap(v); }
/** expmap around identity */
inline static LieVector Retract(const Vector& v) { return Expmap(v); }
/**
* Returns inverse retraction
*/
inline Vector unretract(const LieVector& t2) const { return logmap(t2); }
/** Unretract around identity */
inline static Vector Unretract(const LieVector& t) { return Logmap(t); }
};
} // \namespace gtsam

3
gtsam/base/Makefile.am
View File

@ -25,7 +25,8 @@ headers += Testable.h TestableAssertions.h numericalDerivative.h
sources += timing.cpp debug.cpp
check_PROGRAMS += tests/testDebug tests/testTestableAssertions
# Lie Groups
# Manifolds and Lie Groups
headers += Manifold.h
headers += Lie.h Lie-inl.h lieProxies.h LieScalar.h
sources += LieVector.cpp
check_PROGRAMS += tests/testLieVector tests/testLieScalar

102
gtsam/base/Manifold.h Normal file
View File

@ -0,0 +1,102 @@
/* ----------------------------------------------------------------------------
* 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 Manifold.h
* @brief Base class and basic functions for Manifold types
* @author Richard Roberts
* @author Alex Cunningham
*
* The necessary functions to implement for Manifold are defined
* below with additional details as to the interface. The
* concept checking function in class Manifold will check whether or not
* the function exists and throw compile-time errors.
*
* Returns dimensionality of the tangent space
* inline size_t dim() const;
*
* Returns Retraction update of T
* T retract(const Vector& v) const;
*
* Retract around identity
* static T Retract(const Vector& v);
*
* Returns inverse retraction operation
* A default implementation of unretract(*this, lp) is available:
* Vector unretract(const T& lp) const;
*
* Unretract around identity
* static Vector Unretract(const T& p);
*
*/
#pragma once
#include <string>
#include <gtsam/base/Matrix.h>
namespace gtsam {
/**
* Concept check class for Manifold types
* Requires a mapping between a linear tangent space and the underlying
* manifold, of which Lie is a specialization.
*
* T is the Manifold type, like Point2, Pose3, etc.
*
* By convention, we use capital letters to designate a static function
*/
template <class T>
class ManifoldConcept {
private:
/** concept checking function - implement the functions this demands */
static void concept_check(const T& t) {
/** assignment */
T t2 = t;
/**
* Returns dimensionality of the tangent space
*/
size_t dim_ret = t.dim();
/**
* Returns Retraction update of T
*/
T retract_ret = t.retract(gtsam::zero(dim_ret));
/** expmap around identity */
T retract_identity_ret = T::Retract(gtsam::zero(dim_ret));
/**
* Returns inverse retraction
*/
Vector unretract_ret = t.unretract(t2);
/** Unretract around identity */
Vector unretract_identity_ret = T::Unretract(t);
}
};
} // namespace gtsam
/**
* Macros for using the ManifoldConcept
* - An instantiation for use inside unit tests
* - A typedef for use inside generic algorithms
*
* NOTE: intentionally not in the gtsam namespace to allow for classes not in
* the gtsam namespace to be more easily enforced as testable
*/
/// TODO: find better name for "INST" macro, something like "UNIT" or similar
#define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::ManifoldConcept<T>;
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T;

3
gtsam/base/tests/testLieScalar.cpp
View File

@ -17,11 +17,14 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/LieScalar.h>
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(LieScalar)
GTSAM_CONCEPT_MANIFOLD_INST(LieScalar)
GTSAM_CONCEPT_LIE_INST(LieScalar)
const double tol=1e-9;

3
gtsam/base/tests/testLieVector.cpp
View File

@ -17,11 +17,14 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/LieVector.h>
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(LieVector)
GTSAM_CONCEPT_MANIFOLD_INST(LieVector)
GTSAM_CONCEPT_LIE_INST(LieVector)
/* ************************************************************************* */
TEST( testLieVector, construction ) {