diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 7da6cdbdf..d14847e52 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -28,9 +29,10 @@ #include #include +#include #include #include -using namespace boost; +using namespace boost::assign; #include #include @@ -441,7 +443,8 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { } /* ************************************************************************* */ -TEST(GeneralSFMFactor, Linearize) { +// Frank created these tests after switching to a custom LinearizedFactor +TEST(GeneralSFMFactor, LinearizedFactor) { Point2 z(3., 0.); // Create Values @@ -453,44 +456,42 @@ TEST(GeneralSFMFactor, Linearize) { Point3 l1; values.insert(L(1), l1); - // Test with Empty Model + vector models; { - const SharedNoiseModel model; - Projection factor(z, model, X(1), L(1)); - GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize( - values); - GaussianFactor::shared_ptr actual = factor.linearize(values); - EXPECT(assert_equal(*expected, *actual, 1e-9)); + // Create various noise-models to test all cases + using namespace noiseModel; + Rot2 R = Rot2::fromAngle(0.3); + Matrix2 cov = R.matrix() * R.matrix().transpose(); + models += SharedNoiseModel(), Unit::Create(2), // + Isotropic::Sigma(2, 0.5), Constrained::All(2), Gaussian::Covariance(cov); } - // Test with Unit Model - { - const SharedNoiseModel model(noiseModel::Unit::Create(2)); + + // Now loop over all these noise models + BOOST_FOREACH(SharedNoiseModel model, models) { Projection factor(z, model, X(1), L(1)); - GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize( - values); - GaussianFactor::shared_ptr actual = factor.linearize(values); - EXPECT(assert_equal(*expected, *actual, 1e-9)); - } - // Test with Isotropic noise - { - const SharedNoiseModel model(noiseModel::Isotropic::Sigma(2, 0.5)); - Projection factor(z, model, X(1), L(1)); - GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize( - values); - GaussianFactor::shared_ptr actual = factor.linearize(values); - EXPECT(assert_equal(*expected, *actual, 1e-9)); - } - // Test with Constrained Model - { - const SharedNoiseModel model(noiseModel::Constrained::All(2)); - Projection factor(z, model, X(1), L(1)); - GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize( - values); + + // Test linearize + GaussianFactor::shared_ptr expected = // + factor.NoiseModelFactor::linearize(values); GaussianFactor::shared_ptr actual = factor.linearize(values); EXPECT(assert_equal(*expected, *actual, 1e-9)); + + // Test methods that rely on updateHessian + if (model && !model->isConstrained()) { + // Construct HessianFactor from single JacobianFactor + HessianFactor expectedHessian(*expected), actualHessian(*actual); + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-9)); + + // Construct from GaussianFactorGraph + GaussianFactorGraph gfg1; + gfg1 += expected; + GaussianFactorGraph gfg2; + gfg2 += actual; + HessianFactor hessian1(gfg1), hessian2(gfg2); + EXPECT(assert_equal(hessian1, hessian2, 1e-9)); + } } } - /* ************************************************************************* */ int main() { TestResult tr;