Now uses Adjoint map for derivatives of inverse/compose/between, see doc/math.pdf
							parent
							
								
									98aa08bcd0
								
							
						
					
					
						commit
						28f4493a2b
					
				|  | @ -13,13 +13,13 @@ namespace gtsam { | |||
|   /** Explicit instantiation of base class to export members */ | ||||
|   INSTANTIATE_LIE(Pose2); | ||||
| 
 | ||||
| 	static const Matrix I3 = eye(3); | ||||
| 	static const Matrix I3 = eye(3), Z12 = zeros(1,2); | ||||
|   static const Rot2 R_PI_2(0., 1.); | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   Matrix Pose2::matrix() const { | ||||
|   	Matrix R = r_.matrix(); | ||||
|   	Matrix Z = zeros(1,2); | ||||
|   	R = stack(2, &R, &Z); | ||||
|   	R = stack(2, &R, &Z12); | ||||
|   	Matrix T = Matrix_(3,1, t_.x(), t_.y(), 1.0); | ||||
|   	return collect(2, &R, &T); | ||||
|   } | ||||
|  | @ -34,6 +34,31 @@ namespace gtsam { | |||
|     return t_.equals(q.t_, tol) && r_.equals(q.r_, tol); | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   // Calculate Adjoint map
 | ||||
|   // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
 | ||||
|   Matrix AdjointMap(const Pose2& p) { | ||||
| 		const Rot2 R = p.r(); | ||||
| 		const Point2 t = p.t(); | ||||
|   	double c = R.c(), s = R.s(), x = t.x(), y = t.y(); | ||||
| 		return Matrix_(3,3, | ||||
| 				  c,  -s,   y, | ||||
| 				  s,   c,  -x, | ||||
| 				0.0, 0.0, 1.0 | ||||
| 				); | ||||
| 	} | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   Pose2 inverse(const Pose2& pose) { | ||||
| 		const Rot2& R = pose.r(); | ||||
| 		const Point2& t = pose.t(); | ||||
| 		return Pose2(inverse(R), R.unrotate(Point2(-t.x(), -t.y()))); | ||||
| 	} | ||||
| 
 | ||||
| 	Matrix Dinverse(const Pose2& pose) { | ||||
| 		return -AdjointMap(pose); | ||||
| 	} | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   Point2 transform_to(const Pose2& pose, const Point2& point, boost::optional< | ||||
| 			Matrix&> H1, boost::optional<Matrix&> H2) { | ||||
|  | @ -56,20 +81,12 @@ namespace gtsam { | |||
| 		Matrix H; transform_to(pose, point, boost::none, H); return H; | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   static const Rot2 R_PI_2(0., 1.); | ||||
| 	static Matrix DR1 = Matrix_(1, 3, 0., 0., 1.); | ||||
| 
 | ||||
|   Matrix Dcompose1(const Pose2& p1, const Pose2& p2) { | ||||
| 		Matrix R2_inv = p2.r().transpose(); | ||||
| 		Point2 Rt2_ = R_PI_2 * inverse(p2.r()) * p2.t(); | ||||
| 		Matrix Rt2 = Matrix_(2, 1, Rt2_.x(), Rt2_.y()); | ||||
| 		Matrix Dt1 = collect(2, &R2_inv, &Rt2); | ||||
| 		return gtsam::stack(2, &Dt1, &DR1); | ||||
| 		return AdjointMap(inverse(p2)); | ||||
|   } | ||||
| 
 | ||||
|   Matrix Dcompose2(const Pose2& p1, const Pose2& p2) { | ||||
|   	return eye(3); | ||||
|   	return I3; | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|  | @ -97,8 +114,7 @@ namespace gtsam { | |||
| 		double x = dt.x(), y = dt.y(); | ||||
| 		Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); | ||||
| 
 | ||||
| 		// FD: I don't understand this code (a performance-driven transformation
 | ||||
| 		// from Richard's heavier code) but it works.
 | ||||
| 		// FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
 | ||||
| 		if (H1) { | ||||
| 			double dt1 = -s2 * x + c2 * y; | ||||
| 			double dt2 = -c2 * x - s2 * y; | ||||
|  |  | |||
							
								
								
									
										11
									
								
								cpp/Pose2.h
								
								
								
								
							
							
						
						
									
										11
									
								
								cpp/Pose2.h
								
								
								
								
							|  | @ -71,10 +71,15 @@ namespace gtsam { | |||
|   /** return DOF, dimensionality of tangent space = 3 */ | ||||
|   inline size_t dim(const Pose2&) { return 3; } | ||||
| 
 | ||||
|   /**
 | ||||
|   * Calculate Adjoint map | ||||
|   * Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) | ||||
|   */ | ||||
|   Matrix AdjointMap(const Pose2& p); | ||||
| 
 | ||||
|   /** inverse transformation */ | ||||
|   inline Pose2 inverse(const Pose2& pose) { | ||||
|     return Pose2(inverse(pose.r()), | ||||
|         pose.r().unrotate(Point2(-pose.t().x(), -pose.t().y()))); } | ||||
|   Pose2 inverse(const Pose2& pose); | ||||
|   Matrix Dinverse(const Pose2& pose); | ||||
| 
 | ||||
|   /** compose this transformation onto another (first p1 and then p2) */ | ||||
|   inline Pose2 compose(const Pose2& p0, const Pose2& p1) { | ||||
|  |  | |||
|  | @ -148,8 +148,8 @@ TEST(Pose2, compose_a) | |||
|   Pose2 pose2(M_PI/2.0, Point2(0.0, 2.0)); | ||||
| 
 | ||||
|   Pose2 actual = compose(pose1, pose2); | ||||
|   Matrix actualH1 = Dcompose1(pose1, pose2); | ||||
|   Matrix actualH2 = Dcompose2(pose1, pose2); | ||||
|   Matrix actualDcompose1 = Dcompose1(pose1, pose2); | ||||
|   Matrix actualDcompose2 = Dcompose2(pose1, pose2); | ||||
| 
 | ||||
|   Pose2 expected(3.0*M_PI/4.0, Point2(-sqrt(0.5), 3.0*sqrt(0.5))); | ||||
|   CHECK(assert_equal(expected, actual)); | ||||
|  | @ -162,10 +162,10 @@ TEST(Pose2, compose_a) | |||
|   Matrix expectedH2 = eye(3); | ||||
|   Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5); | ||||
|   Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5); | ||||
|   CHECK(assert_equal(expectedH1,actualH1)); | ||||
|   CHECK(assert_equal(numericalH1,actualH1)); | ||||
|   CHECK(assert_equal(expectedH2,actualH2)); | ||||
|   CHECK(assert_equal(numericalH2,actualH2)); | ||||
|   CHECK(assert_equal(expectedH1,actualDcompose1)); | ||||
|   CHECK(assert_equal(numericalH1,actualDcompose1)); | ||||
|   CHECK(assert_equal(expectedH2,actualDcompose2)); | ||||
|   CHECK(assert_equal(numericalH2,actualDcompose2)); | ||||
| 
 | ||||
|   Point2 point(sqrt(0.5), 3.0*sqrt(0.5)); | ||||
|   Point2 expected_point(-1.0, -1.0); | ||||
|  | @ -186,13 +186,13 @@ TEST(Pose2, compose_b) | |||
| 
 | ||||
|   Pose2 pose_actual_op = pose1 * pose2; | ||||
|   Pose2 pose_actual_fcn = compose(pose1, pose2); | ||||
|   Matrix actualH1 = Dcompose1(pose1, pose2); | ||||
|   Matrix actualH2 = Dcompose2(pose1, pose2); | ||||
|   Matrix actualDcompose1 = Dcompose1(pose1, pose2); | ||||
|   Matrix actualDcompose2 = Dcompose2(pose1, pose2); | ||||
| 
 | ||||
|   Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5); | ||||
|   Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5); | ||||
|   CHECK(assert_equal(numericalH1,actualH1)); | ||||
|   CHECK(assert_equal(numericalH2,actualH2)); | ||||
|   CHECK(assert_equal(numericalH1,actualDcompose1)); | ||||
|   CHECK(assert_equal(numericalH2,actualDcompose2)); | ||||
| 
 | ||||
|   CHECK(assert_equal(pose_expected, pose_actual_op)); | ||||
|   CHECK(assert_equal(pose_expected, pose_actual_fcn)); | ||||
|  | @ -209,13 +209,13 @@ TEST(Pose2, compose_c) | |||
| 
 | ||||
|   Pose2 pose_actual_op = pose1 * pose2; | ||||
|   Pose2 pose_actual_fcn = compose(pose1,pose2); | ||||
|   Matrix actualH1 = Dcompose1(pose1, pose2); | ||||
|   Matrix actualH2 = Dcompose2(pose1, pose2); | ||||
|   Matrix actualDcompose1 = Dcompose1(pose1, pose2); | ||||
|   Matrix actualDcompose2 = Dcompose2(pose1, pose2); | ||||
| 
 | ||||
|   Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5); | ||||
|   Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5); | ||||
|   CHECK(assert_equal(numericalH1,actualH1)); | ||||
|   CHECK(assert_equal(numericalH2,actualH2)); | ||||
|   CHECK(assert_equal(numericalH1,actualDcompose1)); | ||||
|   CHECK(assert_equal(numericalH2,actualDcompose2)); | ||||
| 
 | ||||
|   CHECK(assert_equal(pose_expected, pose_actual_op)); | ||||
|   CHECK(assert_equal(pose_expected, pose_actual_fcn)); | ||||
|  | @ -234,8 +234,12 @@ TEST(Pose2, inverse ) | |||
| 	Point2 l(4,5), g(-4,6); | ||||
| 	CHECK(assert_equal(g,gTl*l)); | ||||
| 	CHECK(assert_equal(l,lTg*g)); | ||||
| } | ||||
| 
 | ||||
| 	// Check derivative
 | ||||
|   Matrix numericalH = numericalDerivative11<Pose2,Pose2>(inverse, lTg, 1e-5); | ||||
|   Matrix actualDinverse = Dinverse(lTg); | ||||
|   CHECK(assert_equal(numericalH,actualDinverse)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Vector homogeneous(const Point2& p) { | ||||
|  | @ -304,7 +308,7 @@ TEST( Pose2, between ) | |||
| 
 | ||||
|   Matrix actualH1,actualH2; | ||||
|   Pose2 expected(M_PI_2, Point2(2,2)); | ||||
|   Pose2 actual1 = between(gT1,gT2); // gT2 * 1Tg does not make sense !!!!!
 | ||||
|   Pose2 actual1 = between(gT1,gT2); | ||||
|   Pose2 actual2 = between(gT1,gT2,actualH1,actualH2); | ||||
|   CHECK(assert_equal(expected,actual1)); | ||||
|   CHECK(assert_equal(expected,actual2)); | ||||
|  | @ -317,6 +321,8 @@ TEST( Pose2, between ) | |||
|   Matrix numericalH1 = numericalDerivative21(between<Pose2>, gT1, gT2, 1e-5); | ||||
|   CHECK(assert_equal(expectedH1,actualH1)); | ||||
|   CHECK(assert_equal(numericalH1,actualH1)); | ||||
| 	// Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
 | ||||
|   CHECK(assert_equal(-AdjointMap(between(gT2,gT1)),actualH1)); | ||||
| 
 | ||||
|   Matrix expectedH2 = Matrix_(3,3, | ||||
|        1.0, 0.0, 0.0, | ||||
|  | @ -326,6 +332,7 @@ TEST( Pose2, between ) | |||
|   Matrix numericalH2 = numericalDerivative22(between<Pose2>, gT1, gT2, 1e-5); | ||||
|   CHECK(assert_equal(expectedH2,actualH2)); | ||||
|   CHECK(assert_equal(numericalH2,actualH2)); | ||||
| 
 | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue