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/Manifold.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/MatrixConstants.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/std_optional_serialization.h>
#include <gtsam/dllexport.h>
#include <gtsam/linear/LossFunctions.h>
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/extended_type_info.hpp>
#include <boost/serialization/singleton.hpp>
#include <boost/serialization/shared_ptr.hpp>
#endif
 
#include <optional>
#include <type_traits>
 
namespace gtsam {

  namespace noiseModel {
 
    // Forward declaration
    class Gaussian;
    class Diagonal;
    class Constrained;
    class Isotropic;
    class Unit;
    class RobustModel;
 
    //---------------------------------------------------------------------------------------

    class GTSAM_EXPORT Base {
 
    public:
      typedef std::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 squaredMahalanobisDistance(const Vector& v) const;

      virtual double mahalanobisDistance(const Vector& v) const {
        return std::sqrt(squaredMahalanobisDistance(v));
      }

      virtual double loss(const double squared_distance) const {
        return 0.5 * squared_distance;
      }

      virtual double loss(const Vector& v) const {
        return loss(squaredMahalanobisDistance(v));
      }
 
      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);
      }

      virtual Vector unweightedWhiten(const Vector& v) const {
        return whiten(v);
      }

      virtual double weight(const Vector& v) const { return 1.0; }
 
    private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
      friend class boost::serialization::access;
      template<class ARCHIVE>
      void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
        ar & BOOST_SERIALIZATION_NVP(dim_);
      }
#endif
    };
 
    //---------------------------------------------------------------------------------------
    template <class T>
    inline bool matchesDimension(const Base& model, const T& measured) {
      static_assert(IsManifold<T>::value,
                    "noiseModel::matchesDimension requires a manifold type.");
      if constexpr (traits<T>::dimension == Eigen::Dynamic) {
        return model.dim() == traits<T>::GetDimension(measured);
      } else {
        return model.dim() == static_cast<size_t>(traits<T>::dimension);
      }
    }

    class GTSAM_EXPORT Gaussian: public Base {
 
    protected:

      std::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_;
      }

      virtual double logDetR() const;
 
    public:
 
      typedef std::shared_ptr<Gaussian> shared_ptr;

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

      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);
 
      void print(const std::string& name) const override;
      bool equals(const Base& expected, double tol=1e-9) const override;
      Vector sigmas() const override;
      Vector whiten(const Vector& v) const override;
      Vector unwhiten(const Vector& v) const override;
      void unwhitenInPlace(Vector& v) const override;
      void unwhitenInPlace(Eigen::Block<Vector>& v) const override;

      Matrix Whiten(const Matrix& H) const override;

      virtual void WhitenInPlace(Matrix& H) const;

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

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

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

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

      virtual Matrix information() const;

      virtual Matrix covariance() const;

      double logDeterminant() const;

      double negLogConstant() const;
 
     private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
      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_);
      }
#endif
    }; // Gaussian
 
    //---------------------------------------------------------------------------------------

    class GTSAM_EXPORT Diagonal : public Gaussian {
    protected:

      Vector sigmas_, invsigmas_, precisions_;

      Diagonal(const Vector& sigmas);

      virtual double logDetR() const override;
 
    public:
      Diagonal();
 
      typedef std::shared_ptr<Diagonal> shared_ptr;
 
      ~Diagonal() override {}

      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);
 
      void print(const std::string& name) const override;
      Vector sigmas() const override { return sigmas_; }
      inline const Vector& sigmasRef() const { return sigmas_; }
      Vector whiten(const Vector& v) const override;
      Vector unwhiten(const Vector& v) const override;
      void whitenInPlace(Vector& v) const override;
      void unwhitenInPlace(Vector& v) const override;
      Matrix Whiten(const Matrix& H) const override;
      void WhitenInPlace(Matrix& H) const override;
      void WhitenInPlace(Eigen::Block<Matrix> H) const override;
      void whitenInPlace(Eigen::Block<Vector>& v) const override;
      void unwhitenInPlace(Eigen::Block<Vector>& v) const override;

      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);}

      Matrix R() const override {
        return invsigmas().asDiagonal();
      }
 
    private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
      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_);
      }
#endif
    }; // Diagonal
 
    //---------------------------------------------------------------------------------------

    class GTSAM_EXPORT Constrained : public Diagonal {
    protected:
 
      // Sigmas are contained in the base class
      Vector mu_; 

      Constrained(const Vector& mu, const Vector& sigmas);
 
    public:
 
      typedef std::shared_ptr<Constrained> shared_ptr;

      Constrained(const Vector& sigmas = Z_1x1);
 
      ~Constrained() override {}

      bool isConstrained() const override { 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);

      static shared_ptr MixedSigmas(double m, const Vector& sigmas);

      static shared_ptr MixedVariances(const Vector& mu, const Vector& variances);
      static shared_ptr MixedVariances(const Vector& variances);

      static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions);
      static shared_ptr MixedPrecisions(const Vector& precisions);

      double squaredMahalanobisDistance(const Vector& v) const override;

      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)));
      }
 
      void print(const std::string& name) const override;

      Vector whiten(const Vector& v) const override;
      void whitenInPlace(Vector& v) const override;
      void whitenInPlace(Eigen::Block<Vector>& v) const override;

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

      Matrix informationFromA(const Matrix& A) const;

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

      shared_ptr unit() const;
 
    private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
      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_);
      }
#endif
 
    }; // 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) {}

      virtual double logDetR() const override;
 
    public:
 
      /* dummy constructor to allow for serialization */
      Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {}
 
      ~Isotropic() override {}
 
      typedef std::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);
      }
 
      void print(const std::string& name) const override;
      double squaredMahalanobisDistance(const Vector& v) const override;
      Vector whiten(const Vector& v) const override;
      Vector unwhiten(const Vector& v) const override;
      Matrix Whiten(const Matrix& H) const override;
      void WhitenInPlace(Matrix& H) const override;
      void whitenInPlace(Vector& v) const override;
      void WhitenInPlace(Eigen::Block<Matrix> H) const override;
      void unwhitenInPlace(Vector& v) const override;
      void unwhitenInPlace(Eigen::Block<Vector>& v) const override;

      inline double sigma() const { return sigma_; }
 
    private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
      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_);
      }
#endif
 
    };
 
    //---------------------------------------------------------------------------------------

    class GTSAM_EXPORT Unit : public Isotropic {
    protected:
      virtual double logDetR() const override;
 
    public:
 
      typedef std::shared_ptr<Unit> shared_ptr;

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

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

      template <class T, std::enable_if_t<!std::is_integral_v<T>, int> = 0>
      static shared_ptr Create(const T& measured) {
        static_assert(IsManifold<T>::value,
                      "noiseModel::Unit::Create requires a manifold type.");
        if constexpr (traits<T>::dimension == Eigen::Dynamic) {
          return Create(traits<T>::GetDimension(measured));
        } else {
          static const shared_ptr kDefault =
              Create(static_cast<size_t>(traits<T>::dimension));
          return kDefault;
        }
      }

      bool isUnit() const override { return true; }
 
      void print(const std::string& name) const override;
      double squaredMahalanobisDistance(const Vector& v) const override;
      Vector whiten(const Vector& v) const override { return v; }
      Vector unwhiten(const Vector& v) const override { return v; }
      Matrix Whiten(const Matrix& H) const override { return H; }
      void WhitenInPlace(Matrix& /*H*/) const override {}
      void WhitenInPlace(Eigen::Block<Matrix> /*H*/) const override {}
      void whitenInPlace(Vector& /*v*/) const override {}
      void unwhitenInPlace(Vector& /*v*/) const override {}
      void whitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
      void unwhitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
 
    private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
      friend class boost::serialization::access;
      template<class ARCHIVE>
      void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
        ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Isotropic);
      }
#endif
    };

    class GTSAM_EXPORT Robust : public Base {
    public:
      typedef std::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) {}

      ~Robust() override {}
 
      void print(const std::string& name) const override;
      bool equals(const Base& expected, double tol=1e-9) const override;

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

      const NoiseModel::shared_ptr& noise() const { return noise_; }
 
      // Functions below are dummy but necessary for the noiseModel::Base
      inline Vector whiten(const Vector& v) const override
      { Vector r = v; this->WhitenSystem(r); return r; }
      inline Matrix Whiten(const Matrix& A) const override
      { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
      inline Vector unwhiten(const Vector& /*v*/) const override
      { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
      inline void whitenInPlace(Vector& v) const override { this->WhitenSystem(v); }
      double loss(const double squared_distance) const override {
        return robust_->loss(std::sqrt(squared_distance));
      }

      double loss(const Vector& v) const override;
 
      // NOTE: This is special because in whiten the base version will do the re-weighting
      // which is incorrect!
      double squaredMahalanobisDistance(const Vector& v) const override {
        return noise_->squaredMahalanobisDistance(v);
      }
 
      // These are really robust iterated re-weighting support functions
      virtual void WhitenSystem(Vector& b) const;
      void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
      void WhitenSystem(Matrix& A, Vector& b) const override;
      void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
      void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
 
      Vector unweightedWhiten(const Vector& v) const override;
      double weight(const Vector& v) const override;
 
      static shared_ptr Create(
        const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise);
 
    private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
      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_));
      }
#endif
    };
 
    // Helper function
    GTSAM_EXPORT std::optional<Vector> checkIfDiagonal(const Matrix& M);

    template <class T>
    Base::shared_ptr validOrDefault(const T& value,
                                    const Base::shared_ptr& model) {
      if (!model) return noiseModel::Unit::Create(value);
      if (noiseModel::matchesDimension(*model, value)) return model;
      throw std::runtime_error(
          "noiseModel::validOrDefault: mis-matched model dimension.");
    }
 
  } // 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