method to get Expmap coefficients
							parent
							
								
									4085d3a64e
								
							
						
					
					
						commit
						23b96d4680
					
				|  | @ -194,6 +194,33 @@ Similarity2 Similarity2::Align(const Pose2Pairs& abPosePairs) { | |||
|   return internal::AlignGivenR(abPointPairs, aRb_estimate); | ||||
| } | ||||
| 
 | ||||
| Matrix2 Similarity2::GetV(double theta, double lambda) { | ||||
|   // Derivation from https://ethaneade.com/lie_groups.pdf page 6
 | ||||
|   const double lambda2 = lambda * lambda, theta2 = theta * theta; | ||||
|   double A, B, C; | ||||
|   if (theta2 > 1e-9) { | ||||
|     A = sin(theta) / theta; | ||||
|     B = (1 - cos(theta)) / theta2; | ||||
|     C = (1 - A) / theta2; | ||||
|   } else { | ||||
|     // Taylor series expansion for theta=0
 | ||||
|     A = 1.0; | ||||
|     B = 0.5 - theta2 / 24.0; | ||||
|     C = 1.0 / 6.0 - theta2 / 120.0; | ||||
|   } | ||||
|   double alpha = 1.0 / (1.0 + theta2 / lambda2); | ||||
|   const double s = exp(lambda); | ||||
| 
 | ||||
|   double s_inv = 1.0 / s; | ||||
|   double X = alpha * (1 - s_inv) / lambda + (1 - alpha) * (A - lambda * B); | ||||
|   double Y = | ||||
|       alpha * (s_inv - 1 + lambda) / lambda2 + (1 - alpha) * (B - lambda * C); | ||||
| 
 | ||||
|   Matrix2 V; | ||||
|   V << X, -theta * Y, theta * Y, X; | ||||
|   return V; | ||||
| } | ||||
| 
 | ||||
| Vector4 Similarity2::Logmap(const Similarity2& S,  //
 | ||||
|                             OptionalJacobian<4, 4> Hm) { | ||||
|   const Vector2 u = S.t_; | ||||
|  |  | |||
|  | @ -141,6 +141,9 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> { | |||
| 
 | ||||
|   using LieAlgebra = Matrix3; | ||||
| 
 | ||||
|   /// Calculate expmap and logmap coefficients.
 | ||||
|   static Matrix2 GetV(double theta, double lambda); | ||||
| 
 | ||||
|   /**
 | ||||
|    * Log map at the identity | ||||
|    * \f$ [t_x, t_y, \delta, \lambda] \f$ | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue