Skip to content

Commit

Permalink
Merge pull request #1932 from borglab/feature/leftRightJacobians
Browse files Browse the repository at this point in the history 
Left and Right SO(3) Jacobians
  • Loading branch information
@dellaert
dellaert authored Dec 16, 2024
2 parents bb7b6b3 + 0c1f087 commit 5125844
Show file tree
Hide file tree
Showing 8 changed files with 271 additions and 126 deletions.
10 changes: 10 additions & 0 deletions gtsam/geometry/Point3.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,16 @@ Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1,
p.x() * q.y() - p.y() * q.x());
}

Point3 doubleCross(const Point3 &p, const Point3 &q, //
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) {
if (H1) *H1 = q.dot(p) * I_3x3 + p * q.transpose() - 2 * q * p.transpose();
if (H2) {
const Matrix3 W = skewSymmetric(p);
*H2 = W * W;
}
return gtsam::cross(p, gtsam::cross(p, q));
}

double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) {
if (H1) *H1 << q.x(), q.y(), q.z();
Expand Down
7 changes: 6 additions & 1 deletion gtsam/geometry/Point3.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,11 +55,16 @@ GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = {});
/// normalize, with optional Jacobian
GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = {});

/// cross product @return this x q
/// cross product @return p x q
GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q,
OptionalJacobian<3, 3> H_p = {},
OptionalJacobian<3, 3> H_q = {});

/// double cross product @return p x (p x q)
GTSAM_EXPORT Point3 doubleCross(const Point3& p, const Point3& q,
OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {});

/// dot product
GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H_p = {},
Expand Down
65 changes: 31 additions & 34 deletions gtsam/geometry/Pose3.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -168,14 +168,14 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
const Vector3 w = xi.head<3>(), v = xi.tail<3>();

// Compute rotation using Expmap
Rot3 R = Rot3::Expmap(w);
Matrix3 Jw;
Rot3 R = Rot3::Expmap(w, Hxi ? &Jw : nullptr);

// Compute translation and optionally its Jacobian in w
Matrix3 Q;
const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr, R);

if (Hxi) {
const Matrix3 Jw = Rot3::ExpmapDerivative(w);
*Hxi << Jw, Z_3x3,
Q, Jw;
}
Expand Down Expand Up @@ -238,14 +238,37 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
#endif
}

/* ************************************************************************* */
namespace pose3 {
struct GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
// Constant used in computeQ
double F; // (B - 0.5) / theta2 or -1/24 for theta->0

ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
: so3::DexpFunctor(omega, nearZeroApprox) {
F = nearZero ? _one_twenty_fourth : (B - 0.5) / theta2;
}

// Compute the bottom-left 3x3 block of the SE(3) Expmap derivative
// TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand
// how to compute mess below from applyLeftJacobian derivatives in w and v.
Matrix3 computeQ(const Vector3& v) const {
const Matrix3 V = skewSymmetric(v);
const Matrix3 WVW = W * V * W;
return -0.5 * V + C * (W * V + V * W - WVW) +
F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW);
}

static constexpr double _one_twenty_fourth = - 1.0 / 24.0;
};
} // namespace pose3

/* ************************************************************************* */
Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
double nearZeroThreshold) {
Matrix3 Q;
const auto w = xi.head<3>();
const auto v = xi.tail<3>();
ExpmapTranslation(w, v, Q, {}, nearZeroThreshold);
return Q;
return pose3::ExpmapFunctor(w).computeQ(v);
}

/* ************************************************************************* */
Expand All @@ -256,39 +279,13 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
const double theta2 = w.dot(w);
bool nearZero = (theta2 <= nearZeroThreshold);

if (Q) {
const Matrix3 V = skewSymmetric(v);
const Matrix3 W = skewSymmetric(w);
const Matrix3 WVW = W * V * W;
const double theta = w.norm();

if (nearZero) {
static constexpr double one_sixth = 1. / 6.;
static constexpr double one_twenty_fourth = 1. / 24.;
static constexpr double one_one_hundred_twentieth = 1. / 120.;

*Q = -0.5 * V + one_sixth * (W * V + V * W - WVW) -
one_twenty_fourth * (W * W * V + V * W * W - 3 * WVW) +
one_one_hundred_twentieth * (WVW * W + W * WVW);
} else {
const double s = sin(theta), c = cos(theta);
const double theta3 = theta2 * theta, theta4 = theta3 * theta,
theta5 = theta4 * theta;

// Invert the sign of odd-order terms to have the right Jacobian
*Q = -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) +
(1 - theta2 / 2 - c) / theta4 * (W * W * V + V * W * W - 3 * WVW) -
0.5 *
((1 - theta2 / 2 - c) / theta4 -
3 * (theta - s - theta3 / 6.) / theta5) *
(WVW * W + W * WVW);
}
}
if (Q) *Q = pose3::ExpmapFunctor(w, nearZero).computeQ(v);

// TODO(Frank): this threshold is *different*. Why?
if (nearZero) {
return v + 0.5 * w.cross(v);
} else {
// NOTE(Frank): t can also be computed by calling applyLeftJacobian(v), but
// formulas below convey geometric insight and creating functor is not free.
Vector3 t_parallel = w * w.dot(v); // translation parallel to axis
Vector3 w_cross_v = w.cross(v); // translation orthogonal to axis
Rot3 rotation = R.value_or(Rot3::Expmap(w));
Expand Down
131 changes: 75 additions & 56 deletions gtsam/geometry/SO3.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,13 +18,14 @@
* @date December 2014
*/

#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/concepts.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/SO3.h>

#include <Eigen/SVD>

#include <cmath>
#include <iostream>
#include <limits>

namespace gtsam {
Expand All @@ -41,7 +42,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) {
return H;
}

GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) {
GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
OptionalJacobian<9, 9> H) {
Matrix3 MR = M * R.matrix();
if (H) *H = Dcompose(R);
return MR;
Expand All @@ -51,85 +53,107 @@ void ExpmapFunctor::init(bool nearZeroApprox) {
nearZero =
nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
if (!nearZero) {
sin_theta = std::sin(theta);
const double sin_theta = std::sin(theta);
A = sin_theta / theta;
const double s2 = std::sin(theta / 2.0);
one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
const double one_minus_cos =
2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
B = one_minus_cos / theta2;
} else {
// Limits as theta -> 0:
A = 1.0;
B = 0.5;
}
}

ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
: theta2(omega.dot(omega)), theta(std::sqrt(theta2)) {
const double wx = omega.x(), wy = omega.y(), wz = omega.z();
W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
: theta2(omega.dot(omega)),
theta(std::sqrt(theta2)),
W(skewSymmetric(omega)),
WW(W * W) {
init(nearZeroApprox);
if (!nearZero) {
K = W / theta;
KK = K * K;
}
}

ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
bool nearZeroApprox)
: theta2(angle * angle), theta(angle) {
const double ax = axis.x(), ay = axis.y(), az = axis.z();
K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
W = K * angle;
: theta2(angle * angle),
theta(angle),
W(skewSymmetric(axis * angle)),
WW(W * W) {
init(nearZeroApprox);
if (!nearZero) {
KK = K * K;
}
}

SO3 ExpmapFunctor::expmap() const {
if (nearZero)
return SO3(I_3x3 + W);
else
return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK);
}
SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); }

DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
if (nearZero) {
dexp_ = I_3x3 - 0.5 * W;
} else {
a = one_minus_cos / theta;
b = 1.0 - sin_theta / theta;
dexp_ = I_3x3 - a * K + b * KK;
C = nearZero ? one_sixth : (1 - A) / theta2;
D = nearZero ? _one_twelfth : (A - 2.0 * B) / theta2;
E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2;
}

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:
// D * omega.transpose() is 1x3 Jacobian of B with respect to omega
// - skewSymmetric(v) is 3x3 Jacobian of B gtsam::cross(omega, v)
*H = Wv * D * 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:
// E * omega.transpose() is 1x3 Jacobian of C with respect to omega
// doubleCrossJacobian is 3x3 Jacobian of C gtsam::doubleCross(omega, v)
*H = WWv * E * omega.transpose() + C * doubleCrossJacobian;
}
return C * WWv;
}

// 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 (H1) {
if (nearZero) {
*H1 = 0.5 * skewSymmetric(v);
} else {
// TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
const Vector3 Kv = K * v;
const double Da = (sin_theta - 2.0 * a) / theta2;
const double Db = (one_minus_cos - 3.0 * b) / theta2;
*H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() -
skewSymmetric(Kv * b / theta) +
(a * I_3x3 - b * K) * skewSymmetric(v / theta);
}
}
if (H2) *H2 = dexp_;
return dexp_ * v;
Matrix3 D_BWv_w, D_CWWv_w;
const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr);
const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr);
if (H1) *H1 = -D_BWv_w + D_CWWv_w;
if (H2) *H2 = rightJacobian();
return v - BWv + CWWv;
}

Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
const Matrix3 invDexp = dexp_.inverse();
const Matrix3 invDexp = rightJacobian().inverse();
const Vector3 c = invDexp * v;
if (H1) {
Matrix3 D_dexpv_omega;
applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping
*H1 = -invDexp * D_dexpv_omega;
Matrix3 D_dexpv_w;
applyDexp(c, D_dexpv_w); // get derivative H of forward mapping
*H1 = -invDexp * D_dexpv_w;
}
if (H2) *H2 = invDexp;
return c;
}

Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
Matrix3 D_BWv_w, D_CWWv_w;
const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr);
const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr);
if (H1) *H1 = D_BWv_w + D_CWWv_w;
if (H2) *H2 = leftJacobian();
return v + BWv + CWWv;
}

} // namespace so3

//******************************************************************************
Expand Down Expand Up @@ -168,12 +192,7 @@ SO3 SO3::ChordalMean(const std::vector<SO3>& rotations) {
template <>
GTSAM_EXPORT
Matrix3 SO3::Hat(const Vector3& xi) {
// skew symmetric matrix X = xi^
Matrix3 Y = Z_3x3;
Y(0, 1) = -xi(2);
Y(0, 2) = +xi(1);
Y(1, 2) = -xi(0);
return Y - Y.transpose();
return skewSymmetric(xi);
}

//******************************************************************************
Expand Down
Loading

0 comments on commit 5125844

Please sign in to comment.