remove distance in noisemodel, replace with error

gtsam/linear/JacobianFactor.cpp

@@ -515,7 +515,7 @@ Vector JacobianFactor::error_vector(const VectorValues& c) const {
 double JacobianFactor::error(const VectorValues& c) const {
   Vector e = unweighted_error(c);
   // Use the noise model distance function to get the correct error if available.
-  if (model_) return 0.5 * model_->distance(e);
+  if (model_) return model_->error(e);
   return 0.5 * e.dot(e);
 }
 

gtsam/linear/NoiseModel.cpp

@@ -369,12 +369,12 @@ Vector Constrained::whiten(const Vector& v) const {
 }
 
 /* ************************************************************************* */
-double Constrained::distance(const Vector& v) const {
+double Constrained::error(const Vector& v) const {
   Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
   for (size_t i=0; i<dim_; ++i)  // add mu weights on constrained variables
     if (constrained(i)) // whiten makes constrained variables zero
       w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild
-  return w.dot(w);
+  return 0.5 * w.dot(w);
 }
 
 /* ************************************************************************* */

gtsam/linear/NoiseModel.h

@@ -90,7 +90,12 @@ namespace gtsam {
       /// Unwhiten an error vector.
       virtual Vector unwhiten(const Vector& v) const = 0;
 
+      /// calculate the error value given error vector
+      virtual double error(const Vector& v) const = 0;
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
       virtual double distance(const Vector& v) const = 0;
+#endif
 
       virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const = 0;
       virtual void WhitenSystem(Matrix& A, Vector& b) const = 0;
@@ -224,8 +229,8 @@ namespace gtsam {
       }
 #endif
 
-      inline virtual double distance(const Vector& v) const {
+      inline virtual double error(const Vector& v) const {
-        return SquaredMahalanobisDistance(v);
+        return 0.5 * SquaredMahalanobisDistance(v);
       }
 
       /**
@@ -477,7 +482,7 @@ namespace gtsam {
        * for non-constrained versions, uses sigmas, otherwise
        * uses the penalty function with mu
        */
-      virtual double distance(const Vector& v) const;
+      virtual double error(const Vector& v) const;
 
       /** Fully constrained variations */
       static shared_ptr All(size_t dim) {
@@ -705,11 +710,11 @@ namespace gtsam {
       { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
       inline virtual Vector unwhiten(const Vector& /*v*/) const
       { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
-      // Fold the use of the m-estimator loss(...) function into distance(...)
+      // Fold the use of the m-estimator loss(...) function into error(...)
-      inline virtual double distance(const Vector& v) const
+      inline virtual double error(const Vector& v) const
       { return robust_->loss(this->unweightedWhiten(v).norm()); }
-      inline virtual double distance_non_whitened(const Vector& v) const
+      // inline virtual double distance_non_whitened(const Vector& v) const
-      { return robust_->loss(v.norm()); }
+      // { return robust_->loss(v.norm()); }
       // TODO: these are really robust iterated re-weighting support functions
       virtual void WhitenSystem(Vector& b) const;
       virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const;

gtsam/linear/tests/testNoiseModel.cpp

@@ -182,8 +182,8 @@ TEST(NoiseModel, ConstrainedMixed )
   EXPECT(assert_equal(Vector3(0.5, 1.0, 0.5),d->whiten(infeasible)));
   EXPECT(assert_equal(Vector3(0.5, 0.0, 0.5),d->whiten(feasible)));
 
-  DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9);
+  DOUBLES_EQUAL(0.5 * (1000.0 + 0.25 + 0.25),d->error(infeasible),1e-9);
-  DOUBLES_EQUAL(0.5,d->distance(feasible),1e-9);
+  DOUBLES_EQUAL(0.5 * 0.5,d->error(feasible),1e-9);
 }
 
 /* ************************************************************************* */
@@ -197,8 +197,8 @@ TEST(NoiseModel, ConstrainedAll )
   EXPECT(assert_equal(Vector3(1.0, 1.0, 1.0),i->whiten(infeasible)));
   EXPECT(assert_equal(Vector3(0.0, 0.0, 0.0),i->whiten(feasible)));
 
-  DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9);
+  DOUBLES_EQUAL(0.5 * 1000.0 * 3.0,i->error(infeasible),1e-9);
-  DOUBLES_EQUAL(0.0,i->distance(feasible),1e-9);
+  DOUBLES_EQUAL(0.0,i->error(feasible),1e-9);
 }
 
 /* ************************************************************************* */
@@ -687,7 +687,7 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone)
   EQUALITY(cov, gaussian->covariance());\
   EXPECT(assert_equal(white, gaussian->whiten(e)));\
   EXPECT(assert_equal(e, gaussian->unwhiten(white)));\
-  EXPECT_DOUBLES_EQUAL(251, gaussian->distance(e), 1e-9);\
+  EXPECT_DOUBLES_EQUAL(0.5 * 251, gaussian->error(e), 1e-9);\
   Matrix A = R.inverse(); Vector b = e;\
   gaussian->WhitenSystem(A, b);\
   EXPECT(assert_equal(I, A));\

gtsam/nonlinear/NonlinearFactor.cpp

@@ -121,7 +121,7 @@ double NoiseModelFactor::error(const Values& c) const {
     const Vector b = unwhitenedError(c);
     check(noiseModel_, b.size());
     if (noiseModel_)
-      return 0.5 * noiseModel_->distance(b);
+      return noiseModel_->error(b);
     else
       return 0.5 * b.squaredNorm();
   } else {