Simplify functor according to Eade
							parent
							
								
									bb7b6b39c7
								
							
						
					
					
						commit
						440c3ea64b
					
				|  | @ -48,12 +48,15 @@ GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, | |||
| } | ||||
| 
 | ||||
| void ExpmapFunctor::init(bool nearZeroApprox) { | ||||
|   WW = W * W; | ||||
|   nearZero = | ||||
|       nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon()); | ||||
|   if (!nearZero) { | ||||
|     sin_theta = std::sin(theta); | ||||
|     const double s2 = std::sin(theta / 2.0); | ||||
|     one_minus_cos = 2.0 * s2 * s2;  // numerically better than [1 - cos(theta)]
 | ||||
|     A = sin_theta / theta; | ||||
|     B = one_minus_cos / theta2; | ||||
|   } | ||||
| } | ||||
| 
 | ||||
|  | @ -62,39 +65,39 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) | |||
|   const double wx = omega.x(), wy = omega.y(), wz = omega.z(); | ||||
|   W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; | ||||
|   init(nearZeroApprox); | ||||
|   if (!nearZero) { | ||||
|     K = W / theta; | ||||
|     KK = K * K; | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, | ||||
|                              bool nearZeroApprox) | ||||
|     : theta2(angle * angle), theta(angle) { | ||||
|   const double ax = axis.x(), ay = axis.y(), az = axis.z(); | ||||
|   K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; | ||||
|   W = K * angle; | ||||
|   W << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; | ||||
|   W *= angle; | ||||
|   init(nearZeroApprox); | ||||
|   if (!nearZero) { | ||||
|     KK = K * K; | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| SO3 ExpmapFunctor::expmap() const { | ||||
|   if (nearZero) | ||||
|     return SO3(I_3x3 + W); | ||||
|     return SO3(I_3x3 + W + 0.5 * WW); | ||||
|   else | ||||
|     return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK); | ||||
|     return SO3(I_3x3 + A * W + B * WW); | ||||
| } | ||||
| 
 | ||||
| Matrix3 DexpFunctor::leftJacobian() const { | ||||
|   if (nearZero) { | ||||
|     return I_3x3 + 0.5 * W + one_sixth * WW; | ||||
|   } else { | ||||
|     return I_3x3 + B * W + C * WW; | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) | ||||
|     : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { | ||||
|   if (nearZero) { | ||||
|     dexp_ = I_3x3 - 0.5 * W; | ||||
|     rightJacobian_ = I_3x3 - 0.5 * W + one_sixth * WW; | ||||
|   } else { | ||||
|     a = one_minus_cos / theta; | ||||
|     b = 1.0 - sin_theta / theta; | ||||
|     dexp_ = I_3x3 - a * K + b * KK; | ||||
|     C = (1 - A) / theta2; | ||||
|     rightJacobian_ = I_3x3 - B * W + C * WW; | ||||
|   } | ||||
| } | ||||
| 
 | ||||
|  | @ -105,21 +108,23 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, | |||
|       *H1 = 0.5 * skewSymmetric(v); | ||||
|     } else { | ||||
|       // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
 | ||||
|       const Vector3 Kv = K * v; | ||||
|       double a = B * theta; | ||||
|       double b = C * theta2; | ||||
|       const Vector3 Kv = W * v / theta; | ||||
|       const double Da = (sin_theta - 2.0 * a) / theta2; | ||||
|       const double Db = (one_minus_cos - 3.0 * b) / theta2; | ||||
|       *H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() - | ||||
|       *H1 = (Db * W / theta - Da * I_3x3) * Kv * omega.transpose() - | ||||
|             skewSymmetric(Kv * b / theta) + | ||||
|             (a * I_3x3 - b * K) * skewSymmetric(v / theta); | ||||
|             (a * I_3x3 - b * W / theta) * skewSymmetric(v / theta); | ||||
|     } | ||||
|   } | ||||
|   if (H2) *H2 = dexp_; | ||||
|   return dexp_ * v; | ||||
|   if (H2) *H2 = rightJacobian_; | ||||
|   return rightJacobian_ * v; | ||||
| } | ||||
| 
 | ||||
| Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, | ||||
|                                   OptionalJacobian<3, 3> H2) const { | ||||
|   const Matrix3 invDexp = dexp_.inverse(); | ||||
|   const Matrix3 invDexp = rightJacobian_.inverse(); | ||||
|   const Vector3 c = invDexp * v; | ||||
|   if (H1) { | ||||
|     Matrix3 D_dexpv_omega; | ||||
|  |  | |||
|  | @ -136,7 +136,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); | |||
| class GTSAM_EXPORT ExpmapFunctor { | ||||
|  protected: | ||||
|   const double theta2; | ||||
|   Matrix3 W, K, KK; | ||||
|   Matrix3 W, WW; | ||||
|   double A, B; // Ethan Eade's constants
 | ||||
|   bool nearZero; | ||||
|   double theta, sin_theta, one_minus_cos;  // only defined if !nearZero
 | ||||
| 
 | ||||
|  | @ -155,13 +156,16 @@ class GTSAM_EXPORT ExpmapFunctor { | |||
| 
 | ||||
| /// Functor that implements Exponential map *and* its derivatives
 | ||||
| class DexpFunctor : public ExpmapFunctor { | ||||
|  protected: | ||||
|   static constexpr double one_sixth = 1.0 / 6.0; | ||||
|   const Vector3 omega; | ||||
|   double a, b; | ||||
|   Matrix3 dexp_; | ||||
|   double C;  // Ethan Eade's C constant
 | ||||
|   Matrix3 rightJacobian_; | ||||
| 
 | ||||
|  public: | ||||
|   /// Constructor with element of Lie algebra so(3)
 | ||||
|   GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); | ||||
|   GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, | ||||
|                                     bool nearZeroApprox = false); | ||||
| 
 | ||||
|   // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
 | ||||
|   // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
 | ||||
|  | @ -169,16 +173,21 @@ class DexpFunctor : public ExpmapFunctor { | |||
|   //   expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
 | ||||
|   // This maps a perturbation v in the tangent space to
 | ||||
|   // a perturbation on the manifold Expmap(dexp * v) */
 | ||||
|   const Matrix3& dexp() const { return dexp_; } | ||||
|   const Matrix3& dexp() const { return rightJacobian_; } | ||||
| 
 | ||||
|   /// Multiplies with dexp(), with optional derivatives
 | ||||
|   GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, | ||||
|                     OptionalJacobian<3, 3> H2 = {}) const; | ||||
|   GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, | ||||
|                                  OptionalJacobian<3, 3> H1 = {}, | ||||
|                                  OptionalJacobian<3, 3> H2 = {}) const; | ||||
| 
 | ||||
|   /// Multiplies with dexp().inverse(), with optional derivatives
 | ||||
|   GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v, | ||||
|                        OptionalJacobian<3, 3> H1 = {}, | ||||
|                        OptionalJacobian<3, 3> H2 = {}) const; | ||||
|                                     OptionalJacobian<3, 3> H1 = {}, | ||||
|                                     OptionalJacobian<3, 3> H2 = {}) const; | ||||
| 
 | ||||
|   // Compute the left Jacobian for Exponential map in SO(3)
 | ||||
|   // Note precomputed, as not used as much
 | ||||
|   Matrix3 leftJacobian() const; | ||||
| }; | ||||
| }  //  namespace so3
 | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue