NonlinearInequalityConstraint.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/NonlinearInequalityConstraint.h>
#include <gtsam/inference/FactorGraph-inst.h>
 
namespace gtsam {
 
/* ********************************************************************************************* */
Vector NonlinearInequalityConstraint::whitenedExpr(const Values& x) const {
  return noiseModel()->whiten(unwhitenedExpr(x));
}
 
/* ********************************************************************************************* */
Vector NonlinearInequalityConstraint::unwhitenedError(const Values& x,
                                                      OptionalMatrixVecType H) const {
  Vector error = unwhitenedExpr(x, H);
  for (size_t i = 0; i < dim(); i++) {
    if (error(i) < 0) {
      error(i) = 0;
      if (H) {
        for (Matrix& m : *H) {
          m.row(i).setZero();
        }
      }
    }
  }
  return error;
}
 
/* ********************************************************************************************* */
bool NonlinearInequalityConstraint::active(const Values& x) const {
  return (unwhitenedExpr(x).array() >= 0).any();
}
 
/* ********************************************************************************************* */
NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorCustom(
    InequalityPenaltyFunction::shared_ptr func, const double mu) const {
  return penaltyFactor(mu);
}
 
/* ********************************************************************************************* */
NonlinearEqualityConstraint::shared_ptr NonlinearInequalityConstraint::createEqualityConstraint()
    const {
  return nullptr;
}
 
/* ********************************************************************************************* */
NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorEquality(
    const double mu) const {
  return createEqualityConstraint()->penaltyFactor(mu);
}
 
/* ********************************************************************************************* */
ScalarExpressionInequalityConstraint::ScalarExpressionInequalityConstraint(
    const Double_& expression, const double& sigma)
    : Base(constrainedNoise(Vector1(sigma)), expression.keysAndDims().first),
      expression_(expression),
      dims_(expression.keysAndDims().second) {}
 
/* ********************************************************************************************* */
ScalarExpressionInequalityConstraint::shared_ptr ScalarExpressionInequalityConstraint::GeqZero(
    const Double_& expression, const double& sigma) {
  Double_ neg_expr = Double_(0.0) - expression;
  return std::make_shared<ScalarExpressionInequalityConstraint>(neg_expr, sigma);
}
 
/* ********************************************************************************************* */
ScalarExpressionInequalityConstraint::shared_ptr ScalarExpressionInequalityConstraint::LeqZero(
    const Double_& expression, const double& sigma) {
  return std::make_shared<ScalarExpressionInequalityConstraint>(expression, sigma);
}
 
/* ********************************************************************************************* */
Vector ScalarExpressionInequalityConstraint::unwhitenedExpr(const Values& x,
                                                            OptionalMatrixVecType H) const {
  // Copy-paste from ExpressionFactor.
  if (H) {
    return Vector1(expression_.valueAndDerivatives(x, keys_, dims_, *H));
  } else {
    return Vector1(expression_.value(x));
  }
}
 
/* ********************************************************************************************* */
NonlinearEqualityConstraint::shared_ptr
ScalarExpressionInequalityConstraint::createEqualityConstraint() const {
  return std::make_shared<ExpressionEqualityConstraint<double>>(
      expression_, 0.0, noiseModel()->sigmas());
}
 
/* ********************************************************************************************* */
NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactor(
    const double mu) const {
  Double_ penalty_expression(RampFunction::Ramp, expression_);
  return std::make_shared<ExpressionFactor<double>>(penaltyNoise(mu), 0.0, penalty_expression);
}
 
/* ********************************************************************************************* */
NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorCustom(
    InequalityPenaltyFunction::shared_ptr func, const double mu) const {
  if (!func) {
    return penaltyFactor(mu);
  }
  // TODO(yetong): can we pass the functor directly to construct the expression?
  Double_ error(func->function(), expression_);
  return std::make_shared<ExpressionFactor<double>>(penaltyNoise(mu), 0.0, error);
}
 
/* ********************************************************************************************* */
NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorEquality(
    const double mu) const {
  return std::make_shared<ExpressionFactor<double>>(penaltyNoise(mu), 0.0, expression_);
}
 
/* ********************************************************************************************* */
size_t NonlinearInequalityConstraints::dim() const {
  size_t dimension = 0;
  for (const auto& constraint : *this) {
    dimension += constraint->dim();
  }
  return dimension;
}
 
/* ********************************************************************************************* */
Vector NonlinearInequalityConstraints::violationVector(const Values& values, bool whiten) const {
  Vector violation(dim());
  size_t start_idx = 0;
  for (const auto& constraint : *this) {
    size_t dim = constraint->dim();
    violation.middleCols(start_idx, dim) =
        whiten ? constraint->whitenedError(values) : constraint->unwhitenedError(values);
    start_idx += dim;
  }
  return violation;
}
 
/* ********************************************************************************************* */
double NonlinearInequalityConstraints::violationNorm(const Values& values) const {
  return violationVector(values, true).norm();
}
 
/* ********************************************************************************************* */
NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraph(const double mu) const {
  NonlinearFactorGraph graph;
  for (const auto& constraint : *this) {
    graph.add(constraint->penaltyFactor(mu));
  }
  return graph;
}
 
/* ********************************************************************************************* */
NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraphCustom(
    InequalityPenaltyFunction::shared_ptr func, const double mu) const {
  NonlinearFactorGraph graph;
  for (const auto& constraint : *this) {
    graph.add(constraint->penaltyFactorCustom(func, mu));
  }
  return graph;
}
 
}  // namespace gtsam