diff --git a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp new file mode 100644 index 000000000..6cc70ef46 --- /dev/null +++ b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp @@ -0,0 +1,114 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + + +// Pose representation is [ Rx Ry Rz Tx Ty Tz ]. +static const int kIndexRx = 0; +static const int kIndexRy = 1; +static const int kIndexRz = 2; +static const int kIndexTx = 3; +static const int kIndexTy = 4; +static const int kIndexTz = 5; + + +typedef PartialPriorFactor TestPartialPriorFactor; + +/// traits +namespace gtsam { +template<> +struct traits : public Testable { +}; +} + +/* ************************************************************************* */ +TEST(PartialPriorFactor, JacobianPartialTranslation) { + Key poseKey(1); + Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + 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(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + + // Calculate numerical derivatives. + Matrix expectedH1 = numericalDerivative11( + 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, JacobianFullTranslation) { + Key poseKey(1); + Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); + + std::vector translationIndices = { kIndexTx, kIndexTy, kIndexTz }; + TestPartialPriorFactor factor(poseKey, translationIndices, measurement.translation(), model); + + // Create a linearization point at the zero-error point + Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + + // Calculate numerical derivatives. + Matrix expectedH1 = numericalDerivative11( + 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, JacobianFullRotation) { + Key poseKey(1); + Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); + + std::vector rotationIndices = { kIndexRx, kIndexRy, kIndexRz }; + TestPartialPriorFactor factor(poseKey, rotationIndices, Rot3::Logmap(measurement.rotation()), model); + + // Create a linearization point at the zero-error point + Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + + // Calculate numerical derivatives. + Matrix expectedH1 = numericalDerivative11( + 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)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */