test Qr with old codebase fails
							parent
							
								
									c4ebab5e44
								
							
						
					
					
						commit
						52fc9cf4ba
					
				|  | @ -190,15 +190,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| /**
 | ||||
|  * Compute the 3x3 bottom-left block Q of the SE3 Expmap derivative matrix | ||||
|  *  J(xi) = [J_(w) Z_3x3; | ||||
|  *             Q   J_(w)] | ||||
|  *  where J_(w) is the SO3 Expmap derivative. | ||||
|  *  (see Chirikjian11book2, pg 44, eq 10.95. | ||||
|  *  The closed-form formula is similar to formula 102 in Barfoot14tro) | ||||
|  */ | ||||
| Matrix3 Pose3::computeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { | ||||
| Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { | ||||
|   const auto w = xi.head<3>(); | ||||
|   const auto v = xi.tail<3>(); | ||||
|   const Matrix3 V = skewSymmetric(v); | ||||
|  | @ -230,8 +222,8 @@ Matrix3 Pose3::computeQforExpmapDerivative(const Vector6& xi, double nearZeroThr | |||
|   } | ||||
|   else { | ||||
|     Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W) | ||||
|         - 1./24.*(W*W*V + V*W*W - 3*W*V*W) | ||||
|         + 1./120.*(W*V*W*W + W*W*V*W); | ||||
|         + 1./24.*(W*W*V + V*W*W - 3*W*V*W) | ||||
|         - 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W); | ||||
|   } | ||||
| #endif | ||||
| 
 | ||||
|  | @ -242,7 +234,7 @@ Matrix3 Pose3::computeQforExpmapDerivative(const Vector6& xi, double nearZeroThr | |||
| Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { | ||||
|   const Vector3 w = xi.head<3>(); | ||||
|   const Matrix3 Jw = Rot3::ExpmapDerivative(w); | ||||
|   const Matrix3 Q = computeQforExpmapDerivative(xi); | ||||
|   const Matrix3 Q = ComputeQforExpmapDerivative(xi); | ||||
|   Matrix6 J; | ||||
|   J << Jw, Z_3x3, Q, Jw; | ||||
|   return J; | ||||
|  | @ -253,7 +245,7 @@ Matrix6 Pose3::LogmapDerivative(const Pose3& pose) { | |||
|   const Vector6 xi = Logmap(pose); | ||||
|   const Vector3 w = xi.head<3>(); | ||||
|   const Matrix3 Jw = Rot3::LogmapDerivative(w); | ||||
|   const Matrix3 Q = computeQforExpmapDerivative(xi); | ||||
|   const Matrix3 Q = ComputeQforExpmapDerivative(xi); | ||||
|   const Matrix3 Q2 = -Jw*Q*Jw; | ||||
|   Matrix6 J; | ||||
|   J << Jw, Z_3x3, Q2, Jw; | ||||
|  |  | |||
|  | @ -181,7 +181,16 @@ public: | |||
|     static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none); | ||||
|   }; | ||||
| 
 | ||||
|   static Matrix3 computeQforExpmapDerivative( | ||||
|   /**
 | ||||
|   * Compute the 3x3 bottom-left block Q of SE3 Expmap right derivative matrix | ||||
|   *  J_r(xi) = [J_(w) Z_3x3; | ||||
|   *             Q_r   J_(w)] | ||||
|   *  where J_(w) is the SO3 Expmap right derivative. | ||||
|   *  (see Chirikjian11book2, pg 44, eq 10.95. | ||||
|   *  The closed-form formula is identical to formula 102 in Barfoot14tro where | ||||
|   *  Q_l of the SE3 Expmap left derivative matrix is given. | ||||
|   */ | ||||
|   static Matrix3 ComputeQforExpmapDerivative( | ||||
|       const Vector6& xi, double nearZeroThreshold = 1e-5); | ||||
| 
 | ||||
|   using LieGroup<Pose3, 6>::inverse; // version with derivative
 | ||||
|  |  | |||
|  | @ -807,6 +807,17 @@ TEST(Pose3, ExpmapDerivative2) { | |||
|   } | ||||
| } | ||||
| 
 | ||||
| TEST( Pose3, ExpmapDerivativeQr) { | ||||
|   Vector6 w = Vector6::Random(); | ||||
|   w.head<3>().normalize(); | ||||
|   w.head<3>() = w.head<3>() * 0.9e-2; | ||||
|   Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01); | ||||
|   Matrix expectedH = numericalDerivative21<Pose3, Vector6, | ||||
|       OptionalJacobian<6, 6> >(&Pose3::Expmap, w, boost::none); | ||||
|   Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>(); | ||||
|   EXPECT(assert_equal(expectedQr, actualQr, 1e-6)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( Pose3, LogmapDerivative) { | ||||
|   Matrix6 actualH; | ||||
|  | @ -818,15 +829,6 @@ TEST( Pose3, LogmapDerivative) { | |||
|   EXPECT(assert_equal(expectedH, actualH)); | ||||
| } | ||||
| 
 | ||||
| TEST( Pose3, computeQforExpmapDerivative) { | ||||
|   Vector6 w = Vector6::Random(); | ||||
|   w.head<3>().normalize(); | ||||
|   w.head<3>() = w.head<3>() * 0.09; | ||||
|   Matrix3 Qexact = Pose3::computeQforExpmapDerivative(w); | ||||
|   Matrix3 Qapprox = Pose3::computeQforExpmapDerivative(w, 0.1); | ||||
|   EXPECT(assert_equal(Qapprox, Qexact, 1e-5)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) { | ||||
|   return Pose3::adjointMap(xi) * v; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue