12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Remove all nearZero paths

release/4.3a0
Frank Dellaert 2024-12-15 23:43:59 -05:00
parent bcfb7d8444
commit d547fe2ec1
2 changed files with 35 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
gtsam/geometry/SO3.cpp
View File

@ -93,15 +93,7 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2; E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2;
} }
Matrix3 DexpFunctor::rightJacobian() const { Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) 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 {
// Wv = omega x * v // Wv = omega x * v
const Vector3 Wv = gtsam::cross(omega, v); const Vector3 Wv = gtsam::cross(omega, v);
if (H) { if (H) {
@ -113,8 +105,8 @@ Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const {
return B * Wv; return B * Wv;
} }
Vector3 DexpFunctor::doubleCross(const Vector3& v, Vector3 DexpFunctor::doubleCrossC(const Vector3& v,
OptionalJacobian<3, 3> H) const { OptionalJacobian<3, 3> H) const {
// WWv = omega x (omega x * v) // WWv = omega x (omega x * v)
Matrix3 doubleCrossJacobian; Matrix3 doubleCrossJacobian;
const Vector3 WWv = const Vector3 WWv =
@ -131,18 +123,12 @@ Vector3 DexpFunctor::doubleCross(const Vector3& v,
// Multiplies v with left Jacobian through vector operations only. // Multiplies v with left Jacobian through vector operations only.
Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const { 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; Matrix3 D_BWv_omega, D_CWWv_omega;
const Vector3 BWv = cross(v, D_BWv_omega); const Vector3 BWv = crossB(v, D_BWv_omega);
const Vector3 CWWv = doubleCross(v, D_CWWv_omega); const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega);
if (H1) *H1 = - D_BWv_omega + D_CWWv_omega; if (H1) *H1 = -D_BWv_omega + D_CWWv_omega;
if (H2) *H2 = rightJacobian(); if (H2) *H2 = rightJacobian();
return v - BWv + CWWv; return v - BWv + CWWv;
}
} }
Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, 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; 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, Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const { OptionalJacobian<3, 3> H2) const {
if (nearZero) { Matrix3 D_BWv_omega, D_CWWv_omega;
if (H1) *H1 = - 0.5 * skewSymmetric(v); const Vector3 BWv = crossB(v, D_BWv_omega);
if (H2) *H2 = I_3x3 + 0.5 * W; const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega);
return v + 0.5 * gtsam::cross(omega, v); if (H1) *H1 = D_BWv_omega + D_CWWv_omega;
} else { if (H2) *H2 = leftJacobian();
Matrix3 D_BWv_omega, D_CWWv_omega; return v + BWv + CWWv;
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;
}
} }
} // namespace so3 } // namespace so3

50
gtsam/geometry/SO3.h
View File

@ -155,7 +155,7 @@ class GTSAM_EXPORT ExpmapFunctor {
}; };
/// Functor that implements Exponential map *and* its derivatives /// Functor that implements Exponential map *and* its derivatives
class DexpFunctor : public ExpmapFunctor { class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
protected: protected:
const Vector3 omega; const Vector3 omega;
double C; // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0 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 double E; // (B - 3.0 * C) / theta2 or -1/60 for theta->0
public: 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) /// Constructor with element of Lie algebra so(3)
GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
bool nearZeroApprox = false);
// NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
// (10.86) and following equations in G.S. Chirikjian, "Stochastic Models, // (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) // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
// This maps a perturbation v in the tangent space to // This maps a perturbation v in the tangent space to
// a perturbation on the manifold Expmap(dexp * v) // a perturbation on the manifold Expmap(dexp * v)
GTSAM_EXPORT Matrix3 rightJacobian() const; Matrix3 rightJacobian() const { return I_3x3 - B * W + C * WW; }
/// 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;
// Compute the left Jacobian for Exponential map in SO(3) // 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 /// Multiplies with leftJacobian(), with optional derivatives
GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v, Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const;
OptionalJacobian<3, 3> H2 = {}) const;
protected: protected:
static constexpr double one_sixth = 1.0 / 6.0; static constexpr double one_sixth = 1.0 / 6.0;