InequalityPenaltyFunction.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/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/expressions.h>
 
namespace gtsam {
 
class GTSAM_EXPORT InequalityPenaltyFunction {
 public:
  typedef std::shared_ptr<InequalityPenaltyFunction> shared_ptr;
  typedef std::function<double(const double& x, OptionalJacobian<1, 1> H)>
      UnaryScalarFunc;
 
  InequalityPenaltyFunction() {}
 
  virtual ~InequalityPenaltyFunction() {}
 
  virtual double operator()(const double& x,
                            OptionalJacobian<1, 1> H = {}) const = 0;
 
  virtual UnaryScalarFunc function() const;
};
 
class GTSAM_EXPORT RampFunction : public InequalityPenaltyFunction {
 public:
  typedef InequalityPenaltyFunction Base;
  typedef RampFunction This;
  typedef std::shared_ptr<This> shared_ptr;
 
 public:
  RampFunction() : Base() {}
 
  virtual double operator()(const double& x,
                            OptionalJacobian<1, 1> H = {}) const override {
    return Ramp(x, H);
  }
 
  virtual UnaryScalarFunc function() const override { return Ramp; }
 
  static double Ramp(const double x, OptionalJacobian<1, 1> H = {});
};
 
class GTSAM_EXPORT SmoothRampPoly2 : public InequalityPenaltyFunction {
 public:
  typedef InequalityPenaltyFunction Base;
  typedef SmoothRampPoly2 This;
  typedef std::shared_ptr<This> shared_ptr;
 
 protected:
  double epsilon_;
  double a_;
 
 public:
  SmoothRampPoly2(const double epsilon = 1)
      : Base(), epsilon_(epsilon), a_(0.5 / epsilon) {}
 
  virtual double operator()(const double& x,
                            OptionalJacobian<1, 1> H = {}) const override;
};
 
class GTSAM_EXPORT SmoothRampPoly3 : public InequalityPenaltyFunction {
 public:
  typedef InequalityPenaltyFunction Base;
  typedef SmoothRampPoly3 This;
  typedef std::shared_ptr<This> shared_ptr;
 
 protected:
  double epsilon_;
  double a_;
  double b_;
 
 public:
  SmoothRampPoly3(const double epsilon = 1)
      : Base(),
        epsilon_(epsilon),
        a_(-1 / (epsilon * epsilon)),
        b_(2 / epsilon) {}
 
  virtual double operator()(const double& x,
                            OptionalJacobian<1, 1> H = {}) const override;
};
 
class GTSAM_EXPORT SoftPlusFunction : public InequalityPenaltyFunction {
 public:
  typedef InequalityPenaltyFunction Base;
  typedef SoftPlusFunction This;
  typedef std::shared_ptr<This> shared_ptr;
 
 protected:
  double k_;
 
 public:
  SoftPlusFunction(const double k = 1) : Base(), k_(k) {}
 
  virtual double operator()(const double& x,
                            OptionalJacobian<1, 1> H = {}) const override;
};
 
}  // namespace gtsam