add testLieScalar
							parent
							
								
									ea92ad7be4
								
							
						
					
					
						commit
						9a805d68bf
					
				
							
								
								
									
										104
									
								
								.cproject
								
								
								
								
							
							
						
						
									
										104
									
								
								.cproject
								
								
								
								
							|  | @ -390,12 +390,19 @@ | |||
| 					</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> | ||||
| 						<runAllBuilders>true</runAllBuilders> | ||||
| 					</target> | ||||
| 					<target name="tests/testLieScalar.run" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments>-j2</buildArguments> | ||||
| 						<buildTarget>tests/testLieScalar.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>true</useDefaultCommand> | ||||
| 						<runAllBuilders>true</runAllBuilders> | ||||
| 					</target> | ||||
| 					<target name="check" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments>-j2</buildArguments> | ||||
|  | @ -510,7 +517,6 @@ | |||
| 					</target> | ||||
| 					<target name="testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments/> | ||||
| 						<buildTarget>testBayesTree.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -518,7 +524,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> | ||||
|  | @ -566,7 +571,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> | ||||
|  | @ -574,7 +578,6 @@ | |||
| 					</target> | ||||
| 					<target name="testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments/> | ||||
| 						<buildTarget>testSymbolicFactor.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -582,7 +585,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> | ||||
|  | @ -662,6 +664,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> | ||||
|  | @ -757,6 +760,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> | ||||
|  | @ -764,6 +768,7 @@ | |||
| 					</target> | ||||
| 					<target name="testGaussianBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments/> | ||||
| 						<buildTarget>testGaussianBayesNet.run</buildTarget> | ||||
| 						<stopOnError>true</stopOnError> | ||||
| 						<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -771,6 +776,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> | ||||
|  | @ -778,6 +784,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> | ||||
|  | @ -785,6 +792,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> | ||||
|  | @ -792,6 +800,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> | ||||
|  | @ -1007,6 +1016,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> | ||||
|  | @ -1046,6 +1056,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> | ||||
|  | @ -1053,6 +1064,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> | ||||
|  | @ -1138,6 +1150,46 @@ | |||
| 						<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> | ||||
|  | @ -1234,46 +1286,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="check" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 						<buildCommand>make</buildCommand> | ||||
| 						<buildArguments>-j2</buildArguments> | ||||
|  |  | |||
|  | @ -0,0 +1,64 @@ | |||
| /**
 | ||||
|  * @file LieScalar.h | ||||
|  * @brief A wrapper around scalar providing Lie compatibility | ||||
|  * @author Kai Ni | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/base/Lie.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| 	/**
 | ||||
| 	 * LieScalar is a wrapper around double to allow it to be a Lie type | ||||
| 	 */ | ||||
| 	struct LieScalar : public Lie<LieScalar>, Testable<LieScalar> { | ||||
| 
 | ||||
| 		/** default constructor - should be unnecessary */ | ||||
| 		LieScalar() {} | ||||
| 
 | ||||
| 		/** wrap a double */ | ||||
| 		LieScalar(double d) : d_(d) {} | ||||
| 
 | ||||
| 		/** print @param s optional string naming the object */ | ||||
| 		inline void print(const std::string& name="") const { | ||||
| 	    std::cout << name << ": " << d_ << std::endl; | ||||
| 		} | ||||
| 
 | ||||
| 		/** equality up to tolerance */ | ||||
| 		inline bool equals(const LieScalar& expected, double tol=1e-5) const { | ||||
| 			return fabs(expected.d_ - d_) <= tol; | ||||
| 		} | ||||
| 
 | ||||
| 		/**
 | ||||
| 		 * Returns dimensionality of the tangent space | ||||
| 		 */ | ||||
| 		inline size_t dim() const { return 1; } | ||||
| 
 | ||||
| 		/**
 | ||||
| 		 * Returns Exponential map update of T | ||||
| 		 * Default implementation calls global binary function | ||||
| 		 */ | ||||
| 		inline LieScalar expmap(const Vector& v) const { return LieScalar(d_ + v(0)); } | ||||
| 
 | ||||
| 		/** expmap around identity */ | ||||
| 		static inline LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); } | ||||
| 
 | ||||
| 		/**
 | ||||
| 		 * Returns Log map | ||||
| 		 * Default Implementation calls global binary function | ||||
| 		 */ | ||||
| 		inline Vector logmap(const LieScalar& lp) const { | ||||
| 			return Vector_(1, lp.d_ - d_); | ||||
| 		} | ||||
| 
 | ||||
| 		/** Logmap around identity - just returns with default cast back */ | ||||
| 		static inline Vector Logmap(const LieScalar& p) { return Vector_(1, p.d_); } | ||||
| 
 | ||||
| 
 | ||||
| 	private: | ||||
| 	    double d_; | ||||
| 	}; | ||||
| } // \namespace gtsam
 | ||||
|  | @ -26,9 +26,9 @@ endif | |||
| headers += Testable.h TestableAssertions.h numericalDerivative.h  | ||||
| 
 | ||||
| # Lie Groups
 | ||||
| headers += Lie.h Lie-inl.h lieProxies.h | ||||
| headers += Lie.h Lie-inl.h lieProxies.h LieScalar.h | ||||
| sources += LieVector.cpp | ||||
| check_PROGRAMS += tests/testLieVector | ||||
| check_PROGRAMS += tests/testLieVector tests/testLieScalar | ||||
| 
 | ||||
| # Data structures
 | ||||
| headers += BTree.h DSF.h | ||||
|  |  | |||
|  | @ -0,0 +1,30 @@ | |||
| /**
 | ||||
|  * @file testLieScalar.cpp | ||||
|  * @author Kai Ni | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| #include <gtsam/base/LieScalar.h> | ||||
| 
 | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( testLieScalar, construction ) { | ||||
| 	double d = 2.; | ||||
| 	LieScalar lie1(d), lie2(d); | ||||
| 
 | ||||
| 	EXPECT(lie1.dim() == 1); | ||||
| 	EXPECT(assert_equal(lie1, lie2)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( testLieScalar, logmap ) { | ||||
| 	LieScalar lie1(1.), lie2(3.); | ||||
| 
 | ||||
| 	EXPECT(assert_equal(Vector_(1, 2.), lie1.logmap(lie2))); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { TestResult tr; return TestRegistry::runAllTests(tr); } | ||||
| /* ************************************************************************* */ | ||||
		Loading…
	
		Reference in New Issue