Remove all nearZero paths
							parent
							
								
									bcfb7d8444
								
							
						
					
					
						commit
						d547fe2ec1
					
				|  | @ -93,15 +93,7 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) | |||
|   E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2; | ||||
| } | ||||
| 
 | ||||
| Matrix3 DexpFunctor::rightJacobian() const { | ||||
|   if (nearZero) { | ||||
|     return I_3x3 - B * W;  // + C * WW;
 | ||||
|   } else { | ||||
|     return I_3x3 - B * W + C * WW; | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const { | ||||
| Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const { | ||||
|   // Wv = omega x * v
 | ||||
|   const Vector3 Wv = gtsam::cross(omega, v); | ||||
|   if (H) { | ||||
|  | @ -113,8 +105,8 @@ Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const { | |||
|   return B * Wv; | ||||
| } | ||||
| 
 | ||||
| Vector3 DexpFunctor::doubleCross(const Vector3& v, | ||||
|                                  OptionalJacobian<3, 3> H) const { | ||||
| Vector3 DexpFunctor::doubleCrossC(const Vector3& v, | ||||
|                                   OptionalJacobian<3, 3> H) const { | ||||
|   // WWv = omega x (omega x * v)
 | ||||
|   Matrix3 doubleCrossJacobian; | ||||
|   const Vector3 WWv = | ||||
|  | @ -131,18 +123,12 @@ Vector3 DexpFunctor::doubleCross(const Vector3& v, | |||
| // Multiplies v with left Jacobian through vector operations only.
 | ||||
| Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, | ||||
|                                OptionalJacobian<3, 3> H2) const { | ||||
|   if (nearZero) { | ||||
|     if (H1) *H1 = 0.5 * skewSymmetric(v); | ||||
|     if (H2) *H2 = I_3x3 - 0.5 * W; | ||||
|     return v - 0.5 * gtsam::cross(omega, v); | ||||
|   } else { | ||||
|   Matrix3 D_BWv_omega, D_CWWv_omega; | ||||
|     const Vector3 BWv = cross(v, D_BWv_omega); | ||||
|     const Vector3 CWWv = doubleCross(v, D_CWWv_omega); | ||||
|     if (H1) *H1 = - D_BWv_omega + D_CWWv_omega; | ||||
|   const Vector3 BWv = crossB(v, D_BWv_omega); | ||||
|   const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega); | ||||
|   if (H1) *H1 = -D_BWv_omega + D_CWWv_omega; | ||||
|   if (H2) *H2 = rightJacobian(); | ||||
|   return v - BWv + CWWv; | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, | ||||
|  | @ -158,29 +144,15 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, | |||
|   return c; | ||||
| } | ||||
| 
 | ||||
| Matrix3 DexpFunctor::leftJacobian() const { | ||||
|   if (nearZero) { | ||||
|     return I_3x3 + 0.5 * W;  // + one_sixth * WW;
 | ||||
|   } else { | ||||
|     return I_3x3 + B * W + C * WW; | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, | ||||
|                                        OptionalJacobian<3, 3> H1, | ||||
|                                        OptionalJacobian<3, 3> H2) const { | ||||
|   if (nearZero) { | ||||
|     if (H1) *H1 = - 0.5 * skewSymmetric(v); | ||||
|     if (H2) *H2 = I_3x3 + 0.5 * W; | ||||
|     return v + 0.5 * gtsam::cross(omega, v); | ||||
|   } else { | ||||
|     Matrix3 D_BWv_omega, D_CWWv_omega; | ||||
|     const Vector3 BWv = cross(v, D_BWv_omega); | ||||
|     const Vector3 CWWv = doubleCross(v, D_CWWv_omega); | ||||
|     if (H1) *H1 = D_BWv_omega + D_CWWv_omega; | ||||
|     if (H2) *H2 = leftJacobian(); | ||||
|     return v + BWv + CWWv; | ||||
|   } | ||||
|   Matrix3 D_BWv_omega, D_CWWv_omega; | ||||
|   const Vector3 BWv = crossB(v, D_BWv_omega); | ||||
|   const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega); | ||||
|   if (H1) *H1 = D_BWv_omega + D_CWWv_omega; | ||||
|   if (H2) *H2 = leftJacobian(); | ||||
|   return v + BWv + CWWv; | ||||
| } | ||||
| 
 | ||||
| }  // namespace so3
 | ||||
|  |  | |||
|  | @ -155,7 +155,7 @@ class GTSAM_EXPORT ExpmapFunctor { | |||
| }; | ||||
| 
 | ||||
| /// Functor that implements Exponential map *and* its derivatives
 | ||||
| class DexpFunctor : public ExpmapFunctor { | ||||
| class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { | ||||
|  protected: | ||||
|   const Vector3 omega; | ||||
|   double C;  // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0
 | ||||
|  | @ -164,15 +164,8 @@ class DexpFunctor : public ExpmapFunctor { | |||
|   double E;  // (B - 3.0 * C) / theta2 or -1/60 for theta->0
 | ||||
| 
 | ||||
|  public: | ||||
|   /// Computes B * (omega x v).
 | ||||
|   Vector3 cross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; | ||||
| 
 | ||||
|   /// Computes C * (omega x (omega x v)).
 | ||||
|   Vector3 doubleCross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; | ||||
| 
 | ||||
|   /// Constructor with element of Lie algebra so(3)
 | ||||
|   GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, | ||||
|                                     bool nearZeroApprox = false); | ||||
|   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,
 | ||||
|  | @ -180,28 +173,31 @@ 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)
 | ||||
|   GTSAM_EXPORT Matrix3 rightJacobian() const; | ||||
| 
 | ||||
|   /// differential of expmap == right Jacobian
 | ||||
|   GTSAM_EXPORT 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; | ||||
| 
 | ||||
|   /// Multiplies with dexp().inverse(), with optional derivatives
 | ||||
|   GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v, | ||||
|                                     OptionalJacobian<3, 3> H1 = {}, | ||||
|                                     OptionalJacobian<3, 3> H2 = {}) const; | ||||
|   Matrix3 rightJacobian() const { return I_3x3 - B * W + C * WW; } | ||||
| 
 | ||||
|   // Compute the left Jacobian for Exponential map in SO(3)
 | ||||
|   GTSAM_EXPORT Matrix3 leftJacobian() const; | ||||
|   Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; } | ||||
| 
 | ||||
|   /// differential of expmap == right Jacobian
 | ||||
|   inline Matrix3 dexp() const { return rightJacobian(); } | ||||
| 
 | ||||
|   /// Computes B * (omega x v).
 | ||||
|   Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; | ||||
| 
 | ||||
|   /// Computes C * (omega x (omega x v)).
 | ||||
|   Vector3 doubleCrossC(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; | ||||
| 
 | ||||
|   /// Multiplies with dexp(), with optional derivatives
 | ||||
|   Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, | ||||
|                     OptionalJacobian<3, 3> H2 = {}) const; | ||||
| 
 | ||||
|   /// Multiplies with dexp().inverse(), with optional derivatives
 | ||||
|   Vector3 applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, | ||||
|                        OptionalJacobian<3, 3> H2 = {}) const; | ||||
| 
 | ||||
|   /// Multiplies with leftJacobian(), with optional derivatives
 | ||||
|   GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v, | ||||
|                                          OptionalJacobian<3, 3> H1 = {}, | ||||
|                                          OptionalJacobian<3, 3> H2 = {}) const; | ||||
|   Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, | ||||
|                             OptionalJacobian<3, 3> H2 = {}) const; | ||||
| 
 | ||||
|  protected: | ||||
|   static constexpr double one_sixth = 1.0 / 6.0; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue