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

Deprecated dexp() and dexpInv, inlined crossB and doubleCrossC

release/4.3a0
Frank Dellaert 2025-04-18 08:59:24 -04:00
parent 2df2ff7567
commit 52efba91d8
5 changed files with 82 additions and 144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
gtsam/geometry/SO3.cpp
View File

@ -144,71 +144,60 @@ DexpFunctor::DexpFunctor(const Vector3& omega, double nearZeroThresholdSq, doubl
DexpFunctor::DexpFunctor(const Vector3& omega) DexpFunctor::DexpFunctor(const Vector3& omega)
: DexpFunctor(omega, kNearZeroThresholdSq, kNearPiThresholdSq) {} : DexpFunctor(omega, kNearZeroThresholdSq, kNearPiThresholdSq) {}
Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const {
// Wv = omega x v
const Vector3 Wv = gtsam::cross(omega, v);
if (H) {
// Apply product rule to (B Wv)
// - E * omega.transpose() is 1x3 Jacobian of B with respect to omega
// - skewSymmetric(v) is 3x3 Jacobian of Wv = gtsam::cross(omega, v)
*H = - Wv * E * omega.transpose() - B * skewSymmetric(v);
}
return B * Wv;
}
Vector3 DexpFunctor::doubleCrossC(const Vector3& v,
OptionalJacobian<3, 3> H) const {
// WWv = omega x (omega x * v)
Matrix3 doubleCrossJacobian;
const Vector3 WWv =
gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr);
if (H) {
// Apply product rule to (C WWv)
// - F * omega.transpose() is 1x3 Jacobian of C with respect to omega
// doubleCrossJacobian is 3x3 Jacobian of WWv = gtsam::doubleCross(omega, v)
*H = - WWv * F * omega.transpose() + C * doubleCrossJacobian;
}
return C * WWv;
}
// 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::applyRightJacobian(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const { OptionalJacobian<3, 3> H2) const {
Matrix3 D_BWv_w, D_CWWv_w; const Vector3 Wv = gtsam::cross(omega, v);
const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr);
const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr); Matrix3 WWv_H_w;
if (H1) *H1 = -D_BWv_w + D_CWWv_w; const Vector3 WWv = gtsam::doubleCross(omega, v, H1 ? &WWv_H_w : nullptr);
if (H1) {
// - E * omega.transpose() is 1x3 Jacobian of B with respect to omega
Matrix3 BWv_H_w = -Wv * E * omega.transpose() - B * skewSymmetric(v);
// - F * omega.transpose() is 1x3 Jacobian of C with respect to omega
Matrix3 CWWv_H_w = -WWv * F * omega.transpose() + C * WWv_H_w;
*H1 = -BWv_H_w + CWWv_H_w;
}
if (H2) *H2 = rightJacobian(); if (H2) *H2 = rightJacobian();
return v - BWv + CWWv; return v - B * Wv + C * WWv;
} }
Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, Vector3 DexpFunctor::applyRightJacobianInverse(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const { OptionalJacobian<3, 3> H2) const {
const Matrix3 invJr = rightJacobianInverse(); const Matrix3 invJr = rightJacobianInverse();
const Vector3 c = invJr * v; const Vector3 c = invJr * v;
if (H1) { if (H1) {
Matrix3 H; Matrix3 H;
applyDexp(c, H); // get derivative H of forward mapping applyRightJacobian(c, H); // get derivative H of forward mapping
*H1 = -invJr * H; *H1 = -invJr * H;
} }
if (H2) *H2 = invJr; if (H2) *H2 = invJr;
return c; return c;
} }
Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, Vector3 so3::DexpFunctor::applyLeftJacobian(const Vector3& v,
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const {
OptionalJacobian<3, 3> H2) const { const Vector3 Wv = gtsam::cross(omega, v);
Matrix3 D_BWv_w, D_CWWv_w;
const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr); Matrix3 WWv_H_w;
const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr); const Vector3 WWv = gtsam::doubleCross(omega, v, H1 ? &WWv_H_w : nullptr);
if (H1) *H1 = D_BWv_w + D_CWWv_w;
if (H1) {
// - E * omega.transpose() is 1x3 Jacobian of B with respect to omega
Matrix3 BWv_H_w = -Wv * E * omega.transpose() - B * skewSymmetric(v);
// - F * omega.transpose() is 1x3 Jacobian of C with respect to omega
Matrix3 CWWv_H_w = -WWv * F * omega.transpose() + C * WWv_H_w;
*H1 = BWv_H_w + CWWv_H_w;
}
if (H2) *H2 = leftJacobian(); if (H2) *H2 = leftJacobian();
return v + BWv + CWWv; return v + B * Wv + C * WWv;
} }
Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v, Vector3 so3::DexpFunctor::applyLeftJacobianInverse(const Vector3& v,
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const {
OptionalJacobian<3, 3> H2) const {
const Matrix3 invJl = leftJacobianInverse(); const Matrix3 invJl = leftJacobianInverse();
const Vector3 c = invJl * v; const Vector3 c = invJl * v;
if (H1) { if (H1) {
@ -284,34 +273,23 @@ template <>
GTSAM_EXPORT GTSAM_EXPORT
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
so3::DexpFunctor local(omega); so3::DexpFunctor local(omega);
if (H) *H = local.dexp(); if (H) *H = local.rightJacobian();
return SO3(local.expmap()); return SO3(local.expmap());
} }
template <> template <>
GTSAM_EXPORT GTSAM_EXPORT
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
return so3::DexpFunctor(omega).dexp(); return so3::DexpFunctor(omega).rightJacobian();
} }
//****************************************************************************** //******************************************************************************
/* Right Jacobian for Log map in SO(3) - equation (10.86) and following
equations in G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie
Groups", Volume 2, 2008.
logmap( Rhat * expmap(omega) ) \approx logmap(Rhat) + Jrinv * omega
where Jrinv = LogmapDerivative(omega). This maps a perturbation on the
manifold (expmap(omega)) to a perturbation in the tangent space (Jrinv *
omega)
*/
template <> template <>
GTSAM_EXPORT GTSAM_EXPORT
Matrix3 SO3::LogmapDerivative(const Vector3& omega) { Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
return so3::DexpFunctor(omega).rightJacobianInverse(); return so3::DexpFunctor(omega).rightJacobianInverse();
} }
//******************************************************************************
template <> template <>
GTSAM_EXPORT GTSAM_EXPORT
Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {

39
gtsam/geometry/SO3.h
View File

@ -192,40 +192,35 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
// Compute the left Jacobian for Exponential map in SO(3) // Compute the left Jacobian for Exponential map in SO(3)
Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; } Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; }
/// Differential of expmap == right Jacobian
inline Matrix3 dexp() const { return rightJacobian(); }
/// Inverse of right Jacobian /// Inverse of right Jacobian
Matrix3 rightJacobianInverse() const { return I_3x3 + 0.5 * W + D * WW; } Matrix3 rightJacobianInverse() const { return I_3x3 + 0.5 * W + D * WW; }
// Inverse of left Jacobian // Inverse of left Jacobian
Matrix3 leftJacobianInverse() const { return I_3x3 - 0.5 * W + D * WW; } Matrix3 leftJacobianInverse() const { return I_3x3 - 0.5 * W + D * WW; }
/// Synonym for rightJacobianInverse /// Multiplies with rightJacobian(), with optional derivatives
inline Matrix3 invDexp() const { return rightJacobianInverse(); } Vector3 applyRightJacobian(const Vector3& v,
OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const;
/// Computes B * (omega x v). /// Multiplies with rightJacobian().inverse(), with optional derivatives
Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; Vector3 applyRightJacobianInverse(const Vector3& v,
OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) 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
Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, Vector3 applyLeftJacobian(const Vector3& v,
OptionalJacobian<3, 3> H2 = {}) const; OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const;
/// Multiplies with leftJacobianInverse(), with optional derivatives /// Multiplies with leftJacobianInverse(), with optional derivatives
Vector3 applyLeftJacobianInverse(const Vector3& v, Vector3 applyLeftJacobianInverse(const Vector3& v,
OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const;
OptionalJacobian<3, 3> H2 = {}) const;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
/// @deprecated: use rightJacobian
inline Matrix3 dexp() const { return rightJacobian(); }
/// @deprecated: use rightJacobianInverse
inline Matrix3 invDexp() const { return rightJacobianInverse(); }
#endif
}; };
} // namespace so3 } // namespace so3

7
gtsam/geometry/geometry.i
View File

@ -224,9 +224,10 @@ namespace so3 {
gtsam::Matrix3 leftJacobian() const; gtsam::Matrix3 leftJacobian() const;
gtsam::Matrix3 rightJacobianInverse() const; gtsam::Matrix3 rightJacobianInverse() const;
gtsam::Matrix3 leftJacobianInverse() const; gtsam::Matrix3 leftJacobianInverse() const;
gtsam::Vector3 applyRightJacobian(const gtsam::Vector3& v) const;
gtsam::Matrix3 dexp() const; gtsam::Vector3 applyRightJacobianInverse(const gtsam::Vector3& v) const;
gtsam::Matrix3 invDexp() const; gtsam::Vector3 applyLeftJacobian(const gtsam::Vector3& v) const;
gtsam::Vector3 applyLeftJacobianInverse(const gtsam::Vector3& v) const;
}; };
} }

76
gtsam/geometry/tests/testSO3.cpp
View File

@ -355,76 +355,40 @@ TEST(SO3, JacobianInverses) {
} }
//****************************************************************************** //******************************************************************************
TEST(SO3, CrossB) { TEST(SO3, ApplyRightJacobian) {
Matrix aH1;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).crossB(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5);
for (const Vector3& v : test_cases::vs) {
local.crossB(v, aH1);
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
}
}
}
}
//******************************************************************************
TEST(SO3, DoubleCrossC) {
Matrix aH1;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).doubleCrossC(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5);
for (const Vector3& v : test_cases::vs) {
local.doubleCrossC(v, aH1);
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
}
}
}
}
//******************************************************************************
TEST(SO3, ApplyDexp) {
Matrix aH1, aH2; Matrix aH1, aH2;
for (bool nearZero : {true, false}) { for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f = std::function<Vector3(const Vector3&, const Vector3&)> f =
[nearZero](const Vector3& omega, const Vector3& v) { [nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyDexp(v); return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyRightJacobian(v);
}; };
for (const Vector3& omega : test_cases::omegas(nearZero)) { for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5); so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5);
for (const Vector3& v : test_cases::vs) { for (const Vector3& v : test_cases::vs) {
EXPECT(assert_equal(Vector3(local.dexp() * v), EXPECT(assert_equal(Vector3(local.rightJacobian() * v),
local.applyDexp(v, aH1, aH2))); local.applyRightJacobian(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(local.dexp(), aH2)); EXPECT(assert_equal(local.rightJacobian(), aH2));
} }
} }
} }
} }
//****************************************************************************** //******************************************************************************
TEST(SO3, ApplyInvDexp) { TEST(SO3, ApplyRightJacobianInverse) {
Matrix aH1, aH2; Matrix aH1, aH2;
for (bool nearZero : {true, false}) { for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f = std::function<Vector3(const Vector3&, const Vector3&)> f =
[nearZero](const Vector3& omega, const Vector3& v) { [nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyInvDexp(v); return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyRightJacobianInverse(v);
}; };
for (const Vector3& omega : test_cases::omegas(nearZero)) { for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5); so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5);
Matrix invJr = local.rightJacobianInverse(); Matrix invJr = local.rightJacobianInverse();
for (const Vector3& v : test_cases::vs) { for (const Vector3& v : test_cases::vs) {
EXPECT( EXPECT(
assert_equal(Vector3(invJr * v), local.applyInvDexp(v, aH1, aH2))); assert_equal(Vector3(invJr * v), local.applyRightJacobianInverse(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(invJr, aH2)); EXPECT(assert_equal(invJr, aH2));
@ -438,14 +402,14 @@ TEST(SO3, ApplyLeftJacobian) {
Matrix aH1, aH2; Matrix aH1, aH2;
for (bool nearZero : {true, false}) { for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f = std::function<Vector3(const Vector3&, const Vector3&)> f =
[nearZero](const Vector3& omega, const Vector3& v) { [nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyLeftJacobian(v); return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyLeftJacobian(v);
}; };
for (const Vector3& omega : test_cases::omegas(nearZero)) { for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5); so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5);
for (const Vector3& v : test_cases::vs) { for (const Vector3& v : test_cases::vs) {
EXPECT(assert_equal(Vector3(local.leftJacobian() * v), EXPECT(assert_equal(Vector3(local.leftJacobian() * v),
local.applyLeftJacobian(v, aH1, aH2))); local.applyLeftJacobian(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(local.leftJacobian(), aH2)); EXPECT(assert_equal(local.leftJacobian(), aH2));
@ -459,15 +423,15 @@ TEST(SO3, ApplyLeftJacobianInverse) {
Matrix aH1, aH2; Matrix aH1, aH2;
for (bool nearZero : {true, false}) { for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f = std::function<Vector3(const Vector3&, const Vector3&)> f =
[nearZero](const Vector3& omega, const Vector3& v) { [nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyLeftJacobianInverse(v); return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyLeftJacobianInverse(v);
}; };
for (const Vector3& omega : test_cases::omegas(nearZero)) { for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5); so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5);
Matrix invJl = local.leftJacobianInverse(); Matrix invJl = local.leftJacobianInverse();
for (const Vector3& v : test_cases::vs) { for (const Vector3& v : test_cases::vs) {
EXPECT(assert_equal(Vector3(invJl * v), EXPECT(assert_equal(Vector3(invJl * v),
local.applyLeftJacobianInverse(v, aH1, aH2))); local.applyLeftJacobianInverse(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(invJl, aH2)); EXPECT(assert_equal(invJl, aH2));

4
gtsam/navigation/TangentPreintegration.cpp
View File

@ -66,7 +66,7 @@ Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body,
// Calculate exact mean propagation // Calculate exact mean propagation
Matrix3 w_tangent_H_theta, invH; Matrix3 w_tangent_H_theta, invH;
const Vector3 w_tangent = // angular velocity mapped back to tangent space const Vector3 w_tangent = // angular velocity mapped back to tangent space
local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0); local.applyRightJacobianInverse(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
const Rot3 R(local.expmap()); // nRb: rotation of body in nav frame const Rot3 R(local.expmap()); // nRb: rotation of body in nav frame
const Vector3 a_nav = R * a_body; const Vector3 a_nav = R * a_body;
const double dt22 = 0.5 * dt * dt; const double dt22 = 0.5 * dt * dt;
@ -79,7 +79,7 @@ Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body,
if (A) { if (A) {
// Exact derivative of R*a with respect to theta: // Exact derivative of R*a with respect to theta:
const Matrix3 a_nav_H_theta = R.matrix() * skewSymmetric(-a_body) * local.dexp(); const Matrix3 a_nav_H_theta = R.matrix() * skewSymmetric(-a_body) * local.rightJacobian();
A->setIdentity(); A->setIdentity();
A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta