Revamp Thresholds in SO3
Frank Dellaert
parent
ac6735bb4d
commit
f213665354
|
|
@ -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
|
||||
|
|
@ -259,8 +258,7 @@ Vector6 Pose3::Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose) {
|
|||
const Vector3 w = Rot3::Logmap(pose.rotation());
|
||||
|
||||
// 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);
|
||||
|
||||
const Vector3 t = pose.translation();
|
||||
const Vector3 u = local.applyLeftJacobianInverse(t);
|
||||
|
|
@ -315,8 +313,7 @@ Matrix6 Pose3::LogmapDerivative(const Vector6& xi) {
|
|||
Vector3 v = xi.segment<3>(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;
|
||||
|
|
|
|||
|
|
@ -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,6 +141,9 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
|
|||
}
|
||||
}
|
||||
|
||||
DexpFunctor::DexpFunctor(const Vector3& omega)
|
||||
: DexpFunctor(omega, kNearZeroThresholdSq, kNearPiThresholdSq) {}
|
||||
|
||||
Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const {
|
||||
// Wv = omega x v
|
||||
const Vector3 Wv = gtsam::cross(omega, v);
|
||||
|
|
@ -275,8 +308,7 @@ Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
|
|||
template <>
|
||||
GTSAM_EXPORT
|
||||
Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
|
||||
const bool nearZero = (omega.dot(omega) <= 1e-5);
|
||||
return so3::DexpFunctor(omega, nearZero).rightJacobianInverse();
|
||||
return so3::DexpFunctor(omega).rightJacobianInverse();
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -292,58 +324,36 @@ 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.
|
||||
// we do something special
|
||||
if (tr + 1.0 < 1e-3) {
|
||||
if (R33 > R22 && R33 > R11) {
|
||||
// R33 is the largest diagonal, a=3, b=1, c=2
|
||||
const double W = R21 - R12;
|
||||
if (tr + 1.0 < so3::kNearPiThresholdSq) {
|
||||
// Used below
|
||||
auto omegaFromQ = [](double W, double r, double w1, double w2, double w3) {
|
||||
const double one_over_r = 1 / r;
|
||||
const double norm = sqrt(w1 * w1 + w2 * w2 + w3 * w3 + W * W);
|
||||
const double sgn_w = W < 0 ? -1.0 : 1.0;
|
||||
const double mag = M_PI - (2 * sgn_w * W) / norm;
|
||||
const double scale = 0.5 * one_over_r * mag;
|
||||
return sgn_w * scale * Vector3(w1, w2, w3);
|
||||
};
|
||||
|
||||
if (R33 > R22 && R33 > R11) { // R33 is the largest diagonal, a=3, b=1, c=2
|
||||
const double Q1 = 2.0 + 2.0 * R33;
|
||||
const double Q2 = R31 + R13;
|
||||
const double Q3 = R23 + R32;
|
||||
const double r = sqrt(Q1);
|
||||
const double one_over_r = 1 / r;
|
||||
const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
|
||||
const double sgn_w = W < 0 ? -1.0 : 1.0;
|
||||
const double mag = M_PI - (2 * sgn_w * W) / norm;
|
||||
const double scale = 0.5 * one_over_r * mag;
|
||||
omega = sgn_w * scale * Vector3(Q2, Q3, Q1);
|
||||
} else if (R22 > R11) {
|
||||
// R22 is the largest diagonal, a=2, b=3, c=1
|
||||
const double W = R13 - R31;
|
||||
omega = omegaFromQ(R21 - R12, sqrt(Q1), R31 + R13, R23 + R32, Q1);
|
||||
} else if (R22 > R11) { // R22 is the largest diagonal, a=2, b=3, c=1
|
||||
const double Q1 = 2.0 + 2.0 * R22;
|
||||
const double Q2 = R23 + R32;
|
||||
const double Q3 = R12 + R21;
|
||||
const double r = sqrt(Q1);
|
||||
const double one_over_r = 1 / r;
|
||||
const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
|
||||
const double sgn_w = W < 0 ? -1.0 : 1.0;
|
||||
const double mag = M_PI - (2 * sgn_w * W) / norm;
|
||||
const double scale = 0.5 * one_over_r * mag;
|
||||
omega = sgn_w * scale * Vector3(Q3, Q1, Q2);
|
||||
} else {
|
||||
// R11 is the largest diagonal, a=1, b=2, c=3
|
||||
const double W = R32 - R23;
|
||||
omega = omegaFromQ(R13 - R31, sqrt(Q1), R23 + R32, Q1, R12 + R21);
|
||||
} else { // R11 is the largest diagonal, a=1, b=2, c=3
|
||||
const double Q1 = 2.0 + 2.0 * R11;
|
||||
const double Q2 = R12 + R21;
|
||||
const double Q3 = R31 + R13;
|
||||
const double r = sqrt(Q1);
|
||||
const double one_over_r = 1 / r;
|
||||
const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
|
||||
const double sgn_w = W < 0 ? -1.0 : 1.0;
|
||||
const double mag = M_PI - (2 * sgn_w * W) / norm;
|
||||
const double scale = 0.5 * one_over_r * mag;
|
||||
omega = sgn_w * scale * Vector3(Q1, Q2, Q3);
|
||||
omega = omegaFromQ(R32 - R23, sqrt(Q1), Q1, R12 + R21, R31 + R13);
|
||||
}
|
||||
} 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::kNearPiThresholdSq) {
|
||||
// this is the normal case -1 < trace < 3
|
||||
double theta = acos((tr - 1.0) / 2.0);
|
||||
magnitude = theta / (2.0 * sin(theta));
|
||||
|
|
@ -351,7 +361,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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
protected:
|
||||
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,
|
||||
|
|
|
|||
|
|
@ -274,44 +274,67 @@ 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());
|
||||
|
||||
//******************************************************************************
|
||||
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));
|
||||
}
|
||||
size_t i = 0;
|
||||
for (const SO3& R : { R0, R1, R2, R3 }) {
|
||||
const bool nearPi = (i == 2 || i == 3); // Flag cases near pi
|
||||
|
||||
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)); // Use default tolerance
|
||||
}
|
||||
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}, //
|
||||
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}};
|
||||
{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
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -319,7 +342,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(),
|
||||
|
|
@ -334,10 +357,10 @@ TEST(SO3, CrossB) {
|
|||
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);
|
||||
return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).crossB(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) {
|
||||
local.crossB(v, aH1);
|
||||
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
|
||||
|
|
@ -352,10 +375,10 @@ TEST(SO3, DoubleCrossC) {
|
|||
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);
|
||||
return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).doubleCrossC(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) {
|
||||
local.doubleCrossC(v, aH1);
|
||||
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
|
||||
|
|
@ -370,10 +393,10 @@ TEST(SO3, ApplyDexp) {
|
|||
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).applyDexp(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)));
|
||||
|
|
@ -391,10 +414,10 @@ TEST(SO3, ApplyInvDexp) {
|
|||
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).applyInvDexp(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(
|
||||
|
|
@ -413,10 +436,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 +457,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),
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -244,8 +243,7 @@ Matrix9 NavState::LogmapDerivative(const Vector9& xi) {
|
|||
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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue