InequalityPenaltyFunction.cpp

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
 
 * -------------------------------------------------------------------------- */
 
#include <gtsam/constrained/InequalityPenaltyFunction.h>
 
namespace gtsam {
 
/* ************************************************************************* */
InequalityPenaltyFunction::UnaryScalarFunc InequalityPenaltyFunction::function()
    const {
  return [this](const double& x, OptionalJacobian<1, 1> H = {}) {
    return this->operator()(x, H);
  };
}
 
/* ************************************************************************* */
double RampFunction::Ramp(const double x, OptionalJacobian<1, 1> H) {
  if (x < 0) {
    if (H) {
      H->setConstant(0);
    }
    return 0;
  } else {
    if (H) {
      H->setConstant(1);
    }
    return x;
  }
}
 
/* ************************************************************************* */
double SmoothRampPoly2::operator()(const double& x,
                                   OptionalJacobian<1, 1> H) const {
  if (x <= 0) {
    if (H) {
      H->setZero();
    }
    return 0;
  } else if (x < epsilon_) {
    if (H) {
      H->setConstant(2 * a_ * x);
    }
    return a_ * pow(x, 2);
  } else {
    if (H) {
      H->setOnes();
    }
    return x - epsilon_ / 2;
  }
}
 
/* ************************************************************************* */
double SmoothRampPoly3::operator()(const double& x,
                                   OptionalJacobian<1, 1> H) const {
  if (x <= 0) {
    if (H) {
      H->setZero();
    }
    return 0;
  } else if (x < epsilon_) {
    if (H) {
      H->setConstant(3 * a_ * pow(x, 2) + 2 * b_ * x);
    }
    return a_ * pow(x, 3) + b_ * pow(x, 2);
  } else {
    if (H) {
      H->setOnes();
    }
    return x;
  }
}
 
/* ************************************************************************* */
double SoftPlusFunction::operator()(const double& x,
                                    OptionalJacobian<1, 1> H) const {
  if (H) {
    H->setConstant(1 / (1 + std::exp(-k_ * x)));
  }
  return std::log(1 + std::exp(k_ * x)) / k_;
}
 
}  // namespace gtsam