Skip to content

Commit

Permalink
Remove all nearZero paths
Browse files Browse the repository at this point in the history
  • Loading branch information
@dellaert
dellaert committed Dec 16, 2024
1 parent bcfb7d8 commit d547fe2
Show file tree
Hide file tree
Showing 2 changed files with 31 additions and 63 deletions.
52 changes: 12 additions & 40 deletions gtsam/geometry/SO3.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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 =
Expand All @@ -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,
Expand All @@ -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
Expand Down
42 changes: 19 additions & 23 deletions gtsam/geometry/SO3.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -164,44 +164,40 @@ 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,
// Information Theory, and Lie Groups", Volume 2, 2008.
// 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;
Matrix3 rightJacobian() const { return I_3x3 - B * W + C * WW; }

// Compute the left Jacobian for Exponential map in SO(3)
Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; }

/// differential of expmap == right Jacobian
GTSAM_EXPORT Matrix3 dexp() const { return rightJacobian(); }
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
GTSAM_EXPORT Vector3 applyDexp(const Vector3& v,
OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
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;

// Compute the left Jacobian for Exponential map in SO(3)
GTSAM_EXPORT Matrix3 leftJacobian() const;
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;
Expand Down

0 comments on commit d547fe2

Please sign in to comment.