Added Manifold and Lie Concept checks, fixed LieScalar and LieVector
Alex Cunningham
parent
ccdd5b8fe6
commit
b8c56f9047
189
.cproject
189
.cproject
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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,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_;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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 ) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue