diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 240b19a46..ad25b611c 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -19,13 +19,13 @@ #include #include #include +#include +#include #include -#include #include #include #include -#include -#include +#include #include #include @@ -101,8 +101,8 @@ TEST( GeneralSFMFactor, equals ) /* ************************************************************************* */ TEST( GeneralSFMFactor, error ) { Point2 z(3.,0.); - const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr factor(new Projection(z, sigma, X(1), L(1))); + const SharedNoiseModel sigma(noiseModel::Unit::Create(2)); + Projection factor(z, sigma, X(1), L(1)); // For the following configuration, the factor predicts 320,240 Values values; Rot3 R; @@ -110,7 +110,7 @@ TEST( GeneralSFMFactor, error ) { Pose3 x1(R,t1); values.insert(X(1), GeneralCamera(x1)); Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal(((Vector) Vector2(-3.0, 0.0)), factor->unwhitenedError(values))); + EXPECT(assert_equal(((Vector) Vector2(-3.0, 0.0)), factor.unwhitenedError(values))); } static const double baseline = 5.0 ; @@ -430,6 +430,95 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { EXPECT(assert_equal(expected, actual, 1e-4)); } +/* ************************************************************************* */ + +static const int DimC = 11, DimL = 3; + +/// Linearize using fixed-size matrices +boost::shared_ptr linearize(const Projection& factor, + const Values& values) { + // Only linearize if the factor is active + if (!factor.active(values)) return boost::shared_ptr(); + + const Key key1 = factor.key1(), key2 = factor.key2(); + Eigen::Matrix H1; + Eigen::Matrix H2; + Vector2 b; + try { + const GeneralCamera& camera = values.at(key1); + const Point3& point = values.at(key2); + Point2 reprojError(camera.project2(point, H1, H2) - factor.measured()); + b = -reprojError.vector(); + } catch (CheiralityException& e) { + // TODO warn if verbose output asked for + return boost::make_shared(); + } + + // Whiten the system if needed + const SharedNoiseModel& noiseModel = factor.get_noiseModel(); + if (noiseModel && !noiseModel->isUnit()) { + // TODO: implement WhitenSystem for fixed size matrices and include above + H1 = noiseModel->Whiten(H1); + H2 = noiseModel->Whiten(H2); + b = noiseModel->Whiten(b); + } + + if (noiseModel && noiseModel->isConstrained()) { + using noiseModel::Constrained; + return boost::make_shared( + key1, H1, key2, H2, b, + boost::static_pointer_cast(noiseModel)->unit()); + } else { + return boost::make_shared(key1, H1, key2, H2, b); + } +} + +/* ************************************************************************* */ +TEST(GeneralSFMFactor, Linearize) { + Point2 z(3.,0.); + + // Create Values + Values values; + Rot3 R; + Point3 t1(0,0,-6); + Pose3 x1(R,t1); + values.insert(X(1), GeneralCamera(x1)); + Point3 l1; values.insert(L(1), l1); + + // Test with Empty Model + { + const SharedNoiseModel model; + Projection factor(z, model, X(1), L(1)); + GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values); + GaussianFactor::shared_ptr actual = linearize(factor, values); + EXPECT(assert_equal(*expected,*actual,1e-9)); + } + // Test with Unit Model + { + const SharedNoiseModel model(noiseModel::Unit::Create(2)); + Projection factor(z, model, X(1), L(1)); + GaussianFactor::shared_ptr expected = factor.linearize(values); + GaussianFactor::shared_ptr actual = linearize(factor, 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.linearize(values); + GaussianFactor::shared_ptr actual = linearize(factor, 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.linearize(values); + GaussianFactor::shared_ptr actual = linearize(factor, values); + EXPECT(assert_equal(*expected,*actual,1e-9)); + } +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */