From f48a2f6273e964ec0dda99f57dcdbd7b179d74a6 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Fri, 26 Feb 2021 07:11:21 -0800 Subject: [PATCH] test cleanup --- .../tests/testConstantVelocityFactor.cpp | 45 ++++++------------- 1 file changed, 14 insertions(+), 31 deletions(-) diff --git a/gtsam/navigation/tests/testConstantVelocityFactor.cpp b/gtsam/navigation/tests/testConstantVelocityFactor.cpp index 5c32fe738..540c27eab 100644 --- a/gtsam/navigation/tests/testConstantVelocityFactor.cpp +++ b/gtsam/navigation/tests/testConstantVelocityFactor.cpp @@ -43,51 +43,34 @@ TEST(ConstantVelocityFactor, VelocityFactor) { const auto state2 = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 2.0}}, Velocity3{0.0, 0.0, 1.0}}; + const auto state3 = NavState{Pose3{Rot3::Yaw(M_PI_2), Point3{0.0, 0.0, 2.0}}, Velocity3{0.0, 0.0, 1.0}}; + const double mu{1000}; const auto noise_model = noiseModel::Constrained::All(9, mu); const auto factor = ConstantVelocityFactor(x1, x2, dt, noise_model); - // TODO make these tests way less verbose! - // ideally I could find an initializer for Vector9 like: Vector9{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}' // positions are the same, secondary state has velocity 1.0 in z, const auto state0_err_origin = factor.evaluateError(origin, state0); - EXPECT(assert_equal(0.0, NavState::dP(state0_err_origin).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dP(state0_err_origin).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dP(state0_err_origin).z(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state0_err_origin).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state0_err_origin).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state0_err_origin).z(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state0_err_origin).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state0_err_origin).y(), tol)); - EXPECT(assert_equal(1.0, NavState::dV(state0_err_origin).z(), tol)); + EXPECT(assert_equal((Vector9() << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0).finished(), state0_err_origin, tol)); // same velocities, different position // second state agrees with initial state + velocity * dt const auto state1_err_state0 = factor.evaluateError(state0, state1); - EXPECT(assert_equal(0.0, NavState::dP(state1_err_state0).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dP(state1_err_state0).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dP(state1_err_state0).z(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state1_err_state0).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state1_err_state0).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state1_err_state0).z(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state1_err_state0).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state1_err_state0).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state1_err_state0).z(), tol)); + EXPECT(assert_equal((Vector9() << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(), state1_err_state0, tol)); + + // same velocities, same position, different rotations + // second state agrees with initial state + velocity * dt + // as we assume that omega is 0.0 this is the same as the above case + // TODO: this should respect omega and actually fail in this case + const auto state3_err_state2 = factor.evaluateError(state0, state1); + EXPECT(assert_equal((Vector9() << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(), state3_err_state2, tol)); // both bodies have the same velocity, - // but state2.pose() does not agree with .update() - // error comes from this pose difference + // but state2.pose() does not agree with state0.update() + // error comes from this position difference const auto state2_err_state0 = factor.evaluateError(state0, state2); - EXPECT(assert_equal(0.0, NavState::dP(state2_err_state0).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dP(state2_err_state0).y(), tol)); - EXPECT(assert_equal(1.0, NavState::dP(state2_err_state0).z(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state2_err_state0).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state2_err_state0).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state2_err_state0).z(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state2_err_state0).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state2_err_state0).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state2_err_state0).z(), tol)); + EXPECT(assert_equal((Vector9() << 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0).finished(), state2_err_state0, tol)); } /* ************************************************************************* */