NoiseModel.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

#pragma once

#include <gtsam/base/Testable.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/dllexport.h>

#include <boost/serialization/nvp.hpp>
#include <boost/serialization/extended_type_info.hpp>
#include <boost/serialization/singleton.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/optional.hpp>

namespace gtsam {

  namespace noiseModel {

    // Forward declaration
    class Gaussian;
    class Diagonal;
    class Constrained;
    class Isotropic;
    class Unit;

    //---------------------------------------------------------------------------------------

    class GTSAM_EXPORT Base {

    public:
      typedef boost::shared_ptr<Base> shared_ptr;

    protected:

      size_t dim_;

    public:

      Base(size_t dim = 1):dim_(dim) {}
      virtual ~Base() {}

      virtual bool isConstrained() const { return false; } // default false

      virtual bool isUnit() const { return false; }  // default false

      inline size_t dim() const { return dim_;}

      virtual void print(const std::string& name = "") const = 0;

      virtual bool equals(const Base& expected, double tol=1e-9) const = 0;

      virtual Vector sigmas() const;

      virtual Vector whiten(const Vector& v) const = 0;

      virtual Matrix Whiten(const Matrix& H) const = 0;

      virtual Vector unwhiten(const Vector& v) const = 0;

      virtual double distance(const Vector& v) const = 0;

      virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const = 0;
      virtual void WhitenSystem(Matrix& A, Vector& b) const = 0;
      virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const = 0;
      virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const = 0;

      virtual void whitenInPlace(Vector& v) const {
        v = whiten(v);
      }

      virtual void unwhitenInPlace(Vector& v) const {
        v = unwhiten(v);
      }

      virtual void whitenInPlace(Eigen::Block<Vector>& v) const {
        v = whiten(v);
      }

      virtual void unwhitenInPlace(Eigen::Block<Vector>& v) const {
        v = unwhiten(v);
      }

    private:
      friend class boost::serialization::access;
      template<class ARCHIVE>
      void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
        ar & BOOST_SERIALIZATION_NVP(dim_);
      }
    };

    //---------------------------------------------------------------------------------------

    class GTSAM_EXPORT Gaussian: public Base {

    protected:

      boost::optional<Matrix> sqrt_information_;

    private:

      const Matrix& thisR() const {
        // should never happen
        if (!sqrt_information_) throw std::runtime_error("Gaussian: has no R matrix");
        return *sqrt_information_;
      }

    protected:

      Gaussian(size_t dim = 1, const boost::optional<Matrix>& sqrt_information = boost::none) :
        Base(dim), sqrt_information_(sqrt_information) {
      }

    public:

      typedef boost::shared_ptr<Gaussian> shared_ptr;

      virtual ~Gaussian() {}

      static shared_ptr SqrtInformation(const Matrix& R, bool smart = true);

      static shared_ptr Information(const Matrix& M, bool smart = true);

      static shared_ptr Covariance(const Matrix& covariance, bool smart = true);

      virtual void print(const std::string& name) const;
      virtual bool equals(const Base& expected, double tol=1e-9) const;
      virtual Vector sigmas() const;
      virtual Vector whiten(const Vector& v) const;
      virtual Vector unwhiten(const Vector& v) const;

      virtual double Mahalanobis(const Vector& v) const;

      inline virtual double distance(const Vector& v) const {
        return Mahalanobis(v);
      }

      virtual Matrix Whiten(const Matrix& H) const;

      virtual void WhitenInPlace(Matrix& H) const;

      virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;

      virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const;
      virtual void WhitenSystem(Matrix& A, Vector& b) const;
      virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const;
      virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;

      virtual boost::shared_ptr<Diagonal> QR(Matrix& Ab) const;

      virtual Matrix R() const { return thisR();}

      virtual Matrix information() const { return R().transpose() * R(); }

      virtual Matrix covariance() const { return information().inverse(); }

    private:
      friend class boost::serialization::access;
      template<class ARCHIVE>
      void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
        ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
        ar & BOOST_SERIALIZATION_NVP(sqrt_information_);
      }

    }; // Gaussian

    //---------------------------------------------------------------------------------------

    class GTSAM_EXPORT Diagonal : public Gaussian {
    protected:

      Vector sigmas_, invsigmas_, precisions_;

    protected:
      Diagonal();

      Diagonal(const Vector& sigmas);

    public:

      typedef boost::shared_ptr<Diagonal> shared_ptr;

      virtual ~Diagonal() {}

      static shared_ptr Sigmas(const Vector& sigmas, bool smart = true);

      static shared_ptr Variances(const Vector& variances, bool smart = true);

      static shared_ptr Precisions(const Vector& precisions, bool smart = true) {
        return Variances(precisions.array().inverse(), smart);
      }

      virtual void print(const std::string& name) const;
      virtual Vector sigmas() const { return sigmas_; }
      virtual Vector whiten(const Vector& v) const;
      virtual Vector unwhiten(const Vector& v) const;
      virtual Matrix Whiten(const Matrix& H) const;
      virtual void WhitenInPlace(Matrix& H) const;
      virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;

      inline double sigma(size_t i) const { return sigmas_(i); }

      inline const Vector& invsigmas() const { return invsigmas_; }
      inline double invsigma(size_t i) const {return invsigmas_(i);}

      inline const Vector& precisions() const { return precisions_; }
      inline double precision(size_t i) const {return precisions_(i);}

      virtual Matrix R() const {
        return invsigmas().asDiagonal();
      }

    private:
      friend class boost::serialization::access;
      template<class ARCHIVE>
      void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
        ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Gaussian);
        ar & BOOST_SERIALIZATION_NVP(sigmas_);
        ar & BOOST_SERIALIZATION_NVP(invsigmas_);
      }
    }; // Diagonal

    //---------------------------------------------------------------------------------------

    class GTSAM_EXPORT Constrained : public Diagonal {
    protected:

      // Sigmas are contained in the base class
      Vector mu_; 

      Constrained(const Vector& sigmas = Z_1x1);

      Constrained(const Vector& mu, const Vector& sigmas);

    public:

      typedef boost::shared_ptr<Constrained> shared_ptr;

      virtual ~Constrained() {}

      virtual bool isConstrained() const { return true; }

      bool constrained(size_t i) const;

      const Vector& mu() const { return mu_; }

      static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas);

      static shared_ptr MixedSigmas(const Vector& sigmas) {
        return MixedSigmas(Vector::Constant(sigmas.size(), 1000.0), sigmas);
      }

      static shared_ptr MixedSigmas(double m, const Vector& sigmas) {
        return MixedSigmas(Vector::Constant(sigmas.size(), m), sigmas);
      }

      static shared_ptr MixedVariances(const Vector& mu, const Vector& variances) {
        return shared_ptr(new Constrained(mu, variances.cwiseSqrt()));
      }
      static shared_ptr MixedVariances(const Vector& variances) {
        return shared_ptr(new Constrained(variances.cwiseSqrt()));
      }

      static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions) {
        return MixedVariances(mu, precisions.array().inverse());
      }
      static shared_ptr MixedPrecisions(const Vector& precisions) {
        return MixedVariances(precisions.array().inverse());
      }

      virtual double distance(const Vector& v) const;

      static shared_ptr All(size_t dim) {
        return shared_ptr(new Constrained(Vector::Constant(dim, 1000.0), Vector::Constant(dim,0)));
      }

      static shared_ptr All(size_t dim, const Vector& mu) {
        return shared_ptr(new Constrained(mu, Vector::Constant(dim,0)));
      }

      static shared_ptr All(size_t dim, double mu) {
        return shared_ptr(new Constrained(Vector::Constant(dim, mu), Vector::Constant(dim,0)));
      }

      virtual void print(const std::string& name) const;

      virtual Vector whiten(const Vector& v) const;

      virtual Matrix Whiten(const Matrix& H) const;
      virtual void WhitenInPlace(Matrix& H) const;
      virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;

      virtual Diagonal::shared_ptr QR(Matrix& Ab) const;

      shared_ptr unit() const;

    private:
      friend class boost::serialization::access;
      template<class ARCHIVE>
      void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
        ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal);
        ar & BOOST_SERIALIZATION_NVP(mu_);
      }

    }; // Constrained

    //---------------------------------------------------------------------------------------

    class GTSAM_EXPORT Isotropic : public Diagonal {
    protected:
      double sigma_, invsigma_;

      Isotropic(size_t dim, double sigma) :
        Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}

      /* dummy constructor to allow for serialization */
      Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {}

    public:

      virtual ~Isotropic() {}

      typedef boost::shared_ptr<Isotropic> shared_ptr;

      static shared_ptr Sigma(size_t dim, double sigma, bool smart = true);

      static shared_ptr Variance(size_t dim, double variance, bool smart = true);

      static shared_ptr Precision(size_t dim, double precision, bool smart = true)  {
        return Variance(dim, 1.0/precision, smart);
      }

      virtual void print(const std::string& name) const;
      virtual double Mahalanobis(const Vector& v) const;
      virtual Vector whiten(const Vector& v) const;
      virtual Vector unwhiten(const Vector& v) const;
      virtual Matrix Whiten(const Matrix& H) const;
      virtual void WhitenInPlace(Matrix& H) const;
      virtual void whitenInPlace(Vector& v) const;
      virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;

      inline double sigma() const { return sigma_; }

    private:
      friend class boost::serialization::access;
      template<class ARCHIVE>
      void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
        ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal);
        ar & BOOST_SERIALIZATION_NVP(sigma_);
        ar & BOOST_SERIALIZATION_NVP(invsigma_);
      }

    };

    //---------------------------------------------------------------------------------------

    class GTSAM_EXPORT Unit : public Isotropic {
    protected:

      Unit(size_t dim=1): Isotropic(dim,1.0) {}

    public:

      typedef boost::shared_ptr<Unit> shared_ptr;

      virtual ~Unit() {}

      static shared_ptr Create(size_t dim) {
        return shared_ptr(new Unit(dim));
      }

      virtual bool isUnit() const { return true; }

      virtual void print(const std::string& name) const;
      virtual double Mahalanobis(const Vector& v) const {return v.dot(v); }
      virtual Vector whiten(const Vector& v) const { return v; }
      virtual Vector unwhiten(const Vector& v) const { return v; }
      virtual Matrix Whiten(const Matrix& H) const { return H; }
      virtual void WhitenInPlace(Matrix& /*H*/) const {}
      virtual void WhitenInPlace(Eigen::Block<Matrix> /*H*/) const {}
      virtual void whitenInPlace(Vector& /*v*/) const {}
      virtual void unwhitenInPlace(Vector& /*v*/) const {}
      virtual void whitenInPlace(Eigen::Block<Vector>& /*v*/) const {}
      virtual void unwhitenInPlace(Eigen::Block<Vector>& /*v*/) const {}

    private:
      friend class boost::serialization::access;
      template<class ARCHIVE>
      void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
        ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Isotropic);
      }
    };

    namespace mEstimator {

      //---------------------------------------------------------------------------------------

      class GTSAM_EXPORT Base {
      public:
        enum ReweightScheme { Scalar, Block };
        typedef boost::shared_ptr<Base> shared_ptr;

      protected:
        ReweightScheme reweight_;

      public:
        Base(const ReweightScheme reweight = Block):reweight_(reweight) {}
        virtual ~Base() {}

        /*
         * This method is responsible for returning the total penalty for a given amount of error.
         * For example, this method is responsible for implementing the quadratic function for an
         * L2 penalty, the absolute value function for an L1 penalty, etc.
         *
         * TODO(mike): There is currently a bug in GTSAM, where none of the mEstimator classes
         * implement a residual function, and GTSAM calls the weight function to evaluate the
         * total penalty, rather than calling the residual function. The weight function should be
         * used during iteratively reweighted least squares optimization, but should not be used to
         * evaluate the total penalty. The long-term solution is for all mEstimators to implement
         * both a weight and a residual function, and for GTSAM to call the residual function when
         * evaluating the total penalty. But for now, I'm leaving this residual method as pure
         * virtual, so the existing mEstimators can inherit this default fallback behavior.
         */
        virtual double residual(double error) const { return 0; };

        /*
         * This method is responsible for returning the weight function for a given amount of error.
         * The weight function is related to the analytic derivative of the residual function. See
         *  http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html
         * for details. This method is required when optimizing cost functions with robust penalties
         * using iteratively re-weighted least squares.
         */
        virtual double weight(double error) const = 0;

        virtual void print(const std::string &s) const = 0;
        virtual bool equals(const Base& expected, double tol=1e-8) const = 0;

        double sqrtWeight(double error) const {
          return std::sqrt(weight(error));
        }

        Vector weight(const Vector &error) const;

        Vector sqrtWeight(const Vector &error) const {
          return weight(error).cwiseSqrt();
        }

        void reweight(Vector &error) const;
        void reweight(std::vector<Matrix> &A, Vector &error) const;
        void reweight(Matrix &A, Vector &error) const;
        void reweight(Matrix &A1, Matrix &A2, Vector &error) const;
        void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const;

      private:
        friend class boost::serialization::access;
        template<class ARCHIVE>
        void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
          ar & BOOST_SERIALIZATION_NVP(reweight_);
        }
      };

      class GTSAM_EXPORT Null : public Base {
      public:
        typedef boost::shared_ptr<Null> shared_ptr;

        Null(const ReweightScheme reweight = Block) : Base(reweight) {}
        virtual ~Null() {}
        virtual double weight(double /*error*/) const { return 1.0; }
        virtual void print(const std::string &s) const;
        virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; }
        static shared_ptr Create() ;

      private:
        friend class boost::serialization::access;
        template<class ARCHIVE>
        void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
          ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
        }
      };

      class GTSAM_EXPORT Fair : public Base {
      protected:
        double c_;

      public:
        typedef boost::shared_ptr<Fair> shared_ptr;

        Fair(double c = 1.3998, const ReweightScheme reweight = Block);
        double weight(double error) const {
          return 1.0 / (1.0 + fabs(error) / c_);
        }
        void print(const std::string &s) const;
        bool equals(const Base& expected, double tol=1e-8) const;
        static shared_ptr Create(double c, const ReweightScheme reweight = Block) ;

      private:
        friend class boost::serialization::access;
        template<class ARCHIVE>
        void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
          ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
          ar & BOOST_SERIALIZATION_NVP(c_);
        }
      };

      class GTSAM_EXPORT Huber : public Base {
      protected:
        double k_;

      public:
        typedef boost::shared_ptr<Huber> shared_ptr;

        Huber(double k = 1.345, const ReweightScheme reweight = Block);
        double weight(double error) const {
          return (error < k_) ? (1.0) : (k_ / fabs(error));
        }
        void print(const std::string &s) const;
        bool equals(const Base& expected, double tol=1e-8) const;
        static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;

      private:
        friend class boost::serialization::access;
        template<class ARCHIVE>
        void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
          ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
          ar & BOOST_SERIALIZATION_NVP(k_);
        }
      };

      class GTSAM_EXPORT Cauchy : public Base {
      protected:
        double k_, ksquared_;

      public:
        typedef boost::shared_ptr<Cauchy> shared_ptr;

        Cauchy(double k = 0.1, const ReweightScheme reweight = Block);
        double weight(double error) const {
          return ksquared_ / (ksquared_ + error*error);
        }
        void print(const std::string &s) const;
        bool equals(const Base& expected, double tol=1e-8) const;
        static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;

      private:
        friend class boost::serialization::access;
        template<class ARCHIVE>
        void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
          ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
          ar & BOOST_SERIALIZATION_NVP(k_);
        }
      };

      class GTSAM_EXPORT Tukey : public Base {
      protected:
        double c_, csquared_;

      public:
        typedef boost::shared_ptr<Tukey> shared_ptr;

        Tukey(double c = 4.6851, const ReweightScheme reweight = Block);
        double weight(double error) const {
          if (std::fabs(error) <= c_) {
            double xc2 = error*error/csquared_;
            return (1.0-xc2)*(1.0-xc2);
          }
          return 0.0;
        }
        void print(const std::string &s) const;
        bool equals(const Base& expected, double tol=1e-8) const;
        static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;

      private:
        friend class boost::serialization::access;
        template<class ARCHIVE>
        void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
          ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
          ar & BOOST_SERIALIZATION_NVP(c_);
        }
      };

      class GTSAM_EXPORT Welsh : public Base {
      protected:
        double c_, csquared_;

      public:
        typedef boost::shared_ptr<Welsh> shared_ptr;

        Welsh(double c = 2.9846, const ReweightScheme reweight = Block);
        double weight(double error) const {
          double xc2 = (error*error)/csquared_;
          return std::exp(-xc2);
        }
        void print(const std::string &s) const;
        bool equals(const Base& expected, double tol=1e-8) const;
        static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;

      private:
        friend class boost::serialization::access;
        template<class ARCHIVE>
        void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
          ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
          ar & BOOST_SERIALIZATION_NVP(c_);
        }
      };

      class GTSAM_EXPORT GemanMcClure : public Base {
      public:
        typedef boost::shared_ptr<GemanMcClure> shared_ptr;

        GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
        virtual ~GemanMcClure() {}
        virtual double weight(double error) const;
        virtual void print(const std::string &s) const;
        virtual bool equals(const Base& expected, double tol=1e-8) const;
        static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;

      protected:
        double c_;

      private:
        friend class boost::serialization::access;
        template<class ARCHIVE>
        void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
          ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
          ar & BOOST_SERIALIZATION_NVP(c_);
        }
      };

      class GTSAM_EXPORT DCS : public Base {
      public:
        typedef boost::shared_ptr<DCS> shared_ptr;

        DCS(double c = 1.0, const ReweightScheme reweight = Block);
        virtual ~DCS() {}
        virtual double weight(double error) const;
        virtual void print(const std::string &s) const;
        virtual bool equals(const Base& expected, double tol=1e-8) const;
        static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;

      protected:
        double c_;

      private:
        friend class boost::serialization::access;
        template<class ARCHIVE>
        void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
          ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
          ar & BOOST_SERIALIZATION_NVP(c_);
        }
      };

      class GTSAM_EXPORT L2WithDeadZone : public Base {
      protected:
          double k_;

      public:
          typedef boost::shared_ptr<L2WithDeadZone> shared_ptr;

          L2WithDeadZone(double k, const ReweightScheme reweight = Block);
          double residual(double error) const {
            const double abs_error = fabs(error);
            return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error);
          }
          double weight(double error) const {
            // note that this code is slightly uglier than above, because there are three distinct
            // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two
            // cases (deadzone, non-deadzone) above.
            if (fabs(error) <= k_) return 0.0;
            else if (error > k_) return (-k_+error)/error;
            else return (k_+error)/error;
          }
          void print(const std::string &s) const;
          bool equals(const Base& expected, double tol=1e-8) const;
          static shared_ptr Create(double k, const ReweightScheme reweight = Block);

      private:
          friend class boost::serialization::access;
          template<class ARCHIVE>
          void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
            ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
            ar & BOOST_SERIALIZATION_NVP(k_);
          }
      };

    } 

    class GTSAM_EXPORT Robust : public Base {
    public:
      typedef boost::shared_ptr<Robust> shared_ptr;

    protected:
      typedef mEstimator::Base RobustModel;
      typedef noiseModel::Base NoiseModel;

      const RobustModel::shared_ptr robust_; 
      const NoiseModel::shared_ptr noise_;   

    public:

      Robust() {};

      Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise)
      : Base(noise->dim()), robust_(robust), noise_(noise) {}

      virtual ~Robust() {}

      virtual void print(const std::string& name) const;
      virtual bool equals(const Base& expected, double tol=1e-9) const;

      const RobustModel::shared_ptr& robust() const { return robust_; }

      const NoiseModel::shared_ptr& noise() const { return noise_; }

      // TODO: functions below are dummy but necessary for the noiseModel::Base
      inline virtual Vector whiten(const Vector& v) const
      { Vector r = v; this->WhitenSystem(r); return r; }
      inline virtual Matrix Whiten(const Matrix& A) const
      { 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."); }
      inline virtual double distance(const Vector& v) const
      { return this->whiten(v).squaredNorm(); }
      // 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
      virtual void WhitenSystem(Vector& b) const;
      virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const;
      virtual void WhitenSystem(Matrix& A, Vector& b) const;
      virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const;
      virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;

      static shared_ptr Create(
        const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise);

    private:
      friend class boost::serialization::access;
      template<class ARCHIVE>
      void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
        ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
        ar & boost::serialization::make_nvp("robust_", const_cast<RobustModel::shared_ptr&>(robust_));
        ar & boost::serialization::make_nvp("noise_", const_cast<NoiseModel::shared_ptr&>(noise_));
      }
    };

    // Helper function
    GTSAM_EXPORT boost::optional<Vector> checkIfDiagonal(const Matrix M);

  } // namespace noiseModel

  typedef noiseModel::Base::shared_ptr SharedNoiseModel;
  typedef noiseModel::Gaussian::shared_ptr SharedGaussian;
  typedef noiseModel::Diagonal::shared_ptr SharedDiagonal;
  typedef noiseModel::Constrained::shared_ptr SharedConstrained;
  typedef noiseModel::Isotropic::shared_ptr SharedIsotropic;

  template<> struct traits<noiseModel::Gaussian> : public Testable<noiseModel::Gaussian> {};
  template<> struct traits<noiseModel::Diagonal> : public Testable<noiseModel::Diagonal> {};
  template<> struct traits<noiseModel::Constrained> : public Testable<noiseModel::Constrained> {};
  template<> struct traits<noiseModel::Isotropic> : public Testable<noiseModel::Isotropic> {};
  template<> struct traits<noiseModel::Unit> : public Testable<noiseModel::Unit> {};

} //\ namespace gtsam