LossFunctions.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/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/dllexport.h>
 
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/extended_type_info.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/version.hpp>
#include <boost/serialization/optional.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/singleton.hpp>
#endif
 
namespace gtsam {
namespace noiseModel {
// clang-format off
// clang-format on
namespace mEstimator {
 
class GTSAM_EXPORT Base {
 public:
  enum ReweightScheme { Scalar, Block };
  typedef std::shared_ptr<Base> shared_ptr;
 
 protected:
  ReweightScheme reweight_;
 
 public:
  Base(const ReweightScheme reweight = Block) : reweight_(reweight) {}
  virtual ~Base() {}
 
  ReweightScheme reweightScheme() const { return reweight_; }
 
  virtual double loss(double distance) const { return 0; }
 
  virtual double weight(double distance) 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 distance) const { return std::sqrt(weight(distance)); }
 
  Vector weight(const Vector &error) const;
 
  Vector sqrtWeight(const Vector &error) const;
 
  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:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
 
  friend class boost::serialization::access;
  template <class ARCHIVE>
  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
    ar &BOOST_SERIALIZATION_NVP(reweight_);
  }
#endif
};
 
class GTSAM_EXPORT Null : public Base {
 public:
  typedef std::shared_ptr<Null> shared_ptr;
 
  Null(const ReweightScheme reweight = Block) : Base(reweight) {}
  ~Null() override {}
  double weight(double /*error*/) const override { return 1.0; }
  double loss(double distance) const override { return 0.5 * distance * distance; }
  void print(const std::string &s) const override;
  bool equals(const Base & /*expected*/, double /*tol*/) const override { return true; }
  static shared_ptr Create();
 
 private:
#ifdef 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);
  }
#endif
};
 
class GTSAM_EXPORT Fair : public Base {
 protected:
  double c_;
 
 public:
  typedef std::shared_ptr<Fair> shared_ptr;
 
  Fair(double c = 1.3998, const ReweightScheme reweight = Block);
  double weight(double distance) const override;
  double loss(double distance) const override;
  void print(const std::string &s) const override;
  bool equals(const Base &expected, double tol = 1e-8) const override;
  static shared_ptr Create(double c, const ReweightScheme reweight = Block);
  double modelParameter() const { return c_; }
 
 private:
#ifdef 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(c_);
  }
#endif
};
 
class GTSAM_EXPORT Huber : public Base {
 protected:
  double k_;
 
 public:
  typedef std::shared_ptr<Huber> shared_ptr;
 
  Huber(double k = 1.345, const ReweightScheme reweight = Block);
  double weight(double distance) const override;
  double loss(double distance) const override;
  void print(const std::string &s) const override;
  bool equals(const Base &expected, double tol = 1e-8) const override;
  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
  double modelParameter() const { return k_; }
 
 private:
#ifdef 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(k_);
  }
#endif
};
 
class GTSAM_EXPORT Cauchy : public Base {
 protected:
  double k_, ksquared_;
 
 public:
  typedef std::shared_ptr<Cauchy> shared_ptr;
 
  Cauchy(double k = 0.1, const ReweightScheme reweight = Block);
  double weight(double distance) const override;
  double loss(double distance) const override;
  void print(const std::string &s) const override;
  bool equals(const Base &expected, double tol = 1e-8) const override;
  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
  double modelParameter() const { return k_; }
 
 private:
#ifdef 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(k_);
    ar &BOOST_SERIALIZATION_NVP(ksquared_);
  }
#endif
};
 
class GTSAM_EXPORT Tukey : public Base {
 protected:
  double c_, csquared_;
 
 public:
  typedef std::shared_ptr<Tukey> shared_ptr;
 
  Tukey(double c = 4.6851, const ReweightScheme reweight = Block);
  double weight(double distance) const override;
  double loss(double distance) const override;
  void print(const std::string &s) const override;
  bool equals(const Base &expected, double tol = 1e-8) const override;
  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
  double modelParameter() const { return c_; }
 
 private:
#ifdef 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(c_);
  }
#endif
};
 
class GTSAM_EXPORT Welsch : public Base {
 protected:
  double c_, csquared_;
 
 public:
  typedef std::shared_ptr<Welsch> shared_ptr;
 
  Welsch(double c = 2.9846, const ReweightScheme reweight = Block);
  double weight(double distance) const override;
  double loss(double distance) const override;
  void print(const std::string &s) const override;
  bool equals(const Base &expected, double tol = 1e-8) const override;
  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
  double modelParameter() const { return c_; }
 
 private:
#ifdef 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(c_);
    ar &BOOST_SERIALIZATION_NVP(csquared_);
  }
#endif
};
 
class GTSAM_EXPORT GemanMcClure : public Base {
 public:
  typedef std::shared_ptr<GemanMcClure> shared_ptr;
 
  GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
  ~GemanMcClure() override {}
  double weight(double distance) const override;
  double loss(double distance) const override;
  void print(const std::string &s) const override;
  bool equals(const Base &expected, double tol = 1e-8) const override;
  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
  double modelParameter() const { return c_; }
 
 protected:
  double c_;
 
 private:
#ifdef 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(c_);
  }
#endif
};
 
class GTSAM_EXPORT DCS : public Base {
 public:
  typedef std::shared_ptr<DCS> shared_ptr;
 
  DCS(double c = 1.0, const ReweightScheme reweight = Block);
  ~DCS() override {}
  double weight(double distance) const override;
  double loss(double distance) const override;
  void print(const std::string &s) const override;
  bool equals(const Base &expected, double tol = 1e-8) const override;
  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
  double modelParameter() const { return c_; }
 
 protected:
  double c_;
 
 private:
#ifdef 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(c_);
  }
#endif
};
 
class GTSAM_EXPORT L2WithDeadZone : public Base {
 protected:
  double k_;
 
 public:
  typedef std::shared_ptr<L2WithDeadZone> shared_ptr;
 
  L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block);
  double weight(double distance) const override;
  double loss(double distance) const override;
  void print(const std::string &s) const override;
  bool equals(const Base &expected, double tol = 1e-8) const override;
  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
  double modelParameter() const { return k_; }
 
 private:
#ifdef 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(k_);
  }
#endif
};
 
class GTSAM_EXPORT AsymmetricTukey : public Base {
 protected:
  double c_, csquared_;
 
 public:
  typedef std::shared_ptr<AsymmetricTukey> shared_ptr;
 
  AsymmetricTukey(double c = 4.6851, const ReweightScheme reweight = Block);
  double weight(double distance) const override;
  double loss(double distance) const override;
  void print(const std::string &s) const override;
  bool equals(const Base &expected, double tol = 1e-8) const override;
  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
  double modelParameter() const { return c_; }
 
 private:
#ifdef 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(c_);
  }
#endif
};
 
class GTSAM_EXPORT AsymmetricCauchy : public Base {
 protected:
  double k_, ksquared_;
 
 public:
  typedef std::shared_ptr<AsymmetricCauchy> shared_ptr;
 
  AsymmetricCauchy(double k = 0.1, const ReweightScheme reweight = Block);
  double weight(double distance) const override;
  double loss(double distance) const override;
  void print(const std::string &s) const override;
  bool equals(const Base &expected, double tol = 1e-8) const override;
  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
  double modelParameter() const { return k_; }
 
 private:
#ifdef 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(k_);
    ar &BOOST_SERIALIZATION_NVP(ksquared_);
  }
#endif
};
 
// Type alias for the custom loss and weight functions
using CustomLossFunction = std::function<double(double)>;
using CustomWeightFunction = std::function<double(double)>;
 
class GTSAM_EXPORT Custom : public Base {
 protected:
  std::function<double(double)> weight_, loss_;
  std::string name_;
 
 public:
  typedef std::shared_ptr<Custom> shared_ptr;
 
  Custom(CustomWeightFunction weight, CustomLossFunction loss,
         const ReweightScheme reweight = Block, std::string name = "Custom");
  double weight(double distance) const override;
  double loss(double distance) const override;
  void print(const std::string &s) const override;
  bool equals(const Base &expected, double tol = 1e-8) const override;
  static shared_ptr Create(std::function<double(double)> weight, std::function<double(double)> loss,
                           const ReweightScheme reweight = Block, const std::string &name = "Custom");
  inline std::string& name() { return name_; }
 
  inline std::function<double(double)>& weightFunction() { return weight_; }
  inline std::function<double(double)>& lossFunction() { return loss_; }
 
  // Default constructor for serialization
  inline Custom() = default;
 
 private:
#ifdef 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(name_);
  }
#endif
};
 
}  // namespace mEstimator
}  // namespace noiseModel
}  // namespace gtsam