Add successful unit test for identity pose
parent
075293cf83
commit
e6b7d9f133
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue