From b60189104cdc12990123ca7b8295bf4b8d7095dc Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 5 Oct 2012 23:23:40 +0000 Subject: [PATCH] Added checks for proper noise model dimension --- gtsam/nonlinear/NonlinearFactor.h | 17 +++++++++++++---- gtsam/slam/tests/testGeneralSFMFactor.cpp | 2 +- .../tests/testGeneralSFMFactor_Cal3Bundler.cpp | 2 +- gtsam/slam/tests/testProjectionFactor.cpp | 2 +- gtsam/slam/tests/testStereoFactor.cpp | 2 +- tests/testInferenceB.cpp | 2 +- tests/testNonlinearEquality.cpp | 4 ++-- 7 files changed, 20 insertions(+), 11 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 293ff2677..d7df2721e 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -286,7 +286,10 @@ public: * This is the raw error, i.e., i.e. \f$ (h(x)-z)/\sigma \f$ in case of a Gaussian */ Vector whitenedError(const Values& c) const { - return noiseModel_->whiten(unwhitenedError(c)); + const Vector unwhitenedErrorVec = unwhitenedError(c); + if(unwhitenedErrorVec.size() != noiseModel_->dim()) + throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension."); + return noiseModel_->whiten(unwhitenedErrorVec); } /** @@ -296,10 +299,14 @@ public: * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. */ virtual double error(const Values& c) const { - if (this->active(c)) - return 0.5 * noiseModel_->distance(unwhitenedError(c)); - else + if (this->active(c)) { + const Vector unwhitenedErrorVec = unwhitenedError(c); + if(unwhitenedErrorVec.size() != noiseModel_->dim()) + throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension."); + return 0.5 * noiseModel_->distance(unwhitenedErrorVec); + } else { return 0.0; + } } /** @@ -317,6 +324,8 @@ public: // Call evaluate error to get Jacobians and b vector std::vector A(this->size()); b = -unwhitenedError(x, A); + if(b.size() != noiseModel_->dim()) + throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension."); this->noiseModel_->WhitenSystem(A,b); diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index e02405aa9..0aeadfa6a 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -80,7 +80,7 @@ static double getGaussian() return sqrt(-2.0f * (double)log(S) / S) * V1; } -static const SharedNoiseModel sigma1(noiseModel::Unit::Create(1)); +static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); /* ************************************************************************* */ TEST( GeneralSFMFactor, equals ) diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 21f5f2008..6d8f36634 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -81,7 +81,7 @@ static double getGaussian() return sqrt(-2.0f * (double)log(S) / S) * V1; } -static const SharedNoiseModel sigma1(noiseModel::Unit::Create(1)); +static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); /* ************************************************************************* */ TEST( GeneralSFMFactor_Cal3Bundler, equals ) diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index 4f3b3b083..ca02c2884 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -38,7 +38,7 @@ static double fov = 60; // degrees static size_t w=640,h=480; static Cal3_S2 K(fov,w,h); -static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); +static SharedNoiseModel sigma(noiseModel::Unit::Create(2)); static shared_ptrK sK(new Cal3_S2(K)); // Convenience for named keys diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 1169f97cb..f8a17522e 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -42,7 +42,7 @@ static boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 24 // point X Y Z in meters static Point3 p(0, 0, 5); -static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); +static SharedNoiseModel sigma(noiseModel::Unit::Create(3)); // Convenience for named keys using symbol_shorthand::X; diff --git a/tests/testInferenceB.cpp b/tests/testInferenceB.cpp index b3dcf6e99..0df25d082 100644 --- a/tests/testInferenceB.cpp +++ b/tests/testInferenceB.cpp @@ -63,7 +63,7 @@ TEST( inference, marginals2) { NonlinearFactorGraph fg; SharedDiagonal poseModel(noiseModel::Isotropic::Sigma(3, 0.1)); - SharedDiagonal pointModel(noiseModel::Isotropic::Sigma(3, 0.1)); + SharedDiagonal pointModel(noiseModel::Isotropic::Sigma(2, 0.1)); fg.add(PriorFactor(X(0), Pose2(), poseModel)); fg.add(BetweenFactor(X(0), X(1), Pose2(1.0,0.0,0.0), poseModel)); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index cb9757bac..f10ce1b35 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -477,7 +477,7 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { Point2 pose1(1.0, 1.0); graph.add(eq2D::UnaryEqualityConstraint(pose1, x1)); - SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,0.1); + SharedDiagonal sigma = noiseModel::Isotropic::Sigma(2, 0.1); // measurement from x1 to l1 Point2 z1(0.0, 5.0); @@ -546,7 +546,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { graph.add(NonlinearEquality(x2, camera2.pose())); // create factors - SharedDiagonal vmodel = noiseModel::Unit::Create(3); + SharedDiagonal vmodel = noiseModel::Unit::Create(2); graph.add(GenericProjectionFactor(camera1.project(landmark), vmodel, x1, l1, shK)); graph.add(GenericProjectionFactor(camera2.project(landmark), vmodel, x2, l2, shK));