Add functionalities to get introspection of the loss function weights and unweighted residuals for the NoiseModelFactor class

gtsam/linear/NoiseModel.h

@@ -117,6 +117,14 @@ namespace gtsam {
         v = unwhiten(v);
       }
 
+      /** Useful function for robust noise models to get the unweighted but whitened error */
+      virtual Vector unweightedWhiten(const Vector& v) const {
+        return whiten(v);
+      }
+
+      /** get the weight from the effective loss function on residual vector v */
+      virtual double weight(const Vector& v) const { return 1.0; }
+
     private:
       /** Serialization function */
       friend class boost::serialization::access;
@@ -684,9 +692,9 @@ 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 residual(...) function into distance(...)
       inline virtual double distance(const Vector& v) const
-      { return this->whiten(v).squaredNorm(); }
+      { return robust_->residual(this->unweightedWhiten(v).norm()); }
-      // TODO(mike): fold the use of the m-estimator residual(...) function into distance(...)
       inline virtual double distance_non_whitened(const Vector& v) const
       { return robust_->residual(v.norm()); }
       // TODO: these are really robust iterated re-weighting support functions
@@ -696,6 +704,14 @@ namespace gtsam {
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const;
       virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;
 
+      virtual Vector unweightedWhiten(const Vector& v) const {
+        return noise_->unweightedWhiten(v);
+      }
+      virtual double weight(const Vector& v) const {
+        // Todo(mikebosse): make the robust weight function input a vector.
+        return robust_->weight(v.norm());
+      }
+
       static shared_ptr Create(
         const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise);
 

gtsam/nonlinear/NonlinearFactor.cpp

@@ -93,6 +93,28 @@ Vector NoiseModelFactor::whitenedError(const Values& c) const {
   return noiseModel_ ? noiseModel_->whiten(b) : b;
 }
 
+/* ************************************************************************* */
+Vector NoiseModelFactor::unweightedWhitenedError(const Values& c) const {
+  const Vector b = unwhitenedError(c);
+  check(noiseModel_, b.size());
+  return noiseModel_ ? noiseModel_->unweightedWhiten(b) : b;
+}
+
+/* ************************************************************************* */
+double NoiseModelFactor::weight(const Values& c) const {
+  if (active(c)) {
+    if (noiseModel_) {
+      const Vector b = unwhitenedError(c);
+      check(noiseModel_, b.size());
+      return 0.5 * noiseModel_->weight(b);
+    }
+    else
+      return 1.0;
+  } else {
+    return 0.0;
+  }
+}
+
 /* ************************************************************************* */
 double NoiseModelFactor::error(const Values& c) const {
   if (active(c)) {

gtsam/nonlinear/NonlinearFactor.h

@@ -226,6 +226,16 @@ public:
    */
   Vector whitenedError(const Values& c) const;
 
+  /**
+   * Vector of errors, whitened, but unweighted by any loss function
+   */
+  Vector unweightedWhitenedError(const Values& c) const;
+
+  /**
+   * Compute the effective weight of the factor from the noise model.
+   */
+  double weight(const Values& c) const;
+
   /**
    * Calculate the error of the factor.
    * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.