Made testLie a bit stronger
							parent
							
								
									d385984f26
								
							
						
					
					
						commit
						6a67ea86af
					
				|  | @ -18,6 +18,7 @@ | |||
| 
 | ||||
| #include <gtsam/base/ProductLieGroup.h> | ||||
| 
 | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/geometry/Point2.h> | ||||
| #include <gtsam/base/testLie.h> | ||||
| 
 | ||||
|  | @ -26,35 +27,81 @@ | |||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| static const double tol = 1e-9; | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| typedef ProductLieGroup<Point2, Point2> MyPoint2Pair; | ||||
| typedef ProductLieGroup<Point2, Pose2> Product; | ||||
| 
 | ||||
| // Define any direct product group to be a model of the multiplicative Group concept
 | ||||
| namespace gtsam { | ||||
| template<> struct traits<MyPoint2Pair> : internal::LieGroupTraits<MyPoint2Pair> { | ||||
|   static void Print(const MyPoint2Pair& m, const string& s = "") { | ||||
|     cout << s << "(" << m.first << "," << m.second << ")" << endl; | ||||
| template<> struct traits<Product> : internal::LieGroupTraits<Product> { | ||||
|   static void Print(const Product& m, const string& s = "") { | ||||
|     cout << s << "(" << m.first << "," << m.second.translation() << "/" | ||||
|         << m.second.theta() << ")" << endl; | ||||
|   } | ||||
|   static bool Equals(const MyPoint2Pair& m1, const MyPoint2Pair& m2, | ||||
|       double tol = 1e-8) { | ||||
|     return m1 == m2; | ||||
|   static bool Equals(const Product& m1, const Product& m2, double tol = 1e-8) { | ||||
|     return m1.first.equals(m2.first, tol) && m1.second.equals(m2.second, tol); | ||||
|   } | ||||
| }; | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| TEST(Lie, ProductLieGroup) { | ||||
|   BOOST_CONCEPT_ASSERT((IsGroup<MyPoint2Pair>)); | ||||
|   BOOST_CONCEPT_ASSERT((IsManifold<MyPoint2Pair>)); | ||||
|   BOOST_CONCEPT_ASSERT((IsLieGroup<MyPoint2Pair>)); | ||||
|   MyPoint2Pair pair1; | ||||
|   Vector4 d; | ||||
|   d << 1, 2, 3, 4; | ||||
|   MyPoint2Pair expected(Point2(1, 2), Point2(3, 4)); | ||||
|   MyPoint2Pair pair2 = pair1.retract(d); | ||||
|   BOOST_CONCEPT_ASSERT((IsGroup<Product>)); | ||||
|   BOOST_CONCEPT_ASSERT((IsManifold<Product>)); | ||||
|   BOOST_CONCEPT_ASSERT((IsLieGroup<Product>)); | ||||
|   Product pair1; | ||||
|   Vector5 d; | ||||
|   d << 1, 2, 0.1, 0.2, 0.3; | ||||
|   Product expected(Point2(1, 2), Pose2::Expmap(Vector3(0.1, 0.2, 0.3))); | ||||
|   Product pair2 = pair1.retract(d); | ||||
|   EXPECT(assert_equal(expected, pair2, 1e-9)); | ||||
|   EXPECT(assert_equal(d, pair1.localCoordinates(pair2), 1e-9)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Product compose_proxy(const Product& A, const Product& B) { | ||||
|   return A.compose(B); | ||||
| } | ||||
| TEST( testProduct, compose ) { | ||||
|   Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1; | ||||
| 
 | ||||
|   Matrix actH1, actH2; | ||||
|   state1.compose(state2, actH1, actH2); | ||||
|   Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2); | ||||
|   Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2); | ||||
|   EXPECT(assert_equal(numericH1, actH1, tol)); | ||||
|   EXPECT(assert_equal(numericH2, actH2, tol)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Product between_proxy(const Product& A, const Product& B) { | ||||
|   return A.between(B); | ||||
| } | ||||
| TEST( testProduct, between ) { | ||||
|   Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1; | ||||
| 
 | ||||
|   Matrix actH1, actH2; | ||||
|   state1.between(state2, actH1, actH2); | ||||
|   Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2); | ||||
|   Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2); | ||||
|   EXPECT(assert_equal(numericH1, actH1, tol)); | ||||
|   EXPECT(assert_equal(numericH2, actH2, tol)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Product inverse_proxy(const Product& A) { | ||||
|   return A.inverse(); | ||||
| } | ||||
| TEST( testProduct, inverse ) { | ||||
|   Product state1(Point2(1, 2), Pose2(3, 4, 5)); | ||||
| 
 | ||||
|   Matrix actH1; | ||||
|   state1.inverse(actH1); | ||||
|   Matrix numericH1 = numericalDerivative11(inverse_proxy, state1); | ||||
|   EXPECT(assert_equal(numericH1, actH1, tol)); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| int main() { | ||||
|   TestResult tr; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue