Final touches
Frank Dellaert
parent
d547fe2ec1
commit
0c1f087dba
|
|
@ -240,12 +240,10 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
namespace pose3 {
|
||||
class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
|
||||
protected:
|
||||
struct GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
|
||||
// Constant used in computeQ
|
||||
double F; // (B - 0.5) / theta2 or -1/24 for theta->0
|
||||
|
||||
public:
|
||||
ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
|
||||
: so3::DexpFunctor(omega, nearZeroApprox) {
|
||||
F = nearZero ? _one_twenty_fourth : (B - 0.5) / theta2;
|
||||
|
|
@ -253,7 +251,7 @@ class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
|
|||
|
||||
// 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 its derivatives in w and v.
|
||||
// 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;
|
||||
|
|
@ -261,7 +259,6 @@ class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
|
|||
F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW);
|
||||
}
|
||||
|
||||
protected:
|
||||
static constexpr double _one_twenty_fourth = - 1.0 / 24.0;
|
||||
};
|
||||
} // namespace pose3
|
||||
|
|
|
|||
|
|
@ -26,7 +26,6 @@
|
|||
|
||||
#include <Eigen/SVD>
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -94,7 +93,7 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
|
|||
}
|
||||
|
||||
Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const {
|
||||
// Wv = omega x * v
|
||||
// Wv = omega x v
|
||||
const Vector3 Wv = gtsam::cross(omega, v);
|
||||
if (H) {
|
||||
// Apply product rule:
|
||||
|
|
@ -123,10 +122,10 @@ Vector3 DexpFunctor::doubleCrossC(const Vector3& v,
|
|||
// Multiplies v with left Jacobian through vector operations only.
|
||||
Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
|
||||
OptionalJacobian<3, 3> H2) const {
|
||||
Matrix3 D_BWv_omega, D_CWWv_omega;
|
||||
const Vector3 BWv = crossB(v, D_BWv_omega);
|
||||
const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega);
|
||||
if (H1) *H1 = -D_BWv_omega + D_CWWv_omega;
|
||||
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;
|
||||
}
|
||||
|
|
@ -136,9 +135,9 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
|
|||
const Matrix3 invDexp = rightJacobian().inverse();
|
||||
const Vector3 c = invDexp * v;
|
||||
if (H1) {
|
||||
Matrix3 D_dexpv_omega;
|
||||
applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping
|
||||
*H1 = -invDexp * D_dexpv_omega;
|
||||
Matrix3 D_dexpv_w;
|
||||
applyDexp(c, D_dexpv_w); // get derivative H of forward mapping
|
||||
*H1 = -invDexp * D_dexpv_w;
|
||||
}
|
||||
if (H2) *H2 = invDexp;
|
||||
return c;
|
||||
|
|
@ -147,10 +146,10 @@ 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_omega, D_CWWv_omega;
|
||||
const Vector3 BWv = crossB(v, D_BWv_omega);
|
||||
const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega);
|
||||
if (H1) *H1 = D_BWv_omega + D_CWWv_omega;
|
||||
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 = leftJacobian();
|
||||
return v + BWv + CWWv;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -132,18 +132,15 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
|
|||
// functor also implements dedicated methods to apply dexp and/or inv(dexp).
|
||||
|
||||
/// Functor implementing Exponential map
|
||||
class GTSAM_EXPORT ExpmapFunctor {
|
||||
protected:
|
||||
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
|
||||
|
||||
void init(bool nearZeroApprox = false);
|
||||
|
||||
public:
|
||||
/// Constructor with element of Lie algebra so(3)
|
||||
explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
|
||||
|
||||
|
|
@ -152,33 +149,34 @@ class GTSAM_EXPORT ExpmapFunctor {
|
|||
|
||||
/// Rodrigues formula
|
||||
SO3 expmap() const;
|
||||
|
||||
protected:
|
||||
void init(bool nearZeroApprox = false);
|
||||
};
|
||||
|
||||
/// Functor that implements Exponential map *and* its derivatives
|
||||
class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
|
||||
protected:
|
||||
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
|
||||
// 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
|
||||
|
||||
public:
|
||||
/// Constructor with element of Lie algebra so(3)
|
||||
explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
|
||||
|
||||
// NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
|
||||
// (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
|
||||
// Information Theory, and Lie Groups", Volume 2, 2008.
|
||||
// expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
|
||||
// This maps a perturbation v in the tangent space to
|
||||
// a perturbation on the manifold Expmap(dexp * v)
|
||||
// Expmap(xi + dxi) \approx Expmap(xi) * Expmap(dexp * dxi)
|
||||
// This maps a perturbation dxi=(w,v) in the tangent space to
|
||||
// a perturbation on the manifold Expmap(dexp * xi)
|
||||
Matrix3 rightJacobian() const { return I_3x3 - B * W + C * WW; }
|
||||
|
||||
// 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
|
||||
/// Differential of expmap == right Jacobian
|
||||
inline Matrix3 dexp() const { return rightJacobian(); }
|
||||
|
||||
/// Computes B * (omega x v).
|
||||
|
|
@ -199,7 +197,6 @@ class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
|
|||
Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
|
||||
OptionalJacobian<3, 3> H2 = {}) const;
|
||||
|
||||
protected:
|
||||
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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue