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

Merge pull request #2099 from borglab/feature/LogmapDerivative 

Revamped DexpFunctor and LogmapDerivative
release/4.3a0
Frank Dellaert 2025-04-22 10:11:54 -04:00 committed by GitHub
parent 3a1188a2ee a04b7e1a55
commit c9498fe0c0
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
15 changed files with 958 additions and 404 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
gtsam/geometry/Pose3.cpp
View File

@ -219,8 +219,7 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
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);
const so3::DexpFunctor local(w);
// Compute rotation using Expmap
#ifdef GTSAM_USE_QUATERNIONS
@ -256,25 +255,17 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
/* ************************************************************************* */
Vector6 Pose3::Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose) {
if (Hpose) *Hpose = LogmapDerivative(pose);
const Vector3 w = Rot3::Logmap(pose.rotation());
const Vector3 T = pose.translation();
const double t = w.norm();
if (t < 1e-10) {
Vector6 log;
log << w, T;
return log;
} else {
const Matrix3 W = skewSymmetric(w / t);
// Formula from Agrawal06iros, equation (14)
// simplified with Mathematica, and multiplying in T to avoid matrix math
const double Tan = tan(0.5 * t);
const Vector3 WT = W * T;
const Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);
Vector6 log;
log << w, u;
return log;
}
// Instantiate functor for Dexp-related operations:
const so3::DexpFunctor local(w);
const Vector3 t = pose.translation();
const Vector3 u = local.applyLeftJacobianInverse(t);
Vector6 xi;
xi << w, u;
if (Hpose) *Hpose = LogmapDerivative(xi);
return xi;
}
/* ************************************************************************* */
@ -309,25 +300,6 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
#endif
}
/* ************************************************************************* */
Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
double nearZeroThreshold) {
const auto w = xi.head<3>();
const auto v = xi.tail<3>();
// Instantiate functor for Dexp-related operations:
bool nearZero = (w.dot(w) <= nearZeroThreshold);
so3::DexpFunctor local(w, nearZero);
// Call applyLeftJacobian to get its Jacobian
Matrix3 H;
local.applyLeftJacobian(v, H);
// Multiply with R^T to account for the Pose3::Create Jacobian.
const Matrix3 R = local.expmap();
return R.transpose() * H;
}
/* ************************************************************************* */
Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) {
Matrix6 J;
@ -335,16 +307,35 @@ Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) {
return J;
}
/* ************************************************************************* */
Matrix6 Pose3::LogmapDerivative(const Vector6& xi) {
const Vector3 w = xi.head<3>();
Vector3 v = xi.segment<3>(3);
// Instantiate functor for Dexp-related operations:
const so3::DexpFunctor local(w);
// Call applyLeftJacobian to get its Jacobians
Matrix3 H_t_w;
local.applyLeftJacobian(v, H_t_w);
// Multiply with R^T to account for NavState::Create Jacobian.
const Matrix3 R = local.expmap();
const Matrix3 Qt = R.transpose() * H_t_w;
// Now compute the blocks of the LogmapDerivative Jacobian
const Matrix3 Jw = Rot3::LogmapDerivative(w);
const Matrix3 Qt2 = -Jw * Qt * Jw;
Matrix6 J;
J << Jw, Z_3x3, Qt2, Jw;
return J;
}
/* ************************************************************************* */
Matrix6 Pose3::LogmapDerivative(const Pose3& pose) {
const Vector6 xi = Logmap(pose);
const Vector3 w = xi.head<3>();
const Matrix3 Jw = Rot3::LogmapDerivative(w);
const Matrix3 Q = ComputeQforExpmapDerivative(xi);
const Matrix3 Q2 = -Jw*Q*Jw;
Matrix6 J;
J << Jw, Z_3x3, Q2, Jw;
return J;
return LogmapDerivative(xi);
}
/* ************************************************************************* */

17
gtsam/geometry/Pose3.h
View File

@ -209,7 +209,10 @@ public:
static Matrix6 ExpmapDerivative(const Vector6& xi);
/// Derivative of Logmap
static Matrix6 LogmapDerivative(const Pose3& xi);
static Matrix6 LogmapDerivative(const Vector6& xi);
/// Derivative of Logmap, Pose3 version. TODO(Frank): deprecate?
static Matrix6 LogmapDerivative(const Pose3& pose);
// Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
struct GTSAM_EXPORT ChartAtOrigin {
@ -217,18 +220,6 @@ public:
static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {});
};
/**
* Compute the 3x3 bottom-left block Q of SE3 Expmap right derivative matrix
* J_r(xi) = [J_(w) Z_3x3;
* Q_r J_(w)]
* where J_(w) is the SO3 Expmap right derivative.
* (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.
*/
static Matrix3 ComputeQforExpmapDerivative(
const Vector6& xi, double nearZeroThreshold = 1e-5);
using LieGroup<Pose3, 6>::inverse; // version with derivative
/**

13
gtsam/geometry/Rot3.h
View File

@ -374,20 +374,13 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
using LieAlgebra = Matrix3;
/**
* Exponential map at identity - create a rotation from canonical coordinates
* Exponential map - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
*/
static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = {}) {
if(H) *H = Rot3::ExpmapDerivative(v);
#ifdef GTSAM_USE_QUATERNIONS
return traits<gtsam::Quaternion>::Expmap(v);
#else
return Rot3(traits<SO3>::Expmap(v));
#endif
}
static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = {});
/**
* Log map at identity - returns the canonical coordinates
* Log map - returns the canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ of this rotation
*/
static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = {});

6
gtsam/geometry/Rot3M.cpp
View File

@ -153,7 +153,11 @@ Point3 Rot3::rotate(const Point3& p,
}
/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Rot3 Rot3::Expmap(const Vector3& v, OptionalJacobian<3,3> H) {
return Rot3(SO3::Expmap(v, H));
}
/* ************************************************************************* */
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
return SO3::Logmap(R.rot_,H);
}

6
gtsam/geometry/Rot3Q.cpp
View File

@ -116,6 +116,12 @@ namespace gtsam {
return Point3(r.x(), r.y(), r.z());
}
/* ************************************************************************* */
Rot3 Rot3::Expmap(const Vector3& v, OptionalJacobian<3,3> H) {
if(H) *H = Rot3::ExpmapDerivative(v);
return traits<gtsam::Quaternion>::Expmap(v);
}
/* ************************************************************************* */
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
return traits<gtsam::Quaternion>::Logmap(R.quaternion_, H);

176
gtsam/geometry/SO3.cpp
View File

@ -42,6 +42,20 @@ 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;
static constexpr double kPi_inv = 1.0 / M_PI;
static constexpr double kPi2 = M_PI * M_PI;
static constexpr double k1_Pi2 = 1.0 / kPi2;
static constexpr double kPi3 = M_PI * kPi2;
static constexpr double k1_Pi3 = 1.0 / kPi3;
static constexpr double k2_Pi3 = 2.0 * k1_Pi3;
static constexpr double k1_4Pi = 0.25 * kPi_inv; // 1/(4*pi)
// --- Thresholds ---
// Tolerance for near zero (theta^2)
static constexpr double kNearZeroThresholdSq = 1e-6;
// Tolerance for near pi (delta^2 = (pi - theta)^2)
static constexpr double kNearPiThresholdSq = 1e-6;
GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) {
Matrix99 H;
auto R = Q.matrix();
@ -58,10 +72,11 @@ GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
return MR;
}
void ExpmapFunctor::init(bool nearZeroApprox) {
nearZero =
nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
void ExpmapFunctor::init(double nearZeroThresholdSq) {
nearZero = (theta2 <= nearZeroThresholdSq);
if (!nearZero) {
// General case: Use standard stable formulas for A and B
const double sin_theta = std::sin(theta);
A = sin_theta / theta;
const double s2 = std::sin(theta / 2.0);
@ -69,36 +84,51 @@ void ExpmapFunctor::init(bool nearZeroApprox) {
2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
B = one_minus_cos / theta2;
} else {
// Taylor expansion at 0
// Taylor expansion at 0 for A, B (Order theta^2)
A = 1.0 - theta2 * one_6th;
B = 0.5 - theta2 * one_24th;
}
}
ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
ExpmapFunctor::ExpmapFunctor(const Vector3& omega) :ExpmapFunctor(kNearZeroThresholdSq, omega) {}
ExpmapFunctor::ExpmapFunctor(double nearZeroThresholdSq, const Vector3& omega)
: theta2(omega.dot(omega)),
theta(std::sqrt(theta2)),
W(skewSymmetric(omega)),
WW(W * W) {
init(nearZeroApprox);
init(nearZeroThresholdSq);
}
ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
bool nearZeroApprox)
ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle)
: theta2(angle * angle),
theta(angle),
W(skewSymmetric(axis * angle)),
WW(W * W) {
init(nearZeroApprox);
init(kNearZeroThresholdSq);
}
Matrix3 ExpmapFunctor::expmap() const { return I_3x3 + A * W + B * WW; }
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
DexpFunctor::DexpFunctor(const Vector3& omega, double nearZeroThresholdSq, double nearPiThresholdSq)
: ExpmapFunctor(nearZeroThresholdSq, omega), omega(omega) {
if (!nearZero) {
C = (1 - A) / theta2;
// General case or nearPi: Use standard stable formulas first
C = (1.0 - A) / theta2; // Usually stable, even near pi (1-0)/pi^2
// Calculate delta = pi - theta (non-negative) for nearPi check
const double delta = M_PI > theta ? M_PI - theta : 0.0;
const double delta2 = delta * delta;
const bool nearPi = (delta2 < nearPiThresholdSq);
if (nearPi) {
// Taylor expansion near pi *only for D* (Order delta)
D = k1_Pi2 + (k2_Pi3 - k1_4Pi) * delta; // D ~ 1/pi^2 + delta*(2/pi^3 - 1/(4*pi))
} else {
// General case D:
D = (1.0 - A / (2.0 * B)) / theta2;
}
// Calculate E and F using standard formulas (stable near pi)
E = (2.0 * B - A) / theta2;
F = (3.0 * C - B) / theta2;
} else {
@ -111,51 +141,46 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
}
}
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 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;
DexpFunctor::DexpFunctor(const Vector3& omega)
: DexpFunctor(omega, kNearZeroThresholdSq, kNearPiThresholdSq) {}
Matrix3 DexpFunctor::rightJacobianInverse() const {
if (theta > M_PI) return rightJacobian().inverse();
return I_3x3 + 0.5 * W + D * WW;
}
Vector3 DexpFunctor::doubleCrossC(const Vector3& v,
OptionalJacobian<3, 3> H) const {
// WWv = omega x (omega x * v)
Matrix3 doubleCrossJacobian;
const Vector3 WWv =
gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr);
if (H) {
// 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;
Matrix3 DexpFunctor::leftJacobianInverse() const {
if (theta > M_PI) return leftJacobian().inverse();
return I_3x3 - 0.5 * W + D * WW;
}
// Multiplies v with left Jacobian through vector operations only.
Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
Vector3 DexpFunctor::applyRightJacobian(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
Matrix3 D_BWv_w, D_CWWv_w;
const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr);
const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr);
if (H1) *H1 = -D_BWv_w + D_CWWv_w;
if (H2) *H2 = rightJacobian();
return v - BWv + CWWv;
const Vector3 Wv = gtsam::cross(omega, v);
Matrix3 WWv_H_w;
const Vector3 WWv = gtsam::doubleCross(omega, v, H1 ? &WWv_H_w : nullptr);
if (H1) {
// - E * omega.transpose() is 1x3 Jacobian of B with respect to omega
Matrix3 BWv_H_w = -Wv * E * omega.transpose() - B * skewSymmetric(v);
// - F * omega.transpose() is 1x3 Jacobian of C with respect to omega
Matrix3 CWWv_H_w = -WWv * F * omega.transpose() + C * WWv_H_w;
*H1 = -BWv_H_w + CWWv_H_w;
}
Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
if (H2) *H2 = rightJacobian();
return v - B * Wv + C * WWv;
}
Vector3 DexpFunctor::applyRightJacobianInverse(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
const Matrix3 invJr = rightJacobianInverse();
const Vector3 c = invJr * v;
if (H1) {
Matrix3 H;
applyDexp(c, H); // get derivative H of forward mapping
applyRightJacobian(c, H); // get derivative H of forward mapping
*H1 = -invJr * H;
}
if (H2) *H2 = invJr;
@ -163,19 +188,26 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
}
Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
Matrix3 D_BWv_w, D_CWWv_w;
const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr);
const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr);
if (H1) *H1 = D_BWv_w + D_CWWv_w;
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const {
const Vector3 Wv = gtsam::cross(omega, v);
Matrix3 WWv_H_w;
const Vector3 WWv = gtsam::doubleCross(omega, v, H1 ? &WWv_H_w : nullptr);
if (H1) {
// - E * omega.transpose() is 1x3 Jacobian of B with respect to omega
Matrix3 BWv_H_w = -Wv * E * omega.transpose() - B * skewSymmetric(v);
// - F * omega.transpose() is 1x3 Jacobian of C with respect to omega
Matrix3 CWWv_H_w = -WWv * F * omega.transpose() + C * WWv_H_w;
*H1 = BWv_H_w + CWWv_H_w;
}
if (H2) *H2 = leftJacobian();
return v + BWv + CWWv;
return v + B * Wv + C * WWv;
}
Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v,
OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const {
const Matrix3 invJl = leftJacobianInverse();
const Vector3 c = invJl * v;
if (H1) {
@ -250,50 +282,24 @@ Matrix3 SO3::AdjointMap() const {
template <>
GTSAM_EXPORT
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
if (H) {
so3::DexpFunctor local(omega);
*H = local.dexp();
if (H) *H = local.rightJacobian();
return SO3(local.expmap());
} else {
return SO3(so3::ExpmapFunctor(omega).expmap());
}
}
template <>
GTSAM_EXPORT
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
return so3::DexpFunctor(omega).dexp();
return so3::DexpFunctor(omega).rightJacobian();
}
//******************************************************************************
/* Right Jacobian for Log map in SO(3) - equation (10.86) and following
equations in G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie
Groups", Volume 2, 2008.
logmap( Rhat * expmap(omega) ) \approx logmap(Rhat) + Jrinv * omega
where Jrinv = LogmapDerivative(omega). This maps a perturbation on the
manifold (expmap(omega)) to a perturbation in the tangent space (Jrinv *
omega)
*/
template <>
GTSAM_EXPORT
Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
using std::cos;
using std::sin;
double theta2 = omega.dot(omega);
if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
double theta = std::sqrt(theta2); // rotation angle
// element of Lie algebra so(3): W = omega^
const Matrix3 W = Hat(omega);
return I_3x3 + 0.5 * W +
(1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) *
W * W;
return so3::DexpFunctor(omega).rightJacobianInverse();
}
//******************************************************************************
template <>
GTSAM_EXPORT
Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
@ -306,9 +312,7 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
const double &R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
const double &R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
// Get trace(R)
const double tr = R.trace();
Vector3 omega;
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
@ -357,7 +361,7 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
} else {
double magnitude;
const double tr_3 = tr - 3.0; // could be non-negative if the matrix is off orthogonal
if (tr_3 < -1e-6) {
if (tr_3 < -so3::kNearZeroThresholdSq) {
// this is the normal case -1 < trace < 3
double theta = acos((tr - 1.0) / 2.0);
magnitude = theta / (2.0 * sin(theta));
@ -365,7 +369,7 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
// use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
// see https://github.com/borglab/gtsam/issues/746 for details
magnitude = 0.5 - tr_3 / 12.0 + tr_3*tr_3/60.0;
magnitude = 0.5 - tr_3 * so3::one_12th + tr_3 * tr_3 * so3::one_60th;
}
omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
}

61
gtsam/geometry/SO3.h
View File

@ -137,23 +137,26 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
struct GTSAM_EXPORT ExpmapFunctor {
const double theta2, theta;
const Matrix3 W, WW;
bool nearZero;
bool nearZero{ false };
// Ethan Eade's constants:
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);
explicit ExpmapFunctor(const Vector3& omega);
/// Constructor with threshold (advanced)
ExpmapFunctor(double nearZeroThresholdSq, const Vector3& axis);
/// Constructor with axis-angle
ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
ExpmapFunctor(const Vector3& axis, double angle);
/// Rodrigues formula
Matrix3 expmap() const;
protected:
void init(bool nearZeroApprox = false);
void init(double nearZeroThresholdSq);
};
/// Functor that implements Exponential map *and* its derivatives
@ -173,7 +176,10 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
double F; // (3C - B) / theta2
/// Constructor with element of Lie algebra so(3)
explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
explicit DexpFunctor(const Vector3& omega);
/// Constructor with custom thresholds (advanced)
explicit DexpFunctor(const Vector3& omega, double nearZeroThresholdSq, double nearPiThresholdSq);
// NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
// (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
@ -186,40 +192,37 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
// Compute the left Jacobian for Exponential map in SO(3)
Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; }
/// 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; }
/// For |omega|>pi uses rightJacobian().inverse(), as unstable beyond pi!
Matrix3 rightJacobianInverse() const;
// Inverse of left Jacobian
Matrix3 leftJacobianInverse() const { return I_3x3 - 0.5 * W + D * WW; }
/// For |omega|>pi uses leftJacobian().inverse(), as unstable beyond pi!
Matrix3 leftJacobianInverse() const;
/// Synonym for rightJacobianInverse
inline Matrix3 invDexp() const { return rightJacobianInverse(); }
/// Multiplies with rightJacobian(), with optional derivatives
Vector3 applyRightJacobian(const Vector3& v,
OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const;
/// Computes B * (omega x v).
Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
/// Computes C * (omega x (omega x v)).
Vector3 doubleCrossC(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
/// Multiplies with dexp(), with optional derivatives
Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
/// Multiplies with dexp().inverse(), with optional derivatives
Vector3 applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
/// Multiplies with rightJacobian().inverse(), with optional derivatives
Vector3 applyRightJacobianInverse(const Vector3& v,
OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const;
/// Multiplies with leftJacobian(), with optional derivatives
Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
Vector3 applyLeftJacobian(const Vector3& v,
OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const;
/// Multiplies with leftJacobianInverse(), with optional derivatives
Vector3 applyLeftJacobianInverse(const Vector3& v,
OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
/// @deprecated: use rightJacobian
inline Matrix3 dexp() const { return rightJacobian(); }
/// @deprecated: use rightJacobianInverse
inline Matrix3 invDexp() const { return rightJacobianInverse(); }
#endif
};
} // namespace so3

95
gtsam/geometry/doc/Rot3.ipynb
View File

@ -1014,7 +1014,7 @@
"source": [
"### Lie group operations\n",
"\n",
"`Rot3` implements the Lie group operations for exponential mapping and log mapping. For more information on Lie groups and their use here, see [GTSAM concepts](https://gtsam.org/notes/GTSAM-Concepts.html)."
"`Rot3` implements the Lie group operations for exponential mapping and log mapping. For more information on Lie groups and their use here, see [GTSAM concepts](https://gtsam.org/notes/GTSAM-Concepts.html), and some detailed math in the [`SO3`](SO3.ipynb) class."
]
},
{
@ -1023,51 +1023,7 @@
"id": "MmBfK0ad1KZ6"
},
"source": [
"The exponential map for $\\text{SO}(3)$ converts a 3D rotation vector (Lie algebra element in $\\mathfrak{so}(3)$) into a rotation matrix (Lie group element in $\\text{SO}(3)$). This is used to map a rotation vector $\\boldsymbol{\\omega} \\in \\mathbb{R}^3$ to a rotation matrix $R \\in \\text{SO}(3)$.\n",
"\n",
"Given a rotation vector $\\boldsymbol{\\omega} = (\\omega_x, \\omega_y, \\omega_z)$, define:\n",
"\n",
"- The rotation axis as the unit vector:\n",
" \n",
" $$\n",
" \\hat{\\omega} = \\frac{\\boldsymbol{\\omega}}{\\theta}, \\quad \\text{where } \\theta = \\|\\boldsymbol{\\omega}\\|\n",
" $$\n",
"\n",
"- The skew-symmetric matrix $[\\boldsymbol{\\omega}]_\\times$:\n",
"\n",
" $$\n",
" [\\boldsymbol{\\omega}]_\\times =\n",
" \\begin{bmatrix}\n",
" 0 & -\\omega_z & \\omega_y \\\\\n",
" \\omega_z & 0 & -\\omega_x \\\\\n",
" -\\omega_y & \\omega_x & 0\n",
" \\end{bmatrix}\n",
" $$\n",
"\n",
"Using Rodrigues' formula, the exponential map is:\n",
"\n",
"$$\n",
"\\exp([\\boldsymbol{\\omega}]_\\times) = I + \\frac{\\sin\\theta}{\\theta} [\\boldsymbol{\\omega}]_\\times + \\frac{1 - \\cos\\theta}{\\theta^2} [\\boldsymbol{\\omega}]_\\times^2\n",
"$$\n",
"\n",
"which results in the rotation matrix:\n",
"\n",
"$$\n",
"R = I + \\frac{\\sin\\theta}{\\theta} [\\boldsymbol{\\omega}]_\\times + \\frac{1 - \\cos\\theta}{\\theta^2} [\\boldsymbol{\\omega}]_\\times^2\n",
"$$\n",
"\n",
"where:\n",
"- $ I $ is the $ 3 \\times 3 $ identity matrix.\n",
"- $ \\theta $ is the magnitude of the rotation vector (rotation angle).\n",
"- $ [\\boldsymbol{\\omega}]_\\times $ represents the skew-symmetric matrix, the infinitesimal rotation generator.\n",
"\n",
"For very small $ \\theta $, we use the Taylor series approximation:\n",
"\n",
"$$\n",
"R \\approx I + [\\boldsymbol{\\omega}]_\\times\n",
"$$\n",
"\n",
"since $ \\sin\\theta \\approx \\theta$ and $ 1 - \\cos\\theta \\approx \\frac{\\theta^2}{2} $."
"The exponential map for $\\text{SO}(3)$ converts a 3D rotation vector (corresponding to a Lie algebra element in $\\mathfrak{so}(3)$) into a rotation matrix (Lie group element in $\\text{SO}(3)$). I.e., we map a rotation vector $\\boldsymbol{\\omega} \\in \\mathbb{R}^3$ to a rotation matrix $R \\in \\text{SO}(3)$."
]
},
{
@ -1126,52 +1082,7 @@
"id": "Yk2nazsK6ixV"
},
"source": [
"The logarithm map for $ \\text{SO}(3) $ is the inverse of the exponential map It converts a rotation matrix $ R \\in SO(3) $ into a 3D rotation vector (a Lie algebra element in $ \\mathfrak{so}(3) $).\n",
"\n",
"Given a rotation matrix $ R $, the corresponding rotation vector $ \\boldsymbol{\\omega} \\in \\mathbb{R}^3 $ is computed as:\n",
"\n",
"$$\n",
"\\boldsymbol{\\omega} = \\theta \\hat{\\omega}\n",
"$$\n",
"\n",
"where:\n",
"- $ \\theta $ is the rotation angle:\n",
" \n",
" $$\n",
" \\theta = \\cos^{-1} \\left( \\frac{\\text{Tr}(R) - 1}{2} \\right)\n",
" $$\n",
"\n",
"- $ \\hat{\\omega} $ is the rotation axis, obtained from the skew-symmetric matrix $ [\\boldsymbol{\\omega}]_\\times $:\n",
"\n",
" $$\n",
" [\\boldsymbol{\\omega}]_\\times = \\frac{\\theta}{2 \\sin\\theta} (R - R^T)\n",
" $$\n",
"\n",
"- Extracting the components:\n",
"\n",
" $$\n",
" \\boldsymbol{\\omega} =\n",
" \\frac{\\theta}{2 \\sin\\theta} (R - R^T)^\\vee\n",
" $$\n",
"\n",
"For small $ \\theta $, we use the Taylor series approximation:\n",
"\n",
"$$\n",
"\\boldsymbol{\\omega} \\approx \\frac{1}{2} (R - R^T)^\\vee\n",
"$$\n",
"\n",
"In both cases, $ (R - R^T)^\\vee $ extracts the unique 3D vector from a skew-symmetric matrix:\n",
"\n",
"$$\n",
"(R - R^T)^\\vee=\n",
"\\begin{bmatrix}\n",
" R_{32} - R_{23} \\\\\n",
" R_{13} - R_{31} \\\\\n",
" R_{21} - R_{12}\n",
" \\end{bmatrix}\n",
"$$\n",
"\n",
"where $ R_{ij} $ are the elements of $ R $."
"The logarithm map for $ \\text{SO}(3) $ is the inverse of the exponential map It converts a rotation matrix $ R \\in SO(3) $ into a 3D rotation vector (corresponding to a Lie algebra element in $ \\mathfrak{so}(3) $)."
]
},
{

558
gtsam/geometry/doc/SO3.ipynb Normal file
View File

File diff suppressed because one or more lines are too long

66
gtsam/geometry/geometry.i
View File

@ -198,6 +198,39 @@ class Rot2 {
};
#include <gtsam/geometry/SO3.h>
namespace so3 {
class ExpmapFunctor {
const double theta2;
const double theta;
const Matrix3 W;
const Matrix3 WW;
const bool nearZero;
double A; // A = sin(theta) / theta
double B; // B = (1 - cos(theta))
ExpmapFunctor(const gtsam::Vector3& omega);
ExpmapFunctor(double nearZeroThresholdSq, const gtsam::Vector3& axis);
ExpmapFunctor(const gtsam::Vector3& axis, double angle);
gtsam::Matrix3 expmap() const;
};
virtual class DexpFunctor : gtsam::so3::ExpmapFunctor {
const gtsam::Vector3 omega;
const double C; // (1 - A) / theta^2
const double D; // (1 - A/2B) / theta2
DexpFunctor(const gtsam::Vector3& omega);
DexpFunctor(const gtsam::Vector3& omega, double nearZeroThresholdSq, double nearPiThresholdSq);
gtsam::Matrix3 rightJacobian() const;
gtsam::Matrix3 leftJacobian() const;
gtsam::Matrix3 rightJacobianInverse() const;
gtsam::Matrix3 leftJacobianInverse() const;
gtsam::Vector3 applyRightJacobian(const gtsam::Vector3& v) const;
gtsam::Vector3 applyRightJacobianInverse(const gtsam::Vector3& v) const;
gtsam::Vector3 applyLeftJacobian(const gtsam::Vector3& v) const;
gtsam::Vector3 applyLeftJacobianInverse(const gtsam::Vector3& v) const;
};
}
class SO3 {
// Standard Constructors
SO3();
@ -220,21 +253,22 @@ class SO3 {
gtsam::SO3 operator*(const gtsam::SO3& R) const;
// Lie Group
static gtsam::SO3 Expmap(gtsam::Vector v);
static gtsam::Vector Logmap(const gtsam::SO3& p);
gtsam::SO3 expmap(gtsam::Vector v);
gtsam::Vector logmap(const gtsam::SO3& p);
static gtsam::Matrix Hat(const gtsam::Vector& xi);
static gtsam::Vector Vee(const gtsam::Matrix& xi);
static gtsam::SO3 Expmap(gtsam::Vector3 v);
static gtsam::Vector3 Logmap(const gtsam::SO3& p);
static gtsam::Matrix3 ExpmapDerivative(const gtsam::Vector3& omega);
static gtsam::Matrix3 LogmapDerivative(const gtsam::Vector3& omega);
gtsam::SO3 expmap(gtsam::Vector3 v);
gtsam::Vector3 logmap(const gtsam::SO3& p);
static gtsam::Matrix3 Hat(const gtsam::Vector3& xi);
static gtsam::Vector3 Vee(const gtsam::Matrix3& xi);
// Manifold
gtsam::SO3 retract(gtsam::Vector v) const;
gtsam::Vector localCoordinates(const gtsam::SO3& R) const;
static gtsam::SO3 Expmap(gtsam::Vector v);
gtsam::SO3 retract(gtsam::Vector3 v) const;
gtsam::Vector3 localCoordinates(const gtsam::SO3& R) const;
// Other methods
gtsam::Vector vec() const;
gtsam::Matrix matrix() const;
gtsam::Vector3 vec() const;
gtsam::Matrix3 matrix() const;
};
#include <gtsam/geometry/SO4.h>
@ -376,6 +410,8 @@ class Rot3 {
// Lie group
static gtsam::Rot3 Expmap(gtsam::Vector v);
static gtsam::Vector Logmap(const gtsam::Rot3& p);
static gtsam::Matrix3 ExpmapDerivative(const gtsam::Vector3& omega);
static gtsam::Matrix3 LogmapDerivative(const gtsam::Vector3& omega);
gtsam::Rot3 expmap(const gtsam::Vector& v);
gtsam::Vector logmap(const gtsam::Rot3& p);
static gtsam::Matrix Hat(const gtsam::Vector& xi);
@ -532,12 +568,17 @@ class Pose3 {
// Lie Group
static gtsam::Pose3 Expmap(gtsam::Vector v);
static gtsam::Vector Logmap(const gtsam::Pose3& p);
static gtsam::Matrix6 ExpmapDerivative(const gtsam::Vector6& xi);
static gtsam::Matrix6 LogmapDerivative(const gtsam::Vector6& xi);
static gtsam::Matrix6 LogmapDerivative(const gtsam::Pose3& xi);
static gtsam::Pose3 Expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H);
static gtsam::Vector Logmap(const gtsam::Pose3& p, Eigen::Ref<Eigen::MatrixXd> H);
gtsam::Pose3 expmap(gtsam::Vector v);
gtsam::Pose3 expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2);
gtsam::Vector logmap(const gtsam::Pose3& p);
gtsam::Vector logmap(const gtsam::Pose3& p, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2);
gtsam::Matrix AdjointMap() const;
gtsam::Vector Adjoint(gtsam::Vector xi) const;
gtsam::Vector Adjoint(gtsam::Vector xi, Eigen::Ref<Eigen::MatrixXd> H_this,
@ -550,8 +591,7 @@ class Pose3 {
static gtsam::Matrix adjointMap_(gtsam::Vector xi);
static gtsam::Vector adjoint_(gtsam::Vector xi, gtsam::Vector y);
static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y);
static gtsam::Matrix ExpmapDerivative(gtsam::Vector xi);
static gtsam::Matrix LogmapDerivative(const gtsam::Pose3& xi);
static gtsam::Matrix Hat(const gtsam::Vector& xi);
static gtsam::Vector Vee(const gtsam::Matrix& xi);

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

@ -891,13 +891,13 @@ TEST(Pose3, ExpmapDerivative) {
//******************************************************************************
namespace test_cases {
std::vector<Vector3> small{{0, 0, 0}, //
static const 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},
static const 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},
auto omegas = [](bool nearZero) -> const std::vector<Vector3>&{ return nearZero ? small : large; };
static const 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
@ -936,7 +936,9 @@ TEST(Pose3, Logmap) {
TEST(Pose3, LogmapDerivatives) {
for (bool nearZero : {true, false}) {
for (const Vector3& w : test_cases::omegas(nearZero)) {
std::cout << "w: " << w.transpose() << std::endl;
for (Vector3 v : test_cases::vs) {
std::cout << "v: " << v.transpose() << std::endl;
const Vector6 xi = (Vector6() << w, v).finished();
Pose3 pose = Pose3::Expmap(xi);
const Matrix6 expectedH =
@ -956,6 +958,59 @@ TEST(Pose3, LogmapDerivatives) {
}
}
//******************************************************************************
TEST(Pose3, LogmapDerivative) {
// Copied from testSO3.cpp
const Rot3 R2((Matrix3() << // Near pi
-0.750767, -0.0285082, -0.659952,
-0.0102558, -0.998445, 0.0547974,
-0.660487, 0.0479084, 0.749307).finished());
const Rot3 R3((Matrix3() << // Near pi
-0.747473, -0.00190019, -0.664289,
-0.0385114, -0.99819, 0.0461892,
-0.663175, 0.060108, 0.746047).finished());
const Rot3 R4((Matrix3() << // Final pose in a drone experiment
0.324237, 0.902975, 0.281968,
-0.674322, 0.429668, -0.600562,
-0.663445, 0.00458662, 0.748211).finished());
// Now creates poses
const Pose3 T0; // Identity
const Vector6 xi(0.1, -0.1, 0.1, 0.1, -0.1, 0.1);
const Pose3 T1 = Pose3::Expmap(xi); // Small rotation
const Pose3 T2(R2, Point3(1, 2, 3));
const Pose3 T3(R3, Point3(1, 2, 3));
const Pose3 T4(R4, Point3(1, 2, 3));
size_t i = 0;
for (const Pose3& T : { T0, T1, T2, T3, T4 }) {
const bool nearPi = (i == 2 || i == 3); // Flag cases near pi
Matrix6 actualH; // H computed by Logmap(T, H) using LogmapDerivative(xi)
const Vector6 xi = Pose3::Logmap(T, actualH);
// 1. Check self-consistency of analytical derivative calculation:
// Does the H returned by Logmap match an independent calculation
// of J_r^{-1} using ExpmapDerivative with the computed xi?
Matrix6 J_r_inv = Pose3::ExpmapDerivative(xi).inverse(); // J_r^{-1}
EXPECT(assert_equal(J_r_inv, actualH)); // This test is crucial and should pass
// 2. Check analytical derivative against numerical derivative:
// Only perform this check AWAY from the pi singularity, where
// numerical differentiation of Logmap is expected to be reliable
// and should match the analytical derivative.
if (!nearPi) {
const Matrix expectedH = numericalDerivative11<Vector6, Pose3>(
std::bind(&Pose3::Logmap, std::placeholders::_1, nullptr), T, 1e-7);
EXPECT(assert_equal(expectedH, actualH, 1e-5)); // 1e-5 needed to pass R4
}
else {
// We accept that the numerical derivative of this specific Logmap implementation
// near pi will not match the standard analytical derivative J_r^{-1}.
}
i++;
}
}
/* ************************************************************************* */
Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) {
return Pose3::adjointMap(xi) * v;

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

@ -274,42 +274,70 @@ TEST(SO3, ExpmapDerivative5) {
//******************************************************************************
TEST(SO3, ExpmapDerivative6) {
const Vector3 thetahat(0.1, 0, 0.1);
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
std::bind(&SO3::Expmap, std::placeholders::_1, nullptr), thetahat);
Matrix3 Jactual;
SO3::Expmap(thetahat, Jactual);
EXPECT(assert_equal(Jexpected, Jactual));
const Vector3 theta(0.1, 0, 0.1);
const Matrix expectedH = numericalDerivative11<SO3, Vector3>(
std::bind(&SO3::Expmap, std::placeholders::_1, nullptr), theta);
Matrix3 actualH;
SO3::Expmap(theta, actualH);
EXPECT(assert_equal(expectedH, actualH));
}
//******************************************************************************
TEST(SO3, LogmapDerivative) {
const Vector3 thetahat(0.1, 0, 0.1);
const SO3 R = SO3::Expmap(thetahat); // some rotation
const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
std::bind(&SO3::Logmap, std::placeholders::_1, nullptr), R);
const Matrix3 Jactual = SO3::LogmapDerivative(thetahat);
EXPECT(assert_equal(Jexpected, Jactual));
}
const SO3 R0; // Identity
const Vector3 omega1(0.1, 0, 0.1);
const SO3 R1 = SO3::Expmap(omega1); // Small rotation
const SO3 R2((Matrix3() << // Near pi
-0.750767, -0.0285082, -0.659952,
-0.0102558, -0.998445, 0.0547974,
-0.660487, 0.0479084, 0.749307).finished());
const SO3 R3((Matrix3() << // Near pi
-0.747473, -0.00190019, -0.664289,
-0.0385114, -0.99819, 0.0461892,
-0.663175, 0.060108, 0.746047).finished());
const SO3 R4((Matrix3() << // Final pose in a drone experiment
0.324237, 0.902975, 0.281968,
-0.674322, 0.429668, -0.600562,
-0.663445, 0.00458662, 0.748211).finished());
size_t i = 0;
for (const SO3& R : { R0, R1, R2, R3, R4 }) {
const bool nearPi = (i == 2 || i == 3); // Flag cases near pi
//******************************************************************************
TEST(SO3, JacobianLogmap) {
const Vector3 thetahat(0.1, 0, 0.1);
const SO3 R = SO3::Expmap(thetahat); // some rotation
const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
std::bind(&SO3::Logmap, std::placeholders::_1, nullptr), R);
Matrix3 Jactual;
SO3::Logmap(R, Jactual);
EXPECT(assert_equal(Jexpected, Jactual));
}
Matrix3 actualH; // H computed by Logmap(R, H) using LogmapDerivative(omega)
const Vector3 omega = SO3::Logmap(R, actualH);
// 1. Check self-consistency of analytical derivative calculation:
// Does the H returned by Logmap match an independent calculation
// of J_r^{-1} using DexpFunctor with the computed omega?
so3::DexpFunctor local(omega);
Matrix3 J_r_inv = local.rightJacobianInverse(); // J_r^{-1} via DexpFunctor
EXPECT(assert_equal(J_r_inv, actualH)); // This test is crucial and should pass
// 2. Check analytical derivative against numerical derivative:
// Only perform this check AWAY from the pi singularity, where
// numerical differentiation of Logmap is expected to be reliable
// and should match the analytical derivative.
if (!nearPi) {
const Matrix expectedH = numericalDerivative11<Vector3, SO3>(
std::bind(&SO3::Logmap, std::placeholders::_1, nullptr), R, 1e-7);
EXPECT(assert_equal(expectedH, actualH, 1e-6)); // 1e-6 needed to pass R4
}
else {
// We accept that the numerical derivative of this specific Logmap implementation
// near pi will not match the standard analytical derivative J_r^{-1}.
}
i++;
}
}
//******************************************************************************
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}, {0.1, 0.2, 0.3}};
{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3},
{0.4, 0.5, 0.6}, {0.7, 0.8, 0.9}, {1.1, 1.2, 1.3}, {1.4, 1.5, 1.6},
{1.7, 1.8, 1.9}, {2, 2, 2}, {3, 3, 3}, {4, 4, 4}, {5, 5, 5} };
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
@ -319,7 +347,7 @@ TEST(SO3, JacobianInverses) {
Matrix HR, HL;
for (bool nearZero : {true, false}) {
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5);
EXPECT(assert_equal<Matrix3>(local.rightJacobian().inverse(),
local.rightJacobianInverse()));
EXPECT(assert_equal<Matrix3>(local.leftJacobian().inverse(),
@ -329,76 +357,40 @@ TEST(SO3, JacobianInverses) {
}
//******************************************************************************
TEST(SO3, CrossB) {
Matrix aH1;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).crossB(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
for (const Vector3& v : test_cases::vs) {
local.crossB(v, aH1);
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
}
}
}
}
//******************************************************************************
TEST(SO3, DoubleCrossC) {
Matrix aH1;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).doubleCrossC(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
for (const Vector3& v : test_cases::vs) {
local.doubleCrossC(v, aH1);
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
}
}
}
}
//******************************************************************************
TEST(SO3, ApplyDexp) {
TEST(SO3, ApplyRightJacobian) {
Matrix aH1, aH2;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyDexp(v);
return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyRightJacobian(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5);
for (const Vector3& v : test_cases::vs) {
EXPECT(assert_equal(Vector3(local.dexp() * v),
local.applyDexp(v, aH1, aH2)));
EXPECT(assert_equal(Vector3(local.rightJacobian() * v),
local.applyRightJacobian(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(local.dexp(), aH2));
EXPECT(assert_equal(local.rightJacobian(), aH2));
}
}
}
}
//******************************************************************************
TEST(SO3, ApplyInvDexp) {
TEST(SO3, ApplyRightJacobianInverse) {
Matrix aH1, aH2;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyInvDexp(v);
return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyRightJacobianInverse(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5);
Matrix invJr = local.rightJacobianInverse();
for (const Vector3& v : test_cases::vs) {
EXPECT(
assert_equal(Vector3(invJr * v), local.applyInvDexp(v, aH1, aH2)));
assert_equal(Vector3(invJr * v), local.applyRightJacobianInverse(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(invJr, aH2));
@ -413,10 +405,10 @@ TEST(SO3, ApplyLeftJacobian) {
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v);
return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyLeftJacobian(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5);
for (const Vector3& v : test_cases::vs) {
EXPECT(assert_equal(Vector3(local.leftJacobian() * v),
local.applyLeftJacobian(v, aH1, aH2)));
@ -434,10 +426,10 @@ TEST(SO3, ApplyLeftJacobianInverse) {
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyLeftJacobianInverse(v);
return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyLeftJacobianInverse(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5);
Matrix invJl = local.leftJacobianInverse();
for (const Vector3& v : test_cases::vs) {
EXPECT(assert_equal(Vector3(invJl * v),

15
gtsam/navigation/NavState.cpp
View File

@ -118,8 +118,7 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) {
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);
const so3::DexpFunctor local(w);
// Compute rotation using Expmap
#ifdef GTSAM_USE_QUATERNIONS
@ -238,15 +237,13 @@ Matrix9 NavState::ExpmapDerivative(const Vector9& xi) {
}
//------------------------------------------------------------------------------
Matrix9 NavState::LogmapDerivative(const NavState& state) {
const Vector9 xi = Logmap(state);
Matrix9 NavState::LogmapDerivative(const Vector9& xi) {
const Vector3 w = xi.head<3>();
Vector3 rho = xi.segment<3>(3);
Vector3 nu = xi.tail<3>();
// Instantiate functor for Dexp-related operations:
const bool nearZero = (w.dot(w) <= 1e-5);
const so3::DexpFunctor local(w, nearZero);
const so3::DexpFunctor local(w);
// Call applyLeftJacobian to get its Jacobians
Matrix3 H_t_w, H_v_w;
@ -270,6 +267,12 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
return J;
}
//------------------------------------------------------------------------------
Matrix9 NavState::LogmapDerivative(const NavState& state) {
const Vector9 xi = Logmap(state);
return LogmapDerivative(xi);
}
//------------------------------------------------------------------------------
Matrix5 NavState::Hat(const Vector9& xi) {
Matrix5 X;

3
gtsam/navigation/NavState.h
View File

@ -245,6 +245,9 @@ public:
static Matrix9 ExpmapDerivative(const Vector9& xi);
/// Derivative of Logmap
static Matrix9 LogmapDerivative(const Vector9& xi);
/// Derivative of Logmap, NavState version
static Matrix9 LogmapDerivative(const NavState& xi);
// Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP

4
gtsam/navigation/TangentPreintegration.cpp
View File

@ -66,7 +66,7 @@ Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body,
// Calculate exact mean propagation
Matrix3 w_tangent_H_theta, invH;
const Vector3 w_tangent = // angular velocity mapped back to tangent space
local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
local.applyRightJacobianInverse(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
const Rot3 R(local.expmap()); // nRb: rotation of body in nav frame
const Vector3 a_nav = R * a_body;
const double dt22 = 0.5 * dt * dt;
@ -79,7 +79,7 @@ Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body,
if (A) {
// Exact derivative of R*a with respect to theta:
const Matrix3 a_nav_H_theta = R.matrix() * skewSymmetric(-a_body) * local.dexp();
const Matrix3 a_nav_H_theta = R.matrix() * skewSymmetric(-a_body) * local.rightJacobian();
A->setIdentity();
A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta