applyLeftJacobian

release/4.3a0
Frank Dellaert 2024-12-15 00:50:38 -05:00
parent 78f17aabc4
commit c7f651d98d
3 changed files with 74 additions and 23 deletions

View File

@ -26,6 +26,7 @@
#include <Eigen/SVD>
#include <cmath>
#include <iostream>
#include <limits>
namespace gtsam {
@ -98,35 +99,47 @@ Matrix3 DexpFunctor::rightJacobian() const {
}
}
// Derivative of w x w x v in w:
static Matrix3 doubleCrossJacobian(const Vector3& w, const Vector3& v) {
return v.dot(w) * I_3x3 + w * v.transpose() - 2 * v * w.transpose();
Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const {
// Wv = omega x * v
const Vector3 Wv = gtsam::cross(omega, v);
if (H) {
// 1x3 Jacobian of B with respect to omega
const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose();
// Apply product rule:
*H = Wv * HB - B * skewSymmetric(v);
}
return B * Wv;
}
Vector3 DexpFunctor::doubleCross(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) {
// 1x3 Jacobian of C with respect to omega
const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose();
// Apply product rule:
*H = WWv * HC + C * doubleCrossJacobian;
}
return C * WWv;
}
// Multiplies v with left Jacobian through vector operations only.
// When Jacobian H1 wrt omega is requested, product rule is invoked twice, once
// for (B * Wv) and (C * WWv). The Jacobian H2 wrt v is just the right Jacobian.
Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
// Wv = omega x * v, with Jacobian -V in w
const Vector3 Wv = gtsam::cross(omega, v);
if (nearZero) {
if (H1) *H1 = 0.5 * skewSymmetric(v);
if (H2) *H2 = I_3x3 - 0.5 * W;
return v - 0.5 * Wv;
return v - 0.5 * gtsam::cross(omega, v);
} else {
// WWv = omega x (omega x * v), with Jacobian doubleCrossJacobian
const Vector3 WWv = gtsam::cross(omega, Wv);
if (H1) {
// 1x3 Jacobians of B and C with respect to theta
const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose();
const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose();
*H1 = -Wv * HB + B * skewSymmetric(v) +
C * doubleCrossJacobian(omega, v) + WWv * HC;
}
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 = rightJacobian();
return v - B * Wv + C * WWv;
return v - BWv + CWWv;
}
}
@ -154,10 +167,18 @@ Matrix3 DexpFunctor::leftJacobian() const {
Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
const Matrix3 Jw = leftJacobian();
if (H1) H1->setZero();
if (H2) *H2 = Jw;
return Jw * v;
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;
}
}
} // namespace so3

View File

@ -160,6 +160,12 @@ class DexpFunctor : public ExpmapFunctor {
const Vector3 omega;
double C; // Ethan Eade's C constant
/// 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;
public:
/// Constructor with element of Lie algebra so(3)
GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega,

View File

@ -19,6 +19,7 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/testLie.h>
#include <gtsam/geometry/SO3.h>
#include <iostream>
using namespace std::placeholders;
using namespace std;
@ -326,6 +327,29 @@ TEST(SO3, ApplyDexp) {
}
}
//******************************************************************************
TEST(SO3, ApplyLeftJacobian) {
Matrix aH1, aH2;
for (bool nearZeroApprox : {false, true}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZeroApprox).applyLeftJacobian(v);
};
for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
so3::DexpFunctor local(omega, nearZeroApprox);
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
Vector3(0.4, 0.3, 0.2)}) {
CHECK(assert_equal(Vector3(local.leftJacobian() * v),
local.applyLeftJacobian(v, aH1, aH2)));
CHECK(assert_equal(numericalDerivative21(f, omega, v), aH1));
CHECK(assert_equal(numericalDerivative22(f, omega, v), aH2));
CHECK(assert_equal(local.leftJacobian(), aH2));
}
}
}
}
//******************************************************************************
TEST(SO3, ApplyInvDexp) {
Matrix aH1, aH2;