Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Better SE(3) and SE2(3) Jacobians #1938

Merged
merged 8 commits into from
Dec 17, 2024
Merged

Better SE(3) and SE2(3) Jacobians #1938

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
76c9537
inverse Jacobians
dellaert Dec 16, 2024
6c84a2b
Use X to map left to right
dellaert Dec 16, 2024
9869725
applyLeftJacobianInverse
dellaert Dec 16, 2024
b113871
No more functor for Q
dellaert Dec 16, 2024
2aa36d4
Modernize ExmapTranslation
dellaert Dec 16, 2024
db5b9de
Taylor expansion
dellaert Dec 16, 2024
49c2a04
Address review comments
dellaert Dec 16, 2024
8e473c0
Address Quaternion on Ubuntu accuracy
dellaert Dec 17, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
97 changes: 44 additions & 53 deletions gtsam/geometry/Pose3.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,8 @@
#include <iostream>
#include <string>

#include "gtsam/geometry/Rot3.h"
dellaert marked this conversation as resolved.
Show resolved Hide resolved

namespace gtsam {

/** instantiate concept checks */
Expand Down Expand Up @@ -111,7 +113,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) {

/* ************************************************************************* */
Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) {
OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) {
if (Hxi) {
Hxi->setZero();
for (int i = 0; i < 6; ++i) {
Expand Down Expand Up @@ -158,26 +160,26 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
/* ************************************************************************* */
Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
return Pose3(interpolate<Rot3>(R_, T.R_, t),
interpolate<Point3>(t_, T.t_, t));
interpolate<Point3>(t_, T.t_, t));
}

/* ************************************************************************* */
/** Modified from Murray94book version (which assumes w and v normalized?) */
dellaert marked this conversation as resolved.
Show resolved Hide resolved
Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
// Get angular velocity omega and translational velocity v from twist xi
const Vector3 w = xi.head<3>(), v = xi.tail<3>();

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

// Compute translation and optionally its Jacobian in w
// Compute translation and optionally its Jacobian Q in w
// The Jacobian in v is the right Jacobian Jr of SO(3), which we already have.
Matrix3 Q;
const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr, R);
const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr);

if (Hxi) {
*Hxi << Jw, Z_3x3,
Q, Jw;
*Hxi << Jr, Z_3x3, //
Q, Jr;
}

return Pose3(R, t);
Expand Down Expand Up @@ -238,60 +240,50 @@ 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) {
const auto w = xi.head<3>();
const auto v = xi.tail<3>();
return pose3::ExpmapFunctor(w).computeQ(v);
Matrix3 Q;
ExpmapTranslation(w, v, Q, {}, nearZeroThreshold);
return Q;
}

/* ************************************************************************* */
// NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas
// t_parallel = w * w.dot(v); // translation parallel to axis
// w_cross_v = w.cross(v); // translation orthogonal to axis
// t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2;
// but functor does not need R, deals automatically with the case where theta2
// is near zero, and also gives us the machinery for the Jacobians.
Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
OptionalJacobian<3, 3> Q,
const std::optional<Rot3>& R,
OptionalJacobian<3, 3> J,
double nearZeroThreshold) {
const double theta2 = w.dot(w);
bool nearZero = (theta2 <= nearZeroThreshold);

if (Q) *Q = pose3::ExpmapFunctor(w, nearZero).computeQ(v);
// Instantiate functor for Dexp-related operations:
so3::DexpFunctor local(w, nearZero);

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));
Vector3 t = (w_cross_v - rotation * w_cross_v + t_parallel) / theta2;
return t;
// Call applyLeftJacobian which is faster than local.leftJacobian() * v if you
// don't need Jacobians, and returns Jacobian of t with respect to w if asked.
Matrix3 H;
Vector t = local.applyLeftJacobian(v, Q ? &H : nullptr);

// We return Jacobians for use in Expmap, so we multiply with X, that
// translates from left to right for our right expmap convention:
if (Q) {
Matrix3 X = local.rightJacobian() * local.leftJacobianInverse();
*Q = X * H;
}

if (J) {
*J = local.rightJacobian(); // = X * local.leftJacobian();
}

return t;
}

/* ************************************************************************* */
Expand Down Expand Up @@ -320,7 +312,6 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const {
}

/* ************************************************************************* */

const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const {
if (Hself) {
*Hself << I_3x3, Z_3x3;
Expand All @@ -338,14 +329,14 @@ Matrix4 Pose3::matrix() const {

/* ************************************************************************* */
Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself,
OptionalJacobian<6, 6> HaTb) const {
OptionalJacobian<6, 6> HaTb) const {
const Pose3& wTa = *this;
return wTa.compose(aTb, Hself, HaTb);
}

/* ************************************************************************* */
Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself,
OptionalJacobian<6, 6> HwTb) const {
OptionalJacobian<6, 6> HwTb) const {
if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap();
if (HwTb) *HwTb = I_6x6;
const Pose3& wTa = *this;
Expand All @@ -354,7 +345,7 @@ Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself,

/* ************************************************************************* */
Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself,
OptionalJacobian<3, 3> Hpoint) const {
OptionalJacobian<3, 3> Hpoint) const {
// Only get matrix once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case
const Matrix3 R = R_.matrix();
Expand All @@ -378,7 +369,7 @@ Matrix Pose3::transformFrom(const Matrix& points) const {

/* ************************************************************************* */
Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself,
OptionalJacobian<3, 3> Hpoint) const {
OptionalJacobian<3, 3> Hpoint) const {
// Only get transpose once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case
const Matrix3 Rt = R_.transpose();
Expand Down Expand Up @@ -484,7 +475,7 @@ std::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
std::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) {
throw std::invalid_argument(
"Pose3:Align expects 3*N matrices of equal shape.");
"Pose3:Align expects 3*N matrices of equal shape.");
}
Point3Pairs abPointPairs;
for (Eigen::Index j = 0; j < a.cols(); j++) {
Expand Down
10 changes: 6 additions & 4 deletions gtsam/geometry/Pose3.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -223,17 +223,19 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
const Vector6& xi, double nearZeroThreshold = 1e-5);

/**
* Compute the translation part of the exponential map, with derivative.
* Compute the translation part of the exponential map, with Jacobians.
* @param w 3D angular velocity
* @param v 3D velocity
* @param Q Optionally, compute 3x3 Jacobian wrpt w
* @param R Optionally, precomputed as Rot3::Expmap(w)
* @param J Optionally, compute 3x3 Jacobian wrpt v = right Jacobian of SO(3)
* @param nearZeroThreshold threshold for small values
* Note Q is 3x3 bottom-left block of SE3 Expmap right derivative matrix
* @note This function returns Jacobians Q and J corresponding to the bottom
* blocks of the SE(3) exponential, and translated from left to right from the
* applyLeftJacobian Jacobians.
*/
static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v,
OptionalJacobian<3, 3> Q = {},
const std::optional<Rot3>& R = {},
OptionalJacobian<3, 3> J = {},
double nearZeroThreshold = 1e-5);

using LieGroup<Pose3, 6>::inverse; // version with derivative
Expand Down
58 changes: 41 additions & 17 deletions gtsam/geometry/SO3.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,19 +87,29 @@ SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); }

DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
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;
if (!nearZero) {
C = (1 - A) / theta2;
D = (1.0 - A / (2.0 * B)) / theta2;
E = (2.0 * B - A) / theta2;
F = (3.0 * C - B) / theta2;
} else {
// Limit as theta -> 0
// TODO(Frank): flipping signs here does not trigger any tests: harden!
C = one_sixth;
D = one_twelfth;
E = one_twelfth;
F = one_sixtieth;
}
}

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);
// 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;
}
Expand All @@ -111,10 +121,10 @@ Vector3 DexpFunctor::doubleCrossC(const Vector3& v,
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;
// 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;
}
Expand All @@ -132,14 +142,14 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,

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

Expand All @@ -154,6 +164,20 @@ Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
return v + BWv + CWWv;
}

Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v,
OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
const Matrix3 invJl = leftJacobianInverse();
const Vector3 c = invJl * v;
if (H1) {
Matrix3 H;
applyLeftJacobian(c, H); // get derivative H of forward mapping
*H1 = -invJl * H;
}
if (H2) *H2 = invJl;
return c;
}

} // namespace so3

//******************************************************************************
Expand Down
30 changes: 25 additions & 5 deletions gtsam/geometry/SO3.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -157,10 +157,16 @@ struct GTSAM_EXPORT ExpmapFunctor {
/// Functor that implements Exponential map *and* its derivatives
struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
const Vector3 omega;
double C; // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0

// Ethan's C constant used in Jacobians
double C; // (1 - A) / theta^2 or 1/6 for theta->0

// Constant used in inverse Jacobians
double D; // (1 - A/2B) / theta2 or 1/12 for theta->0

// Constants used in cross and doubleCross
double D; // (A - 2.0 * B) / theta2 or -1/12 for theta->0
double E; // (B - 3.0 * C) / theta2 or -1/60 for theta->0
double E; // (A - 2.0 * B) / theta2 or -1/12 for theta->0
dellaert marked this conversation as resolved.
Show resolved Hide resolved
double F; // (B - 3.0 * C) / theta2 or -1/60 for theta->0

/// Constructor with element of Lie algebra so(3)
explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
Expand All @@ -179,6 +185,15 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
/// Differential of expmap == right Jacobian
inline Matrix3 dexp() const { return rightJacobian(); }

/// Inverse of right Jacobian
Matrix3 rightJacobianInverse() const { return I_3x3 + 0.5 * W + D * WW; }

// Inverse of left Jacobian
Matrix3 leftJacobianInverse() const { return I_3x3 - 0.5 * W + D * WW; }

/// Synonym for rightJacobianInverse
inline Matrix3 invDexp() const { return rightJacobianInverse(); }

/// Computes B * (omega x v).
Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;

Expand All @@ -197,9 +212,14 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;

/// Multiplies with leftJacobianInverse(), with optional derivatives
Vector3 applyLeftJacobianInverse(const Vector3& v,
OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;

static constexpr double one_sixth = 1.0 / 6.0;
static constexpr double _one_twelfth = -1.0 / 12.0;
static constexpr double _one_sixtieth = -1.0 / 60.0;
static constexpr double one_twelfth = 1.0 / 12.0;
static constexpr double one_sixtieth = 1.0 / 60.0;
};
} // namespace so3

Expand Down
11 changes: 0 additions & 11 deletions gtsam/geometry/tests/testPose3.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -900,17 +900,6 @@ TEST(Pose3, ExpmapDerivative4) {
}
}

TEST( Pose3, ExpmapDerivativeQr) {
dellaert marked this conversation as resolved.
Show resolved Hide resolved
dellaert marked this conversation as resolved.
Show resolved Hide resolved
Vector6 w = Vector6::Random();
dellaert marked this conversation as resolved.
Show resolved Hide resolved
w.head<3>().normalize();
w.head<3>() = w.head<3>() * 0.9e-2;
Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01);
Matrix expectedH = numericalDerivative21<Pose3, Vector6,
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>();
EXPECT(assert_equal(expectedQr, actualQr, 1e-6));
}

/* ************************************************************************* */
TEST( Pose3, LogmapDerivative) {
Matrix6 actualH;
Expand Down
Loading
Loading