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

92
gtsam/geometry/Pose3.cpp
View File

@ -11,13 +11,14 @@
/**
* @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/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/concepts.h>
#include <cmath>
#include <iostream>
@ -162,22 +163,24 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
}
/* ************************************************************************* */
/** 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) {
// 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);
@ -238,60 +241,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;
}
/* ************************************************************************* */
@ -320,7 +313,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;

12
gtsam/geometry/Pose3.h
View File

@ -11,7 +11,7 @@
/**
*@file Pose3.h
*@brief 3D Pose
* @brief 3D Pose manifold SO(3) x R^3 and group SE(3)
*/
// \callgraph
@ -223,17 +223,19 @@ public:
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

73
gtsam/geometry/SO3.cpp
View File

@ -33,6 +33,15 @@ namespace gtsam {
//******************************************************************************
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) {
Matrix99 H;
auto R = Q.matrix();
@ -60,9 +69,9 @@ void ExpmapFunctor::init(bool nearZeroApprox) {
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;
// Taylor expansion at 0
A = 1.0 - theta2 * one_6th;
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)
: 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 {
// 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 {
// 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;
}
@ -111,10 +130,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;
}
@ -132,14 +151,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;
}
@ -154,6 +173,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
//******************************************************************************

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 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 {
const double theta2, theta;
const Matrix3 W, WW;
bool nearZero;
// Ethan Eade's constants:
double A; // A = sin(theta) / theta or 1 for theta->0
double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0
double A; // A = sin(theta) / theta
double B; // B = (1 - cos(theta))
/// Constructor with element of Lie algebra so(3)
explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
@ -155,12 +157,20 @@ struct GTSAM_EXPORT ExpmapFunctor {
};
/// 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 {
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
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; // (2B - A) / theta2
double F; // (3C - B) / theta2
/// Constructor with element of Lie algebra so(3)
explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
@ -179,6 +189,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;
@ -197,9 +216,10 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
Vector3 applyLeftJacobian(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;
/// Multiplies with leftJacobianInverse(), with optional derivatives
Vector3 applyLeftJacobianInverse(const Vector3& v,
OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
};
} // namespace so3

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

@ -835,38 +835,7 @@ TEST(Pose3, Align2) {
}
/* ************************************************************************* */
TEST( Pose3, ExpmapDerivative1) {
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) {
TEST(Pose3, ExpmapDerivative) {
// Iserles05an (Lie-group Methods) says:
// 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)
@ -900,26 +869,71 @@ TEST(Pose3, ExpmapDerivative4) {
}
}
TEST( Pose3, ExpmapDerivativeQr) {
Vector6 w = Vector6::Random();
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));
//******************************************************************************
namespace test_cases {
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{{0, 0, 0}, {1, 0, 0}, {0, 1, 0},
{0, 0, 1}, {.1, .2, .3}, {1, -2, 3}};
auto omegas = [](bool nearZero) { return nearZero ? small : large; };
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) {
Matrix6 actualH;
Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
Pose3 p = Pose3::Expmap(w);
EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5));
Matrix expectedH = numericalDerivative21<Vector6, Pose3,
OptionalJacobian<6, 6> >(&Pose3::Logmap, p, {});
//******************************************************************************
// Check logmap for all small values, as we don't want wrapping.
TEST(Pose3, Logmap) {
static constexpr bool nearZero = true;
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);
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));
}
//******************************************************************************
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{
{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; };
std::vector<Vector3> vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}};
} // 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) {
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) {
Matrix aH1, aH2;
@ -390,22 +429,22 @@ TEST(SO3, ApplyLeftJacobian) {
}
//******************************************************************************
TEST(SO3, ApplyInvDexp) {
TEST(SO3, ApplyLeftJacobianInverse) {
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);
return so3::DexpFunctor(omega, nearZero).applyLeftJacobianInverse(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
Matrix invDexp = local.dexp().inverse();
Matrix invJl = local.leftJacobianInverse();
for (const Vector3& v : test_cases::vs) {
EXPECT(assert_equal(Vector3(invDexp * v),
local.applyInvDexp(v, aH1, aH2)));
EXPECT(assert_equal(Vector3(invJl * v),
local.applyLeftJacobianInverse(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
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) {
// Get angular velocity w, translational velocity v, and acceleration a
Vector3 w = xi.head<3>();
Vector3 rho = xi.segment<3>(3);
Vector3 nu = xi.tail<3>();
// 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>();
// 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;
Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr, R);
Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr, R);
Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr);
Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr);
if (Hxi) {
const Matrix3 Jw = Rot3::ExpmapDerivative(w);
*Hxi << Jw, Z_3x3, Z_3x3,
Qt, Jw, Z_3x3,
Qv, Z_3x3, Jw;
*Hxi << Jr, Z_3x3, Z_3x3, //
Qt, Jr, Z_3x3, //
Qv, Z_3x3, Jr;
}
return NavState(R, t, v);
@ -235,8 +234,8 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
Matrix3 Qt, Qv;
const Rot3 R = Rot3::Expmap(w);
Pose3::ExpmapTranslation(w, rho, Qt, R);
Pose3::ExpmapTranslation(w, nu, Qv, R);
Pose3::ExpmapTranslation(w, rho, Qt);
Pose3::ExpmapTranslation(w, nu, Qv);
const Matrix3 Jw = Rot3::LogmapDerivative(w);
const Matrix3 Qt2 = -Jw * Qt * Jw;
const Matrix3 Qv2 = -Jw * Qv * Jw;