Skip to content

Commit

Permalink
inlined ExpmapTranslation
Browse files Browse the repository at this point in the history
  • Loading branch information
@dellaert
dellaert committed Jan 15, 2025
1 parent 4cbf673 commit 04e04ee
Show file tree
Hide file tree
Showing 3 changed files with 66 additions and 71 deletions.
72 changes: 33 additions & 39 deletions gtsam/geometry/Pose3.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -183,16 +183,37 @@ 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>();

// Instantiate functor for Dexp-related operations:
const bool nearZero = (w.dot(w) <= 1e-5);
const so3::DexpFunctor local(w, nearZero);

// Compute rotation using Expmap
Matrix3 Jr;
Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
#ifdef GTSAM_USE_QUATERNIONS
const Rot3 R = traits<gtsam::Quaternion>::Expmap(v);
#else
const Rot3 R(local.expmap());
#endif

// 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);
// The translation t = local.leftJacobian() * v.
// Here we call applyLeftJacobian, which is faster if you don't need
// Jacobians, and returns Jacobian of t with respect to w if asked.
// 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.
Matrix3 H;
const Vector3 t = local.applyLeftJacobian(v, Hxi ? &H : nullptr);

if (Hxi) {
// The Jacobian of expmap is given by the right Jacobian of SO(3):
const Matrix3 Jr = local.rightJacobian();
// We multiply H, the derivative of applyLeftJacobian in omega, with
// X = Jr * Jl^{-1},
// which translates from left to right for our right expmap convention:
const Matrix3 X = Jr * local.leftJacobianInverse();
const Matrix3 Q = X * H;
*Hxi << Jr, Z_3x3, //
Q, Jr;
}
Expand Down Expand Up @@ -260,45 +281,18 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
double nearZeroThreshold) {
const auto w = xi.head<3>();
const auto v = xi.tail<3>();
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,
OptionalJacobian<3, 3> J,
double nearZeroThreshold) {
const double theta2 = w.dot(w);
bool nearZero = (theta2 <= nearZeroThreshold);

// Instantiate functor for Dexp-related operations:
bool nearZero = (w.dot(w) <= nearZeroThreshold);
so3::DexpFunctor local(w, nearZero);

// 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.
// Call applyLeftJacobian to get its Jacobian
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();
}
local.applyLeftJacobian(v, H);

return t;
// Multiply with X, translates from left to right for our expmap convention:
const Matrix3 X = local.rightJacobian() * local.leftJacobianInverse();
return X * H;
}

/* ************************************************************************* */
Expand Down
17 changes: 0 additions & 17 deletions gtsam/geometry/Pose3.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -220,27 +220,10 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
* (see Chirikjian11book2, pg 44, eq 10.95.
* The closed-form formula is identical to formula 102 in Barfoot14tro where
* Q_l of the SE3 Expmap left derivative matrix is given.
* This is the Jacobian of ExpmapTranslation and computed there.
*/
static Matrix3 ComputeQforExpmapDerivative(
const Vector6& xi, double nearZeroThreshold = 1e-5);

/**
* 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 J Optionally, compute 3x3 Jacobian wrpt v = right Jacobian of SO(3)
* @param nearZeroThreshold threshold for small values
* @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 = {},
OptionalJacobian<3, 3> J = {},
double nearZeroThreshold = 1e-5);

using LieGroup<Pose3, 6>::inverse; // version with derivative

/**
Expand Down
48 changes: 33 additions & 15 deletions gtsam/navigation/NavState.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -117,20 +117,29 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) {
// Get angular velocity w and components rho (for t) and nu (for v) from xi
Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>();

// Instantiate functor for Dexp-related operations:
const bool nearZero = (w.dot(w) <= 1e-5);
const so3::DexpFunctor local(w, nearZero);

// Compute rotation using Expmap
Matrix3 Jr;
Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
#ifdef GTSAM_USE_QUATERNIONS
const Rot3 R = traits<gtsam::Quaternion>::Expmap(v);
#else
const Rot3 R(local.expmap());
#endif

// Compute translations and optionally their Jacobians Q in w
// The Jacobians with respect to rho and nu are equal to Jr
Matrix3 Qt, Qv;
Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr);
Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr);
// Compute translation and velocity. See Pose3::Expmap
Matrix3 H_t_w, H_v_w;
const Vector3 t = local.applyLeftJacobian(rho, Hxi ? &H_t_w : nullptr);
const Vector3 v = local.applyLeftJacobian(nu, Hxi ? &H_v_w : nullptr);

if (Hxi) {
// See Pose3::Expamp for explanation of the Jacobians
const Matrix3 Jr = local.rightJacobian();
const Matrix3 X = Jr * local.leftJacobianInverse();
*Hxi << Jr, Z_3x3, Z_3x3, //
Qt, Jr, Z_3x3, //
Qv, Z_3x3, Jr;
X * H_t_w, Jr, Z_3x3, //
X * H_v_w, Z_3x3, Jr;
}

return NavState(R, t, v);
Expand Down Expand Up @@ -231,11 +240,21 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
const Vector3 w = xi.head<3>();
Vector3 rho = xi.segment<3>(3);
Vector3 nu = xi.tail<3>();

Matrix3 Qt, Qv;
const Rot3 R = Rot3::Expmap(w);
Pose3::ExpmapTranslation(w, rho, Qt);
Pose3::ExpmapTranslation(w, nu, Qv);

// Instantiate functor for Dexp-related operations:
const bool nearZero = (w.dot(w) <= 1e-5);
const so3::DexpFunctor local(w, nearZero);

// Call applyLeftJacobian to get its Jacobians
Matrix3 H_t_w, H_v_w;
local.applyLeftJacobian(rho, H_t_w);
local.applyLeftJacobian(nu, H_v_w);

// Multiply with X, translates from left to right for our expmap convention:
const Matrix3 X = local.rightJacobian() * local.leftJacobianInverse();
const Matrix3 Qt = X * H_t_w;
const Matrix3 Qv = X * H_v_w;

const Matrix3 Jw = Rot3::LogmapDerivative(w);
const Matrix3 Qt2 = -Jw * Qt * Jw;
const Matrix3 Qv2 = -Jw * Qv * Jw;
Expand All @@ -247,7 +266,6 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
return J;
}


//------------------------------------------------------------------------------
NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
ChartJacobian Hxi) {
Expand Down

0 comments on commit 04e04ee

Please sign in to comment.