diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index e99dba3bc..a1f8774e5 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -1,18 +1,19 @@ /** - * @file testBetweenFactor.cpp + * @file testBetweenFactor.cpp * @brief - * @author Duy-Nguyen Ta - * @date Aug 2, 2013 + * @author Duy-Nguyen Ta, Varun Agrawal + * @date Aug 2, 2013 */ +#include +#include #include +#include #include #include +#include #include -#include -#include - using namespace boost::placeholders; using namespace gtsam; using namespace gtsam::symbol_shorthand; @@ -48,28 +49,71 @@ TEST(BetweenFactor, Rot3) { } /* ************************************************************************* */ -/* // Constructor scalar TEST(BetweenFactor, ConstructorScalar) { SharedNoiseModel model; - double measured_value = 0.0; - BetweenFactor factor(1, 2, measured_value, model); + double measured = 0.0; + BetweenFactor factor(1, 2, measured, model); } +/* ************************************************************************* */ // Constructor vector3 TEST(BetweenFactor, ConstructorVector3) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); - Vector3 measured_value(1, 2, 3); - BetweenFactor factor(1, 2, measured_value, model); + Vector3 measured(1, 2, 3); + BetweenFactor factor(1, 2, measured, model); } +/* ************************************************************************* */ // Constructor dynamic sized vector TEST(BetweenFactor, ConstructorDynamicSizeVector) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0); - Vector measured_value(5); measured_value << 1, 2, 3, 4, 5; - BetweenFactor factor(1, 2, measured_value, model); + Vector measured(5); measured << 1, 2, 3, 4, 5; + BetweenFactor factor(1, 2, measured, model); +} + +/* ************************************************************************* */ +TEST(BetweenFactor, Point3Jacobians) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); + Point3 measured(1, 2, 3); + BetweenFactor factor(1, 2, measured, model); + + Values values; + values.insert(1, Point3(0, 0, 0)); + values.insert(2, Point3(1, 2, 3)); + Vector3 error = factor.evaluateError(Point3(0, 0, 0), Point3(1, 2, 3)); + EXPECT(assert_equal(Vector3::Zero(), error, 1e-9)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +TEST(BetweenFactor, Rot3Jacobians) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); + Rot3 measured = Rot3::Ry(M_PI_2); + BetweenFactor factor(1, 2, measured, model); + + Values values; + values.insert(1, Rot3()); + values.insert(2, Rot3::Ry(M_PI_2)); + Vector3 error = factor.evaluateError(Rot3(), Rot3::Ry(M_PI_2)); + EXPECT(assert_equal(Vector3::Zero(), error, 1e-9)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +TEST(BetweenFactor, Pose3Jacobians) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 1.0); + Pose3 measured(Rot3(), Point3(1, 2, 3)); + BetweenFactor factor(1, 2, measured, model); + + Pose3 pose1, pose2(Rot3(), Point3(1, 2, 3)); + Values values; + values.insert(1, pose1); + values.insert(2, pose2); + Vector6 error = factor.evaluateError(pose1, pose2); + EXPECT(assert_equal(Vector6::Zero(), error, 1e-9)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); } -*/ /* ************************************************************************* */ int main() {