fix Sim2 Expmap and Logmap
							parent
							
								
									23b96d4680
								
							
						
					
					
						commit
						5ccf9dbd14
					
				|  | @ -223,11 +223,11 @@ Matrix2 Similarity2::GetV(double theta, double lambda) { | |||
| 
 | ||||
| Vector4 Similarity2::Logmap(const Similarity2& S,  //
 | ||||
|                             OptionalJacobian<4, 4> Hm) { | ||||
|   const Vector2 u = S.t_; | ||||
|   const Vector1 w = Rot2::Logmap(S.R_); | ||||
|   const double s = log(S.s_); | ||||
|   const double lambda = log(S.s_); | ||||
|   // In Expmap, t = V * u -> in Logmap, u = V^{-1} * t
 | ||||
|   Vector4 result; | ||||
|   result << u, w, s; | ||||
|   result << GetV(w[0], lambda).inverse() * S.t_, w, lambda; | ||||
|   if (Hm) { | ||||
|     throw std::runtime_error("Similarity2::Logmap: derivative not implemented"); | ||||
|   } | ||||
|  | @ -236,13 +236,14 @@ Vector4 Similarity2::Logmap(const Similarity2& S,  // | |||
| 
 | ||||
| Similarity2 Similarity2::Expmap(const Vector4& v,  //
 | ||||
|                                 OptionalJacobian<4, 4> Hm) { | ||||
|   const Vector2 t = v.head<2>(); | ||||
|   const Rot2 R = Rot2::Expmap(v.segment<1>(2)); | ||||
|   const double s = v[3]; | ||||
|   const Vector2 u = v.head<2>(); | ||||
|   const double theta = v[2]; | ||||
|   const double lambda = v[3]; | ||||
|   if (Hm) { | ||||
|     throw std::runtime_error("Similarity2::Expmap: derivative not implemented"); | ||||
|   } | ||||
|   return Similarity2(R, t, s); | ||||
|   const Matrix2 V = GetV(theta, lambda); | ||||
|   return Similarity2(Rot2::Expmap(v.segment<1>(2)), V * u, exp(lambda)); | ||||
| } | ||||
| 
 | ||||
| Matrix4 Similarity2::AdjointMap() const { | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue