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

Address comments

release/4.3a0
Frank Dellaert 2024-12-15 12:11:56 -05:00
parent 8193bdbec3
commit f7a678e54e
2 changed files with 5 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/navigation/NavState.cpp
View File

@ -264,8 +264,6 @@ Vector9 NavState::ChartAtOrigin::Local(const NavState& state,
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
NavState NavState::retract(const Vector9& xi, // NavState NavState::retract(const Vector9& xi, //
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
// return LieGroup<NavState, 9>::retract(xi, H1, H2);
Rot3 nRb = R_; Rot3 nRb = R_;
Point3 n_t = t_, n_v = v_; Point3 n_t = t_, n_v = v_;
Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb; Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb;

10
gtsam/navigation/tests/testNavState.cpp
View File

@ -490,7 +490,7 @@ TEST(NavState, Expmap_b) {
(Vector(9) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished()); (Vector(9) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished());
NavState expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(-100.0, 0.0, 0.0), NavState expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(-100.0, 0.0, 0.0),
Point3(100.0, 0.0, 0.0)); Point3(100.0, 0.0, 0.0));
EXPECT(assert_equal(expected, p2, 1e-2)); EXPECT(assert_equal(expected, p2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -566,7 +566,7 @@ TEST(NavState, retract_localCoordinates2) {
EXPECT(assert_equal(t2, t1.retract(d12))); EXPECT(assert_equal(t2, t1.retract(d12)));
Vector d21 = t2.localCoordinates(t1); Vector d21 = t2.localCoordinates(t1);
EXPECT(assert_equal(t1, t2.retract(d21))); EXPECT(assert_equal(t1, t2.retract(d21)));
// EXPECT(assert_equal(d12, -d21)); // NOTE(FRANK): d12 !== -d21 for arbitrary retractions.
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(NavState, manifold_expmap) { TEST(NavState, manifold_expmap) {
@ -675,9 +675,9 @@ TEST(NavState, ChartDerivatives) {
NavState id; NavState id;
if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
CHECK_CHART_DERIVATIVES(id, id); CHECK_CHART_DERIVATIVES(id, id);
// CHECK_CHART_DERIVATIVES(id,T2); CHECK_CHART_DERIVATIVES(id,T2);
// CHECK_CHART_DERIVATIVES(T2,id); CHECK_CHART_DERIVATIVES(T2,id);
// CHECK_CHART_DERIVATIVES(T2,T3); CHECK_CHART_DERIVATIVES(T2,T3);
} }
} }