Merge pull request #1938 from borglab/feature/betterSE3Jacobian
Better SE(3) and SE2(3) Jacobiansrelease/4.3a0
Frank Dellaert
GitHub
commit
8c903c2515
|
|
@ -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>
|
||||
|
|
@ -111,7 +112,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) {
|
||||
|
|
@ -158,26 +159,28 @@ 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?) */
|
||||
// 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;
|
||||
|
|
@ -338,14 +330,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;
|
||||
|
|
@ -354,7 +346,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();
|
||||
|
|
@ -378,7 +370,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();
|
||||
|
|
@ -484,7 +476,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++) {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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, {});
|
||||
EXPECT(assert_equal(expectedH, actualH));
|
||||
//******************************************************************************
|
||||
// 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
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue