Merge pull request #287 from borglab/feature/equality_constraints

add equality constraint representation

gtdynamics/optimizer/EqualityConstraint-inl.h

@@ -0,0 +1,94 @@
+/* ----------------------------------------------------------------------------
+ * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file  EqualityConstraint-inl.h
+ * @brief Equality constraints in constrained optimization.
+ * @author: Yetong Zhang, Frank Dellaert
+ */
+
+#pragma once
+
+namespace gtdynamics {
+
+gtsam::NoiseModelFactor::shared_ptr DoubleExpressionEquality::createFactor(
+    const double mu, boost::optional<gtsam::Vector&> bias) const {
+  auto noise = gtsam::noiseModel::Isotropic::Sigma(1, tolerance_ / sqrt(mu));
+  double measure = 0.0;
+  if (bias) {
+    measure = -(*bias)(0);
+  }
+  return gtsam::NoiseModelFactor::shared_ptr(
+      new gtsam::ExpressionFactor<double>(noise, measure, expression_));
+}
+
+bool DoubleExpressionEquality::feasible(const gtsam::Values& x) const {
+  double result = expression_.value(x);
+  return abs(result) <= tolerance_;
+}
+
+gtsam::Vector DoubleExpressionEquality::operator()(
+    const gtsam::Values& x) const {
+  double result = expression_.value(x);
+  return (gtsam::Vector(1) << result).finished();
+}
+
+gtsam::Vector DoubleExpressionEquality::toleranceScaledViolation(
+    const gtsam::Values& x) const {
+  double result = expression_.value(x);
+  return (gtsam::Vector(1) << result / tolerance_).finished();
+}
+
+size_t DoubleExpressionEquality::dim() const { return 1; }
+
+template <int P>
+gtsam::NoiseModelFactor::shared_ptr VectorExpressionEquality<P>::createFactor(
+    const double mu, boost::optional<gtsam::Vector&> bias) const {
+  auto noise = gtsam::noiseModel::Diagonal::Sigmas(tolerance_ / sqrt(mu));
+  VectorP measure = VectorP::Zero();
+  if (bias) {
+    measure = -*bias;
+  }
+  return gtsam::NoiseModelFactor::shared_ptr(
+      new gtsam::ExpressionFactor<VectorP>(noise, measure, expression_));
+}
+
+template <int P>
+bool VectorExpressionEquality<P>::feasible(const gtsam::Values& x) const {
+  VectorP result = expression_.value(x);
+  for (int i = 0; i < P; i++) {
+    if (abs(result[i]) > tolerance_[i]) {
+      return false;
+    }
+  }
+  return true;
+}
+
+template <int P>
+gtsam::Vector VectorExpressionEquality<P>::operator()(
+    const gtsam::Values& x) const {
+  return expression_.value(x);
+}
+
+template <int P>
+gtsam::Vector VectorExpressionEquality<P>::toleranceScaledViolation(
+    const gtsam::Values& x) const {
+  auto violation = expression_.value(x);
+  // TODO(yetong): figure out how to perform element-wise division
+  VectorP scaled_violation;
+  for (int i = 0; i < P; i++) {
+    scaled_violation(i) = violation(i) / tolerance_(i);
+  }
+  return scaled_violation;
+}
+
+template <int P>
+size_t VectorExpressionEquality<P>::dim() const {
+  return P;
+}
+
+}  // namespace gtdynamics

gtdynamics/optimizer/EqualityConstraint.h

@@ -0,0 +1,142 @@
+/* ----------------------------------------------------------------------------
+ * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file  EqualityConstraint.h
+ * @brief Equality constraints in constrained optimization.
+ * @author: Yetong Zhang, Frank Dellaert
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/ExpressionFactor.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtdynamics {
+
+/**
+ * Equality constraint base class.
+ */
+class EqualityConstraint {
+ public:
+  typedef EqualityConstraint This;
+  typedef boost::shared_ptr<This> shared_ptr;
+
+  /** Default constructor. */
+  EqualityConstraint() {}
+
+  /** Destructor. */
+  virtual ~EqualityConstraint() {}
+
+  /**
+   * @brief Create a factor representing the component in the merit function.
+   *
+   * @param mu penalty parameter.
+   * @param bias additional bias.
+   * @return a factor representing 1/2 mu||g(x)+bias||_Diag(tolerance^2)^2.
+   */
+  virtual gtsam::NoiseModelFactor::shared_ptr createFactor(
+      const double mu,
+      boost::optional<gtsam::Vector&> bias = boost::none) const = 0;
+
+  /**
+   * @brief Check if constraint violation is within tolerance.
+   *
+   * @param x values to evalute constraint at.
+   * @return bool representing if is feasible.
+   */
+  virtual bool feasible(const gtsam::Values& x) const = 0;
+
+  /**
+   * @brief Evaluate the constraint violation, g(x).
+   *
+   * @param x values to evalute constraint at.
+   * @return a vector representing the constraint violation in each dimension.
+   */
+  virtual gtsam::Vector operator()(const gtsam::Values& x) const = 0;
+
+  /** @brief Constraint violation scaled by tolerance, e.g. g(x)/tolerance. */
+  virtual gtsam::Vector toleranceScaledViolation(
+      const gtsam::Values& x) const = 0;
+
+  /** @brief return the dimension of the constraint. */
+  virtual size_t dim() const = 0;
+};
+
+/** Equality constraint that force g(x) = 0, where g(x) is a scalar-valued
+ * function. */
+class DoubleExpressionEquality : public EqualityConstraint {
+ public:
+ protected:
+  gtsam::Expression<double> expression_;
+  double tolerance_;
+
+ public:
+  /**
+   * @brief Constructor.
+   *
+   * @param expression  expression representing g(x).
+   * @param tolerance   scalar representing tolerance.
+   */
+  DoubleExpressionEquality(const gtsam::Expression<double>& expression,
+                           const double& tolerance)
+      : expression_(expression), tolerance_(tolerance) {}
+
+  gtsam::NoiseModelFactor::shared_ptr createFactor(
+      const double mu,
+      boost::optional<gtsam::Vector&> bias = boost::none) const override;
+
+  bool feasible(const gtsam::Values& x) const override;
+
+  gtsam::Vector operator()(const gtsam::Values& x) const override;
+
+  gtsam::Vector toleranceScaledViolation(const gtsam::Values& x) const override;
+
+  size_t dim() const override;
+};
+
+/** Equality constraint that force g(x) = 0, where g(x) is a vector-valued
+ * function. */
+template <int P>
+class VectorExpressionEquality : public EqualityConstraint {
+ public:
+  using VectorP = Eigen::Matrix<double, P, 1>;
+
+ protected:
+  gtsam::Expression<VectorP> expression_;
+  VectorP tolerance_;
+
+ public:
+  /**
+   * @brief Constructor.
+   *
+   * @param expression  expression representing g(x).
+   * @param tolerance   vector representing tolerance in each dimension.
+   */
+  VectorExpressionEquality(const gtsam::Expression<VectorP>& expression,
+                           const VectorP& tolerance)
+      : expression_(expression), tolerance_(tolerance) {}
+
+  gtsam::NoiseModelFactor::shared_ptr createFactor(
+      const double mu,
+      boost::optional<gtsam::Vector&> bias = boost::none) const override;
+
+  bool feasible(const gtsam::Values& x) const override;
+
+  gtsam::Vector operator()(const gtsam::Values& x) const override;
+
+  gtsam::Vector toleranceScaledViolation(const gtsam::Values& x) const override;
+
+  size_t dim() const override;
+};
+
+/// Container of EqualityConstraint.
+typedef std::vector<EqualityConstraint::shared_ptr> EqualityConstraints;
+
+}  // namespace gtdynamics
+
+#include <gtdynamics/optimizer/EqualityConstraint-inl.h>

tests/constrainedExample.h

@@ -0,0 +1,81 @@
+/* ----------------------------------------------------------------------------
+ * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file  constrainedExample.h
+ * @brief Examples for constrained optimization. From
+ * http://www.seas.ucla.edu/~vandenbe/133B/lectures/nllseq.pdf
+ * @author Yetong Zhang
+ */
+
+#pragma once
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/ExpressionFactor.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/nonlinear/expressions.h>
+
+namespace gtdynamics {
+namespace constrained_example {
+
+using gtsam::Double_;
+using gtsam::Expression;
+using gtsam::Key;
+using gtsam::Symbol;
+using gtsam::Vector1;
+using gtsam::Vector2;
+using gtsam::Vector2_;
+
+/** First cost function. */
+double cost1(const double &x1, const double &x2,
+             gtsam::OptionalJacobian<1, 1> H_x1 = boost::none,
+             gtsam::OptionalJacobian<1, 1> H_x2 = boost::none) {
+  double result = x1 + exp(-x2);
+  if (H_x1) H_x1->setConstant(1.0);
+  if (H_x2) H_x2->setConstant(-exp(-x2));
+  return result;
+}
+
+/** Second cost function. */
+double cost2(const double &x1, const double &x2,
+             gtsam::OptionalJacobian<1, 1> H_x1 = boost::none,
+             gtsam::OptionalJacobian<1, 1> H_x2 = boost::none) {
+  double result = x1 * x1 + 2 * x2 + 1;
+  if (H_x1) H_x1->setConstant(2 * x1);
+  if (H_x2) H_x2->setConstant(2.0);
+  return result;
+}
+
+/** Constraint function g(x). */
+double constraint1(const double &x1, const double &x2,
+                   gtsam::OptionalJacobian<1, 1> H_x1 = boost::none,
+                   gtsam::OptionalJacobian<1, 1> H_x2 = boost::none) {
+  double result = x1 + x1 * x1 * x1 + x2 + x2 * x2;
+  if (H_x1) H_x1->setConstant(1 + 3 * x1 * x1);
+  if (H_x2) H_x2->setConstant(1 + 2 * x2);
+  return result;
+}
+
+Symbol x1_key('x', 1);
+Symbol x2_key('x', 2);
+
+Double_ x1_expr(x1_key);
+Double_ x2_expr(x2_key);
+Double_ cost1_expr(cost1, x1_expr, x2_expr);
+Double_ cost2_expr(cost2, x1_expr, x2_expr);
+Double_ constraint1_expr(constraint1, x1_expr, x2_expr);
+
+/// A 2-dimensional function that adds up 2 Vector2.
+Vector2_ x1_vec_expr(x1_key);
+Vector2_ x2_vec_expr(x2_key);
+Vector2_ constraint_sum_vector2_expr = x1_vec_expr + x2_vec_expr;
+
+}  // namespace constrained_example
+
+}  // namespace gtdynamics