Added checks for proper noise model dimension

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,11 +299,15 @@ 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;
     }
+  }
 
   /**
    * Linearize a non-linearFactorN to get a GaussianFactor,
@@ -317,6 +324,8 @@ public:
     // Call evaluate error to get Jacobians and b vector
     std::vector<Matrix> 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);
 

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 )

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 )

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

gtsam/slam/tests/testStereoFactor.cpp

@@ -42,7 +42,7 @@ static boost::shared_ptr<Cal3_S2Stereo> 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;

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<Pose2>(X(0), Pose2(), poseModel));
   fg.add(BetweenFactor<Pose2>(X(0), X(1), Pose2(1.0,0.0,0.0), poseModel));

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<Pose3>(x2, camera2.pose()));
 
   // create  factors
-  SharedDiagonal vmodel = noiseModel::Unit::Create(3);
+  SharedDiagonal vmodel = noiseModel::Unit::Create(2);
   graph.add(GenericProjectionFactor<Pose3,Point3,Cal3_S2>(camera1.project(landmark), vmodel, x1, l1, shK));
   graph.add(GenericProjectionFactor<Pose3,Point3,Cal3_S2>(camera2.project(landmark), vmodel, x2, l2, shK));