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

Taylor expansion

release/4.3a0
Frank Dellaert 2024-12-16 18:35:17 -05:00
parent 2aa36d4f7a
commit db5b9dee65
3 changed files with 83 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
gtsam/geometry/SO3.cpp
View File

@ -33,6 +33,15 @@ namespace gtsam {
//******************************************************************************
namespace so3 {
static constexpr double one_6th = 1.0 / 6.0;
static constexpr double one_12th = 1.0 / 12.0;
static constexpr double one_24th = 1.0 / 24.0;
static constexpr double one_60th = 1.0 / 60.0;
static constexpr double one_120th = 1.0 / 120.0;
static constexpr double one_180th = 1.0 / 180.0;
static constexpr double one_720th = 1.0 / 720.0;
static constexpr double one_1260th = 1.0 / 1260.0;
GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) {
Matrix99 H;
auto R = Q.matrix();
@ -60,9 +69,9 @@ void ExpmapFunctor::init(bool nearZeroApprox) {
2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
B = one_minus_cos / theta2;
} else {
// Limits as theta -> 0:
A = 1.0;
B = 0.5;
// Taylor expansion at 0
A = 1.0 - theta2 * one_6th;
B = 0.5 - theta2 * one_24th;
}
}
@ -93,12 +102,12 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
E = (2.0 * B - A) / theta2;
F = (3.0 * C - B) / theta2;
} else {
// Limit as theta -> 0
// Taylor expansion at 0
// TODO(Frank): flipping signs here does not trigger any tests: harden!
C = one_sixth;
D = one_twelfth;
E = one_twelfth;
F = one_sixtieth;
C = one_6th - theta2 * one_120th;
D = one_12th + theta2 * one_720th;
E = one_12th - theta2 * one_180th;
F = one_60th - theta2 * one_1260th;
}
}

16
gtsam/geometry/SO3.h
View File

@ -138,8 +138,8 @@ struct GTSAM_EXPORT ExpmapFunctor {
bool nearZero;
// Ethan Eade's constants:
double A; // A = sin(theta) / theta or 1 for theta->0
double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0
double A; // A = sin(theta) / theta
double B; // B = (1 - cos(theta))
/// Constructor with element of Lie algebra so(3)
explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
@ -159,14 +159,14 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
const Vector3 omega;
// Ethan's C constant used in Jacobians
double C; // (1 - A) / theta^2 or 1/6 for theta->0
double C; // (1 - A) / theta^2
// Constant used in inverse Jacobians
double D; // (1 - A/2B) / theta2 or 1/12 for theta->0
double D; // (1 - A/2B) / theta2
// Constants used in cross and doubleCross
double E; // (A - 2.0 * B) / theta2 or -1/12 for theta->0
double F; // (B - 3.0 * C) / theta2 or -1/60 for theta->0
double E; // (2B - A) / theta2
double F; // (3C - B) / theta2
/// Constructor with element of Lie algebra so(3)
explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
@ -216,10 +216,6 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
Vector3 applyLeftJacobianInverse(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

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

@ -835,38 +835,7 @@ TEST(Pose3, Align2) {
}
/* ************************************************************************* */
TEST( Pose3, ExpmapDerivative1) {
Matrix6 actualH;
Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
Pose3::Expmap(w,actualH);
Matrix expectedH = numericalDerivative21<Pose3, Vector6,
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
EXPECT(assert_equal(expectedH, actualH));
}
/* ************************************************************************* */
TEST( Pose3, ExpmapDerivative2) {
Matrix6 actualH;
Vector6 w; w << 1.0, -2.0, 3.0, -10.0, -20.0, 30.0;
Pose3::Expmap(w,actualH);
Matrix expectedH = numericalDerivative21<Pose3, Vector6,
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
EXPECT(assert_equal(expectedH, actualH));
}
/* ************************************************************************* */
TEST( Pose3, ExpmapDerivative3) {
Matrix6 actualH;
Vector6 w; w << 0.0, 0.0, 0.0, -10.0, -20.0, 30.0;
Pose3::Expmap(w,actualH);
Matrix expectedH = numericalDerivative21<Pose3, Vector6,
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
// Small angle approximation is not as precise as numerical derivative?
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}
/* ************************************************************************* */
TEST(Pose3, ExpmapDerivative4) {
TEST(Pose3, ExpmapDerivative) {
// Iserles05an (Lie-group Methods) says:
// scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
// matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)
@ -900,15 +869,65 @@ TEST(Pose3, ExpmapDerivative4) {
}
}
/* ************************************************************************* */
TEST( Pose3, LogmapDerivative) {
Matrix6 actualH;
Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
Pose3 p = Pose3::Expmap(w);
EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5));
Matrix expectedH = numericalDerivative21<Vector6, Pose3,
OptionalJacobian<6, 6> >(&Pose3::Logmap, p, {});
EXPECT(assert_equal(expectedH, actualH));
//******************************************************************************
namespace test_cases {
std::vector<Vector3> small{{0, 0, 0}, //
{1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //,
{1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}};
std::vector<Vector3> large{{0, 0, 0}, {1, 0, 0}, {0, 1, 0},
{0, 0, 1}, {.1, .2, .3}, {1, -2, 3}};
auto omegas = [](bool nearZero) { return nearZero ? small : large; };
std::vector<Vector3> vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1},
{.4, .3, .2}, {4, 5, 6}, {-10, -20, 30}};
} // namespace test_cases
//******************************************************************************
TEST(Pose3, ExpmapDerivatives) {
for (bool nearZero : {true, false}) {
for (const Vector3& w : test_cases::omegas(nearZero)) {
for (Vector3 v : test_cases::vs) {
const Vector6 xi = (Vector6() << w, v).finished();
const Matrix6 expectedH =
numericalDerivative21<Pose3, Vector6, OptionalJacobian<6, 6> >(
&Pose3::Expmap, xi, {});
Matrix actualH;
Pose3::Expmap(xi, actualH);
EXPECT(assert_equal(expectedH, actualH));
}
}
}
}
//******************************************************************************
// Check logmap for all small values, as we don't want wrapping.
TEST(Pose3, Logmap) {
static constexpr bool nearZero = true;
for (const Vector3& w : test_cases::omegas(nearZero)) {
for (Vector3 v : test_cases::vs) {
const Vector6 xi = (Vector6() << w, v).finished();
Pose3 pose = Pose3::Expmap(xi);
EXPECT(assert_equal(xi, Pose3::Logmap(pose), 1e-5));
}
}
}
//******************************************************************************
// Check logmap derivatives for all values
TEST(Pose3, LogmapDerivatives) {
for (bool nearZero : {true, false}) {
for (const Vector3& w : test_cases::omegas(nearZero)) {
for (Vector3 v : test_cases::vs) {
const Vector6 xi = (Vector6() << w, v).finished();
Pose3 pose = Pose3::Expmap(xi);
const Matrix6 expectedH =
numericalDerivative21<Vector6, Pose3, OptionalJacobian<6, 6> >(
&Pose3::Logmap, pose, {});
Matrix actualH;
Pose3::Logmap(pose, actualH);
EXPECT(assert_equal(expectedH, actualH));
}
}
}
}
/* ************************************************************************* */