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

test cleanup

release/4.3a0
Asa Hammond 2021-02-26 07:11:21 -08:00
parent 68cdb41706
commit f48a2f6273
1 changed files with 14 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
gtsam/navigation/tests/testConstantVelocityFactor.cpp
View File

@ -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));
}
/* ************************************************************************* */