Add successful unit test for identity pose

release/4.3a0
Milo Knowles 2021-03-20 17:57:10 -04:00
parent 075293cf83
commit e6b7d9f133
1 changed files with 26 additions and 0 deletions

View File

@ -38,6 +38,32 @@ struct traits<TestPartialPriorFactor> : public Testable<TestPartialPriorFactor>
};
}
/* ************************************************************************* */
TEST(PartialPriorFactor, JacobianAtIdentity)
{
Key poseKey(1);
Pose3 measurement = Pose3::identity();
SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25);
TestPartialPriorFactor factor(poseKey, kIndexTy, measurement.translation().x(), model);
// Create a linearization point at the zero-error point
Pose3 pose = Pose3::identity();
// Calculate numerical derivatives.
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), pose);
// Use the factor to calculate the derivative.
Matrix actualH1;
factor.evaluateError(pose, actualH1);
// Verify we get the expected error.
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
}
/* ************************************************************************* */
TEST(PartialPriorFactor, JacobianPartialTranslation) {
Key poseKey(1);