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

Merge pull request #1938 from borglab/feature/betterSE3Jacobian 

Better SE(3) and SE2(3) Jacobians
release/4.3a0
Frank Dellaert 2024-12-17 08:50:52 -05:00 committed by GitHub
parent 5125844d19 8e473c04e8
commit 8c903c2515
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
7 changed files with 260 additions and 161 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
gtsam/geometry/Pose3.cpp
View File

@ -11,13 +11,14 @@
/** /**
* @file Pose3.cpp * @file Pose3.cpp
* @brief 3D Pose * @brief 3D Pose manifold SO(3) x R^3 and group SE(3)
*/ */
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/base/concepts.h> #include <gtsam/base/concepts.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/concepts.h>
#include <cmath> #include <cmath>
#include <iostream> #include <iostream>
@ -111,7 +112,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) {
/* ************************************************************************* */ /* ************************************************************************* */
Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, 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) { if (Hxi) {
Hxi->setZero(); Hxi->setZero();
for (int i = 0; i < 6; ++i) { for (int i = 0; i < 6; ++i) {
@ -158,26 +159,28 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::interpolateRt(const Pose3& T, double t) const { Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
return Pose3(interpolate<Rot3>(R_, T.R_, t), 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?) */ // Expmap is implemented in so3::ExpmapFunctor::expmap, based on Ethan Eade's
// elegant Lie group document, at https://www.ethaneade.org/lie.pdf.
Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
// Get angular velocity omega and translational velocity v from twist xi // Get angular velocity omega and translational velocity v from twist xi
const Vector3 w = xi.head<3>(), v = xi.tail<3>(); const Vector3 w = xi.head<3>(), v = xi.tail<3>();
// Compute rotation using Expmap // Compute rotation using Expmap
Matrix3 Jw; Matrix3 Jr;
Rot3 R = Rot3::Expmap(w, Hxi ? &Jw : nullptr); 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; Matrix3 Q;
const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr, R); const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr);
if (Hxi) { if (Hxi) {
*Hxi << Jw, Z_3x3, *Hxi << Jr, Z_3x3, //
Q, Jw; Q, Jr;
} }
return Pose3(R, t); return Pose3(R, t);
@ -238,60 +241,50 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
#endif #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, Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
double nearZeroThreshold) { double nearZeroThreshold) {
const auto w = xi.head<3>(); const auto w = xi.head<3>();
const auto v = xi.tail<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, Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
OptionalJacobian<3, 3> Q, OptionalJacobian<3, 3> Q,
const std::optional<Rot3>& R, OptionalJacobian<3, 3> J,
double nearZeroThreshold) { double nearZeroThreshold) {
const double theta2 = w.dot(w); const double theta2 = w.dot(w);
bool nearZero = (theta2 <= nearZeroThreshold); 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) { // Call applyLeftJacobian which is faster than local.leftJacobian() * v if you
return v + 0.5 * w.cross(v); // don't need Jacobians, and returns Jacobian of t with respect to w if asked.
} else { Matrix3 H;
// NOTE(Frank): t can also be computed by calling applyLeftJacobian(v), but Vector t = local.applyLeftJacobian(v, Q ? &H : nullptr);
// formulas below convey geometric insight and creating functor is not free.
Vector3 t_parallel = w * w.dot(v); // translation parallel to axis // We return Jacobians for use in Expmap, so we multiply with X, that
Vector3 w_cross_v = w.cross(v); // translation orthogonal to axis // translates from left to right for our right expmap convention:
Rot3 rotation = R.value_or(Rot3::Expmap(w)); if (Q) {
Vector3 t = (w_cross_v - rotation * w_cross_v + t_parallel) / theta2; Matrix3 X = local.rightJacobian() * local.leftJacobianInverse();
return t; *Q = X * H;
} }
if (J) {
*J = local.rightJacobian(); // = X * local.leftJacobian();
}
return t;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -320,7 +313,6 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const { const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const {
if (Hself) { if (Hself) {
*Hself << I_3x3, Z_3x3; *Hself << I_3x3, Z_3x3;
@ -338,14 +330,14 @@ Matrix4 Pose3::matrix() const {
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself, Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself,
OptionalJacobian<6, 6> HaTb) const { OptionalJacobian<6, 6> HaTb) const {
const Pose3& wTa = *this; const Pose3& wTa = *this;
return wTa.compose(aTb, Hself, HaTb); return wTa.compose(aTb, Hself, HaTb);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself, 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 (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap();
if (HwTb) *HwTb = I_6x6; if (HwTb) *HwTb = I_6x6;
const Pose3& wTa = *this; const Pose3& wTa = *this;
@ -354,7 +346,7 @@ Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself,
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 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, // Only get matrix once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case // as well as multiple conversions in the Quaternion case
const Matrix3 R = R_.matrix(); const Matrix3 R = R_.matrix();
@ -378,7 +370,7 @@ Matrix Pose3::transformFrom(const Matrix& points) const {
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself, 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, // Only get transpose once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case // as well as multiple conversions in the Quaternion case
const Matrix3 Rt = R_.transpose(); const Matrix3 Rt = R_.transpose();
@ -484,7 +476,7 @@ std::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
std::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) { std::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) { if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) {
throw std::invalid_argument( throw std::invalid_argument(
"Pose3:Align expects 3*N matrices of equal shape."); "Pose3:Align expects 3*N matrices of equal shape.");
} }
Point3Pairs abPointPairs; Point3Pairs abPointPairs;
for (Eigen::Index j = 0; j < a.cols(); j++) { for (Eigen::Index j = 0; j < a.cols(); j++) {

12
gtsam/geometry/Pose3.h
View File

@ -11,7 +11,7 @@
/** /**
*@file Pose3.h *@file Pose3.h
*@brief 3D Pose * @brief 3D Pose manifold SO(3) x R^3 and group SE(3)
*/ */
// \callgraph // \callgraph
@ -223,17 +223,19 @@ public:
const Vector6& xi, double nearZeroThreshold = 1e-5); 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 w 3D angular velocity
* @param v 3D velocity * @param v 3D velocity
* @param Q Optionally, compute 3x3 Jacobian wrpt w * @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 * @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, static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v,
OptionalJacobian<3, 3> Q = {}, OptionalJacobian<3, 3> Q = {},
const std::optional<Rot3>& R = {}, OptionalJacobian<3, 3> J = {},
double nearZeroThreshold = 1e-5); double nearZeroThreshold = 1e-5);
using LieGroup<Pose3, 6>::inverse; // version with derivative using LieGroup<Pose3, 6>::inverse; // version with derivative

73
gtsam/geometry/SO3.cpp
View File

@ -33,6 +33,15 @@ namespace gtsam {
//****************************************************************************** //******************************************************************************
namespace so3 { namespace so3 {
static constexpr double one_6th = 1.0 / 6.0;
static constexpr double one_12th = 1.0 / 12.0;
static constexpr double one_24th = 1.0 / 24.0;
static constexpr double one_60th = 1.0 / 60.0;
static constexpr double one_120th = 1.0 / 120.0;
static constexpr double one_180th = 1.0 / 180.0;
static constexpr double one_720th = 1.0 / 720.0;
static constexpr double one_1260th = 1.0 / 1260.0;
GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) { GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) {
Matrix99 H; Matrix99 H;
auto R = Q.matrix(); auto R = Q.matrix();
@ -60,9 +69,9 @@ void ExpmapFunctor::init(bool nearZeroApprox) {
2.0 * s2 * s2; // numerically better than [1 - cos(theta)] 2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
B = one_minus_cos / theta2; B = one_minus_cos / theta2;
} else { } else {
// Limits as theta -> 0: // Taylor expansion at 0
A = 1.0; A = 1.0 - theta2 * one_6th;
B = 0.5; B = 0.5 - theta2 * one_24th;
} }
} }
@ -87,19 +96,29 @@ SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); }
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) { : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
C = nearZero ? one_sixth : (1 - A) / theta2; if (!nearZero) {
D = nearZero ? _one_twelfth : (A - 2.0 * B) / theta2; C = (1 - A) / theta2;
E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2; D = (1.0 - A / (2.0 * B)) / theta2;
E = (2.0 * B - A) / theta2;
F = (3.0 * C - B) / theta2;
} else {
// Taylor expansion at 0
// TODO(Frank): flipping signs here does not trigger any tests: harden!
C = one_6th - theta2 * one_120th;
D = one_12th + theta2 * one_720th;
E = one_12th - theta2 * one_180th;
F = one_60th - theta2 * one_1260th;
}
} }
Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const { Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const {
// Wv = omega x v // Wv = omega x v
const Vector3 Wv = gtsam::cross(omega, v); const Vector3 Wv = gtsam::cross(omega, v);
if (H) { if (H) {
// Apply product rule: // Apply product rule to (B Wv)
// D * omega.transpose() is 1x3 Jacobian of B with respect to omega // - E * omega.transpose() is 1x3 Jacobian of B with respect to omega
// - skewSymmetric(v) is 3x3 Jacobian of B gtsam::cross(omega, v) // - skewSymmetric(v) is 3x3 Jacobian of Wv = gtsam::cross(omega, v)
*H = Wv * D * omega.transpose() - B * skewSymmetric(v); *H = - Wv * E * omega.transpose() - B * skewSymmetric(v);
} }
return B * Wv; return B * Wv;
} }
@ -111,10 +130,10 @@ Vector3 DexpFunctor::doubleCrossC(const Vector3& v,
const Vector3 WWv = const Vector3 WWv =
gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr); gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr);
if (H) { if (H) {
// Apply product rule: // Apply product rule to (C WWv)
// E * omega.transpose() is 1x3 Jacobian of C with respect to omega // - F * omega.transpose() is 1x3 Jacobian of C with respect to omega
// doubleCrossJacobian is 3x3 Jacobian of C gtsam::doubleCross(omega, v) // doubleCrossJacobian is 3x3 Jacobian of WWv = gtsam::doubleCross(omega, v)
*H = WWv * E * omega.transpose() + C * doubleCrossJacobian; *H = - WWv * F * omega.transpose() + C * doubleCrossJacobian;
} }
return C * WWv; return C * WWv;
} }
@ -132,14 +151,14 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const { OptionalJacobian<3, 3> H2) const {
const Matrix3 invDexp = rightJacobian().inverse(); const Matrix3 invJr = rightJacobianInverse();
const Vector3 c = invDexp * v; const Vector3 c = invJr * v;
if (H1) { if (H1) {
Matrix3 D_dexpv_w; Matrix3 H;
applyDexp(c, D_dexpv_w); // get derivative H of forward mapping applyDexp(c, H); // get derivative H of forward mapping
*H1 = -invDexp * D_dexpv_w; *H1 = -invJr * H;
} }
if (H2) *H2 = invDexp; if (H2) *H2 = invJr;
return c; return c;
} }
@ -154,6 +173,20 @@ Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
return v + BWv + CWWv; 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 } // namespace so3
//****************************************************************************** //******************************************************************************

36
gtsam/geometry/SO3.h
View File

@ -132,14 +132,16 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
// functor also implements dedicated methods to apply dexp and/or inv(dexp). // functor also implements dedicated methods to apply dexp and/or inv(dexp).
/// Functor implementing Exponential map /// Functor implementing Exponential map
/// Math is based on Ethan Eade's elegant Lie group document, at
/// https://www.ethaneade.org/lie.pdf.
struct GTSAM_EXPORT ExpmapFunctor { struct GTSAM_EXPORT ExpmapFunctor {
const double theta2, theta; const double theta2, theta;
const Matrix3 W, WW; const Matrix3 W, WW;
bool nearZero; bool nearZero;
// Ethan Eade's constants: // Ethan Eade's constants:
double A; // A = sin(theta) / theta or 1 for theta->0 double A; // A = sin(theta) / theta
double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0 double B; // B = (1 - cos(theta))
/// Constructor with element of Lie algebra so(3) /// Constructor with element of Lie algebra so(3)
explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false); explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
@ -155,12 +157,20 @@ struct GTSAM_EXPORT ExpmapFunctor {
}; };
/// Functor that implements Exponential map *and* its derivatives /// Functor that implements Exponential map *and* its derivatives
/// Math extends Ethan theme of elegant I + aW + bWW expressions.
/// See https://www.ethaneade.org/lie.pdf expmap (82) and left Jacobian (83).
struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
const Vector3 omega; 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
// Constant used in inverse Jacobians
double D; // (1 - A/2B) / theta2
// Constants used in cross and doubleCross // Constants used in cross and doubleCross
double D; // (A - 2.0 * B) / theta2 or -1/12 for theta->0 double E; // (2B - A) / theta2
double E; // (B - 3.0 * C) / theta2 or -1/60 for theta->0 double F; // (3C - B) / theta2
/// Constructor with element of Lie algebra so(3) /// Constructor with element of Lie algebra so(3)
explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
@ -179,6 +189,15 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
/// Differential of expmap == right Jacobian /// Differential of expmap == right Jacobian
inline Matrix3 dexp() const { return rightJacobian(); } 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). /// Computes B * (omega x v).
Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
@ -197,9 +216,10 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const; OptionalJacobian<3, 3> H2 = {}) const;
static constexpr double one_sixth = 1.0 / 6.0; /// Multiplies with leftJacobianInverse(), with optional derivatives
static constexpr double _one_twelfth = -1.0 / 12.0; Vector3 applyLeftJacobianInverse(const Vector3& v,
static constexpr double _one_sixtieth = -1.0 / 60.0; OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
}; };
} // namespace so3 } // namespace so3

114
gtsam/geometry/tests/testPose3.cpp
View File

@ -835,38 +835,7 @@ TEST(Pose3, Align2) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, ExpmapDerivative1) { TEST(Pose3, ExpmapDerivative) {
Matrix6 actualH;
Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
Pose3::Expmap(w,actualH);
Matrix expectedH = numericalDerivative21<Pose3, Vector6,
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
EXPECT(assert_equal(expectedH, actualH));
}
/* ************************************************************************* */
TEST( Pose3, ExpmapDerivative2) {
Matrix6 actualH;
Vector6 w; w << 1.0, -2.0, 3.0, -10.0, -20.0, 30.0;
Pose3::Expmap(w,actualH);
Matrix expectedH = numericalDerivative21<Pose3, Vector6,
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
EXPECT(assert_equal(expectedH, actualH));
}
/* ************************************************************************* */
TEST( Pose3, ExpmapDerivative3) {
Matrix6 actualH;
Vector6 w; w << 0.0, 0.0, 0.0, -10.0, -20.0, 30.0;
Pose3::Expmap(w,actualH);
Matrix expectedH = numericalDerivative21<Pose3, Vector6,
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
// Small angle approximation is not as precise as numerical derivative?
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}
/* ************************************************************************* */
TEST(Pose3, ExpmapDerivative4) {
// Iserles05an (Lie-group Methods) says: // Iserles05an (Lie-group Methods) says:
// scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
// matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t) // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)
@ -900,26 +869,71 @@ TEST(Pose3, ExpmapDerivative4) {
} }
} }
TEST( Pose3, ExpmapDerivativeQr) { //******************************************************************************
Vector6 w = Vector6::Random(); namespace test_cases {
w.head<3>().normalize(); std::vector<Vector3> small{{0, 0, 0}, //
w.head<3>() = w.head<3>() * 0.9e-2; {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //,
Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01); {1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}};
Matrix expectedH = numericalDerivative21<Pose3, Vector6, std::vector<Vector3> large{{0, 0, 0}, {1, 0, 0}, {0, 1, 0},
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {}); {0, 0, 1}, {.1, .2, .3}, {1, -2, 3}};
Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>(); auto omegas = [](bool nearZero) { return nearZero ? small : large; };
EXPECT(assert_equal(expectedQr, actualQr, 1e-6)); std::vector<Vector3> vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1},
{.4, .3, .2}, {4, 5, 6}, {-10, -20, 30}};
} // namespace test_cases
//******************************************************************************
TEST(Pose3, ExpmapDerivatives) {
for (bool nearZero : {true, false}) {
for (const Vector3& w : test_cases::omegas(nearZero)) {
for (Vector3 v : test_cases::vs) {
const Vector6 xi = (Vector6() << w, v).finished();
const Matrix6 expectedH =
numericalDerivative21<Pose3, Vector6, OptionalJacobian<6, 6> >(
&Pose3::Expmap, xi, {});
Matrix actualH;
Pose3::Expmap(xi, actualH);
EXPECT(assert_equal(expectedH, actualH));
}
}
}
} }
/* ************************************************************************* */ //******************************************************************************
TEST( Pose3, LogmapDerivative) { // Check logmap for all small values, as we don't want wrapping.
Matrix6 actualH; TEST(Pose3, Logmap) {
Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0; static constexpr bool nearZero = true;
Pose3 p = Pose3::Expmap(w); for (const Vector3& w : test_cases::omegas(nearZero)) {
EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5)); for (Vector3 v : test_cases::vs) {
Matrix expectedH = numericalDerivative21<Vector6, Pose3, const Vector6 xi = (Vector6() << w, v).finished();
OptionalJacobian<6, 6> >(&Pose3::Logmap, p, {}); Pose3 pose = Pose3::Expmap(xi);
EXPECT(assert_equal(expectedH, actualH)); EXPECT(assert_equal(xi, Pose3::Logmap(pose)));
}
}
}
//******************************************************************************
// Check logmap derivatives for all values
TEST(Pose3, LogmapDerivatives) {
for (bool nearZero : {true, false}) {
for (const Vector3& w : test_cases::omegas(nearZero)) {
for (Vector3 v : test_cases::vs) {
const Vector6 xi = (Vector6() << w, v).finished();
Pose3 pose = Pose3::Expmap(xi);
const Matrix6 expectedH =
numericalDerivative21<Vector6, Pose3, OptionalJacobian<6, 6> >(
&Pose3::Logmap, pose, {});
Matrix actualH;
Pose3::Logmap(pose, actualH);
#ifdef GTSAM_USE_QUATERNIONS
// TODO(Frank): Figure out why quaternions are not as accurate.
// Hint: 6 cases fail on Ubuntu 22.04, but none on MacOS.
EXPECT(assert_equal(expectedH, actualH, 1e-7));
#else
EXPECT(assert_equal(expectedH, actualH));
#endif
}
}
}
} }
/* ************************************************************************* */ /* ************************************************************************* */

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

@ -303,14 +303,31 @@ TEST(SO3, JacobianLogmap) {
EXPECT(assert_equal(Jexpected, Jactual)); EXPECT(assert_equal(Jexpected, Jactual));
} }
//******************************************************************************
namespace test_cases { namespace test_cases {
std::vector<Vector3> small{{0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}}; std::vector<Vector3> small{{0, 0, 0}, //
{1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //,
{1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}};
std::vector<Vector3> large{ std::vector<Vector3> large{
{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}}; {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}};
auto omegas = [](bool nearZero) { return nearZero ? small : large; }; auto omegas = [](bool nearZero) { return nearZero ? small : large; };
std::vector<Vector3> vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}}; std::vector<Vector3> vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}};
} // namespace test_cases } // namespace test_cases
//******************************************************************************
TEST(SO3, JacobianInverses) {
Matrix HR, HL;
for (bool nearZero : {true, false}) {
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
EXPECT(assert_equal<Matrix3>(local.rightJacobian().inverse(),
local.rightJacobianInverse()));
EXPECT(assert_equal<Matrix3>(local.leftJacobian().inverse(),
local.leftJacobianInverse()));
}
}
}
//****************************************************************************** //******************************************************************************
TEST(SO3, CrossB) { TEST(SO3, CrossB) {
Matrix aH1; Matrix aH1;
@ -368,6 +385,28 @@ TEST(SO3, ApplyDexp) {
} }
} }
//******************************************************************************
TEST(SO3, ApplyInvDexp) {
Matrix aH1, aH2;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyInvDexp(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
Matrix invJr = local.rightJacobianInverse();
for (const Vector3& v : test_cases::vs) {
EXPECT(
assert_equal(Vector3(invJr * v), local.applyInvDexp(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(invJr, aH2));
}
}
}
}
//****************************************************************************** //******************************************************************************
TEST(SO3, ApplyLeftJacobian) { TEST(SO3, ApplyLeftJacobian) {
Matrix aH1, aH2; Matrix aH1, aH2;
@ -390,22 +429,22 @@ TEST(SO3, ApplyLeftJacobian) {
} }
//****************************************************************************** //******************************************************************************
TEST(SO3, ApplyInvDexp) { 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 =
[=](const Vector3& omega, const Vector3& v) { [=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyInvDexp(v); return so3::DexpFunctor(omega, nearZero).applyLeftJacobianInverse(v);
}; };
for (const Vector3& omega : test_cases::omegas(nearZero)) { for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero); so3::DexpFunctor local(omega, nearZero);
Matrix invDexp = local.dexp().inverse(); Matrix invJl = local.leftJacobianInverse();
for (const Vector3& v : test_cases::vs) { for (const Vector3& v : test_cases::vs) {
EXPECT(assert_equal(Vector3(invDexp * v), EXPECT(assert_equal(Vector3(invJl * v),
local.applyInvDexp(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(invDexp, aH2)); EXPECT(assert_equal(invJl, aH2));
} }
} }
} }

27
gtsam/navigation/NavState.cpp
View File

@ -114,24 +114,23 @@ NavState NavState::inverse() const {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) {
// Get angular velocity w, translational velocity v, and acceleration a // Get angular velocity w and components rho (for t) and nu (for v) from xi
Vector3 w = xi.head<3>(); Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>();
Vector3 rho = xi.segment<3>(3);
Vector3 nu = xi.tail<3>();
// Compute rotation using Expmap // Compute rotation using Expmap
Rot3 R = Rot3::Expmap(w); Matrix3 Jr;
Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
// Compute translations and optionally their Jacobians // Compute translations and optionally their Jacobians Q in w
// The Jacobians with respect to rho and nu are equal to Jr
Matrix3 Qt, Qv; Matrix3 Qt, Qv;
Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr, R); Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr);
Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr, R); Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr);
if (Hxi) { if (Hxi) {
const Matrix3 Jw = Rot3::ExpmapDerivative(w); *Hxi << Jr, Z_3x3, Z_3x3, //
*Hxi << Jw, Z_3x3, Z_3x3, Qt, Jr, Z_3x3, //
Qt, Jw, Z_3x3, Qv, Z_3x3, Jr;
Qv, Z_3x3, Jw;
} }
return NavState(R, t, v); return NavState(R, t, v);
@ -235,8 +234,8 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
Matrix3 Qt, Qv; Matrix3 Qt, Qv;
const Rot3 R = Rot3::Expmap(w); const Rot3 R = Rot3::Expmap(w);
Pose3::ExpmapTranslation(w, rho, Qt, R); Pose3::ExpmapTranslation(w, rho, Qt);
Pose3::ExpmapTranslation(w, nu, Qv, R); Pose3::ExpmapTranslation(w, nu, Qv);
const Matrix3 Jw = Rot3::LogmapDerivative(w); const Matrix3 Jw = Rot3::LogmapDerivative(w);
const Matrix3 Qt2 = -Jw * Qt * Jw; const Matrix3 Qt2 = -Jw * Qt * Jw;
const Matrix3 Qv2 = -Jw * Qv * Jw; const Matrix3 Qv2 = -Jw * Qv * Jw;