Merge pull request #1932 from borglab/feature/leftRightJacobians
Left and Right SO(3) Jacobiansrelease/4.3a0
Frank Dellaert
GitHub
commit
5125844d19
|
|
@ -69,6 +69,16 @@ Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1,
|
|||
p.x() * q.y() - p.y() * q.x());
|
||||
}
|
||||
|
||||
Point3 doubleCross(const Point3 &p, const Point3 &q, //
|
||||
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) {
|
||||
if (H1) *H1 = q.dot(p) * I_3x3 + p * q.transpose() - 2 * q * p.transpose();
|
||||
if (H2) {
|
||||
const Matrix3 W = skewSymmetric(p);
|
||||
*H2 = W * W;
|
||||
}
|
||||
return gtsam::cross(p, gtsam::cross(p, q));
|
||||
}
|
||||
|
||||
double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
|
||||
OptionalJacobian<1, 3> H2) {
|
||||
if (H1) *H1 << q.x(), q.y(), q.z();
|
||||
|
|
|
|||
|
|
@ -55,11 +55,16 @@ GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = {});
|
|||
/// normalize, with optional Jacobian
|
||||
GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = {});
|
||||
|
||||
/// cross product @return this x q
|
||||
/// cross product @return p x q
|
||||
GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q,
|
||||
OptionalJacobian<3, 3> H_p = {},
|
||||
OptionalJacobian<3, 3> H_q = {});
|
||||
|
||||
/// double cross product @return p x (p x q)
|
||||
GTSAM_EXPORT Point3 doubleCross(const Point3& p, const Point3& q,
|
||||
OptionalJacobian<3, 3> H1 = {},
|
||||
OptionalJacobian<3, 3> H2 = {});
|
||||
|
||||
/// dot product
|
||||
GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
|
||||
OptionalJacobian<1, 3> H_p = {},
|
||||
|
|
|
|||
|
|
@ -168,14 +168,14 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
|
|||
const Vector3 w = xi.head<3>(), v = xi.tail<3>();
|
||||
|
||||
// Compute rotation using Expmap
|
||||
Rot3 R = Rot3::Expmap(w);
|
||||
Matrix3 Jw;
|
||||
Rot3 R = Rot3::Expmap(w, Hxi ? &Jw : nullptr);
|
||||
|
||||
// Compute translation and optionally its Jacobian in w
|
||||
Matrix3 Q;
|
||||
const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr, R);
|
||||
|
||||
if (Hxi) {
|
||||
const Matrix3 Jw = Rot3::ExpmapDerivative(w);
|
||||
*Hxi << Jw, Z_3x3,
|
||||
Q, Jw;
|
||||
}
|
||||
|
|
@ -238,14 +238,37 @@ 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) {
|
||||
Matrix3 Q;
|
||||
const auto w = xi.head<3>();
|
||||
const auto v = xi.tail<3>();
|
||||
ExpmapTranslation(w, v, Q, {}, nearZeroThreshold);
|
||||
return Q;
|
||||
return pose3::ExpmapFunctor(w).computeQ(v);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -256,39 +279,13 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
|
|||
const double theta2 = w.dot(w);
|
||||
bool nearZero = (theta2 <= nearZeroThreshold);
|
||||
|
||||
if (Q) {
|
||||
const Matrix3 V = skewSymmetric(v);
|
||||
const Matrix3 W = skewSymmetric(w);
|
||||
const Matrix3 WVW = W * V * W;
|
||||
const double theta = w.norm();
|
||||
if (Q) *Q = pose3::ExpmapFunctor(w, nearZero).computeQ(v);
|
||||
|
||||
if (nearZero) {
|
||||
static constexpr double one_sixth = 1. / 6.;
|
||||
static constexpr double one_twenty_fourth = 1. / 24.;
|
||||
static constexpr double one_one_hundred_twentieth = 1. / 120.;
|
||||
|
||||
*Q = -0.5 * V + one_sixth * (W * V + V * W - WVW) -
|
||||
one_twenty_fourth * (W * W * V + V * W * W - 3 * WVW) +
|
||||
one_one_hundred_twentieth * (WVW * W + W * WVW);
|
||||
} else {
|
||||
const double s = sin(theta), c = cos(theta);
|
||||
const double theta3 = theta2 * theta, theta4 = theta3 * theta,
|
||||
theta5 = theta4 * theta;
|
||||
|
||||
// Invert the sign of odd-order terms to have the right Jacobian
|
||||
*Q = -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) +
|
||||
(1 - theta2 / 2 - c) / theta4 * (W * W * V + V * W * W - 3 * WVW) -
|
||||
0.5 *
|
||||
((1 - theta2 / 2 - c) / theta4 -
|
||||
3 * (theta - s - theta3 / 6.) / theta5) *
|
||||
(WVW * W + W * WVW);
|
||||
}
|
||||
}
|
||||
|
||||
// TODO(Frank): this threshold is *different*. Why?
|
||||
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));
|
||||
|
|
|
|||
|
|
@ -18,13 +18,14 @@
|
|||
* @date December 2014
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/concepts.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/SO3.h>
|
||||
|
||||
#include <Eigen/SVD>
|
||||
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -41,7 +42,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) {
|
|||
return H;
|
||||
}
|
||||
|
||||
GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) {
|
||||
GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
|
||||
OptionalJacobian<9, 9> H) {
|
||||
Matrix3 MR = M * R.matrix();
|
||||
if (H) *H = Dcompose(R);
|
||||
return MR;
|
||||
|
|
@ -51,85 +53,107 @@ void ExpmapFunctor::init(bool nearZeroApprox) {
|
|||
nearZero =
|
||||
nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
|
||||
if (!nearZero) {
|
||||
sin_theta = std::sin(theta);
|
||||
const double sin_theta = std::sin(theta);
|
||||
A = sin_theta / theta;
|
||||
const double s2 = std::sin(theta / 2.0);
|
||||
one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
|
||||
const double one_minus_cos =
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
|
||||
: theta2(omega.dot(omega)), theta(std::sqrt(theta2)) {
|
||||
const double wx = omega.x(), wy = omega.y(), wz = omega.z();
|
||||
W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
|
||||
: theta2(omega.dot(omega)),
|
||||
theta(std::sqrt(theta2)),
|
||||
W(skewSymmetric(omega)),
|
||||
WW(W * W) {
|
||||
init(nearZeroApprox);
|
||||
if (!nearZero) {
|
||||
K = W / theta;
|
||||
KK = K * K;
|
||||
}
|
||||
}
|
||||
|
||||
ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
|
||||
bool nearZeroApprox)
|
||||
: theta2(angle * angle), theta(angle) {
|
||||
const double ax = axis.x(), ay = axis.y(), az = axis.z();
|
||||
K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
|
||||
W = K * angle;
|
||||
: theta2(angle * angle),
|
||||
theta(angle),
|
||||
W(skewSymmetric(axis * angle)),
|
||||
WW(W * W) {
|
||||
init(nearZeroApprox);
|
||||
if (!nearZero) {
|
||||
KK = K * K;
|
||||
}
|
||||
}
|
||||
|
||||
SO3 ExpmapFunctor::expmap() const {
|
||||
if (nearZero)
|
||||
return SO3(I_3x3 + W);
|
||||
else
|
||||
return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK);
|
||||
}
|
||||
SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); }
|
||||
|
||||
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
|
||||
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
|
||||
if (nearZero) {
|
||||
dexp_ = I_3x3 - 0.5 * W;
|
||||
} else {
|
||||
a = one_minus_cos / theta;
|
||||
b = 1.0 - sin_theta / theta;
|
||||
dexp_ = I_3x3 - a * K + b * KK;
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
return B * Wv;
|
||||
}
|
||||
|
||||
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:
|
||||
// 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;
|
||||
}
|
||||
return C * WWv;
|
||||
}
|
||||
|
||||
// Multiplies v with left Jacobian through vector operations only.
|
||||
Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
|
||||
OptionalJacobian<3, 3> H2) const {
|
||||
if (H1) {
|
||||
if (nearZero) {
|
||||
*H1 = 0.5 * skewSymmetric(v);
|
||||
} else {
|
||||
// TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
|
||||
const Vector3 Kv = K * v;
|
||||
const double Da = (sin_theta - 2.0 * a) / theta2;
|
||||
const double Db = (one_minus_cos - 3.0 * b) / theta2;
|
||||
*H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() -
|
||||
skewSymmetric(Kv * b / theta) +
|
||||
(a * I_3x3 - b * K) * skewSymmetric(v / theta);
|
||||
}
|
||||
}
|
||||
if (H2) *H2 = dexp_;
|
||||
return dexp_ * v;
|
||||
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;
|
||||
}
|
||||
|
||||
Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
|
||||
OptionalJacobian<3, 3> H2) const {
|
||||
const Matrix3 invDexp = dexp_.inverse();
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
if (H2) *H2 = leftJacobian();
|
||||
return v + BWv + CWWv;
|
||||
}
|
||||
|
||||
} // namespace so3
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -168,12 +192,7 @@ SO3 SO3::ChordalMean(const std::vector<SO3>& rotations) {
|
|||
template <>
|
||||
GTSAM_EXPORT
|
||||
Matrix3 SO3::Hat(const Vector3& xi) {
|
||||
// skew symmetric matrix X = xi^
|
||||
Matrix3 Y = Z_3x3;
|
||||
Y(0, 1) = -xi(2);
|
||||
Y(0, 2) = +xi(1);
|
||||
Y(1, 2) = -xi(0);
|
||||
return Y - Y.transpose();
|
||||
return skewSymmetric(xi);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
|||
|
|
@ -26,7 +26,6 @@
|
|||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/dllexport.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -133,16 +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:
|
||||
const double theta2;
|
||||
Matrix3 W, K, KK;
|
||||
struct GTSAM_EXPORT ExpmapFunctor {
|
||||
const double theta2, theta;
|
||||
const Matrix3 W, WW;
|
||||
bool nearZero;
|
||||
double theta, sin_theta, one_minus_cos; // only defined if !nearZero
|
||||
|
||||
void init(bool nearZeroApprox = false);
|
||||
// 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
|
||||
|
||||
public:
|
||||
/// Constructor with element of Lie algebra so(3)
|
||||
explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
|
||||
|
||||
|
|
@ -151,34 +149,57 @@ class GTSAM_EXPORT ExpmapFunctor {
|
|||
|
||||
/// Rodrigues formula
|
||||
SO3 expmap() const;
|
||||
|
||||
protected:
|
||||
void init(bool nearZeroApprox = false);
|
||||
};
|
||||
|
||||
/// Functor that implements Exponential map *and* its derivatives
|
||||
class DexpFunctor : public ExpmapFunctor {
|
||||
struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
|
||||
const Vector3 omega;
|
||||
double a, b;
|
||||
Matrix3 dexp_;
|
||||
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)
|
||||
GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
|
||||
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) */
|
||||
const Matrix3& dexp() const { return dexp_; }
|
||||
// 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
|
||||
inline Matrix3 dexp() const { return rightJacobian(); }
|
||||
|
||||
/// 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
|
||||
GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
|
||||
Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
|
||||
OptionalJacobian<3, 3> H2 = {}) const;
|
||||
|
||||
/// Multiplies with dexp().inverse(), with optional derivatives
|
||||
GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v,
|
||||
OptionalJacobian<3, 3> H1 = {},
|
||||
Vector3 applyInvDexp(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;
|
||||
|
||||
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;
|
||||
};
|
||||
} // namespace so3
|
||||
|
||||
|
|
|
|||
|
|
@ -154,6 +154,17 @@ TEST( Point3, cross2) {
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Point3, doubleCross) {
|
||||
Matrix aH1, aH2;
|
||||
std::function<Point3(const Point3&, const Point3&)> f =
|
||||
[](const Point3& p, const Point3& q) { return doubleCross(p, q); };
|
||||
const Point3 omega(1, 2, 3), theta(4, 5, 6);
|
||||
doubleCross(omega, theta, aH1, aH2);
|
||||
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST (Point3, normalize) {
|
||||
Matrix actualH;
|
||||
|
|
|
|||
|
|
@ -845,7 +845,28 @@ TEST( Pose3, ExpmapDerivative1) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3, ExpmapDerivative2) {
|
||||
TEST( Pose3, ExpmapDerivative2) {
|
||||
Matrix6 actualH;
|
||||
Vector6 w; w << 1.0, -2.0, 3.0, -10.0, -20.0, 30.0;
|
||||
Pose3::Expmap(w,actualH);
|
||||
Matrix expectedH = numericalDerivative21<Pose3, Vector6,
|
||||
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
|
||||
EXPECT(assert_equal(expectedH, actualH));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose3, ExpmapDerivative3) {
|
||||
Matrix6 actualH;
|
||||
Vector6 w; w << 0.0, 0.0, 0.0, -10.0, -20.0, 30.0;
|
||||
Pose3::Expmap(w,actualH);
|
||||
Matrix expectedH = numericalDerivative21<Pose3, Vector6,
|
||||
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
|
||||
// Small angle approximation is not as precise as numerical derivative?
|
||||
EXPECT(assert_equal(expectedH, actualH, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3, ExpmapDerivative4) {
|
||||
// Iserles05an (Lie-group Methods) says:
|
||||
// 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)
|
||||
|
|
|
|||
|
|
@ -303,19 +303,61 @@ 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> 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, CrossB) {
|
||||
Matrix aH1;
|
||||
for (bool nearZero : {true, false}) {
|
||||
std::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
[=](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 =
|
||||
[=](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) {
|
||||
Matrix aH1, aH2;
|
||||
for (bool nearZeroApprox : {true, false}) {
|
||||
for (bool nearZero : {true, false}) {
|
||||
std::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
[=](const Vector3& omega, const Vector3& v) {
|
||||
return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
|
||||
return so3::DexpFunctor(omega, nearZero).applyDexp(v);
|
||||
};
|
||||
for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
|
||||
Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
|
||||
so3::DexpFunctor local(omega, nearZeroApprox);
|
||||
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
|
||||
Vector3(0.4, 0.3, 0.2)}) {
|
||||
for (const Vector3& omega : test_cases::omegas(nearZero)) {
|
||||
so3::DexpFunctor local(omega, nearZero);
|
||||
for (const Vector3& v : test_cases::vs) {
|
||||
EXPECT(assert_equal(Vector3(local.dexp() * v),
|
||||
local.applyDexp(v, aH1, aH2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
|
||||
|
|
@ -327,19 +369,38 @@ TEST(SO3, ApplyDexp) {
|
|||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3, ApplyInvDexp) {
|
||||
TEST(SO3, ApplyLeftJacobian) {
|
||||
Matrix aH1, aH2;
|
||||
for (bool nearZeroApprox : {true, false}) {
|
||||
for (bool nearZero : {true, false}) {
|
||||
std::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
[=](const Vector3& omega, const Vector3& v) {
|
||||
return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
|
||||
return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v);
|
||||
};
|
||||
for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
|
||||
Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
|
||||
so3::DexpFunctor local(omega, nearZeroApprox);
|
||||
for (const Vector3& omega : test_cases::omegas(nearZero)) {
|
||||
so3::DexpFunctor local(omega, nearZero);
|
||||
for (const Vector3& v : test_cases::vs) {
|
||||
EXPECT(assert_equal(Vector3(local.leftJacobian() * v),
|
||||
local.applyLeftJacobian(v, aH1, aH2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
|
||||
EXPECT(assert_equal(local.leftJacobian(), aH2));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
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 invDexp = local.dexp().inverse();
|
||||
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
|
||||
Vector3(0.4, 0.3, 0.2)}) {
|
||||
for (const Vector3& v : test_cases::vs) {
|
||||
EXPECT(assert_equal(Vector3(invDexp * v),
|
||||
local.applyInvDexp(v, aH1, aH2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
|
||||
|
|
|
|||
Loading…
Reference in New Issue