LinearInequality.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/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
 
namespace gtsam {
 
typedef Eigen::RowVectorXd RowVector;
 
class LinearInequality: public JacobianFactor {
public:
  typedef LinearInequality This; 
  typedef JacobianFactor Base; 
  typedef std::shared_ptr<This> shared_ptr; 
 
private:
  Key dualKey_;
  bool active_;
 
public:
  LinearInequality() :
      Base(), active_(true) {
  }
 
  explicit LinearInequality(const HessianFactor& hf) {
    throw std::runtime_error(
        "Cannot convert HessianFactor to LinearInequality");
  }
 
  explicit LinearInequality(const JacobianFactor& jf, Key dualKey) :
      Base(jf), dualKey_(dualKey), active_(true) {
    if (!jf.isConstrained()) {
      throw std::runtime_error(
          "Cannot convert an unconstrained JacobianFactor to LinearInequality");
    }
 
    if (jf.get_model()->dim() != 1) {
      throw std::runtime_error("Only support single-valued inequality factor!");
    }
  }
 
  LinearInequality(Key i1, const RowVector& A1, double b, Key dualKey) :
      Base(i1, A1, (Vector(1) << b).finished(),
          noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) {
  }
 
  LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2,
      double b, Key dualKey) :
      Base(i1, A1, i2, A2, (Vector(1) << b).finished(),
          noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) {
  }
 
  LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2,
      Key i3, const RowVector& A3, double b, Key dualKey) :
      Base(i1, A1, i2, A2, i3, A3, (Vector(1) << b).finished(),
          noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) {
  }
 
  template<typename TERMS>
  LinearInequality(const TERMS& terms, double b, Key dualKey) :
      Base(terms, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_(
          dualKey), active_(true) {
  }
 
  ~LinearInequality() override {
  }
 
  bool equals(const GaussianFactor& lf, double tol = 1e-9) const override {
    return Base::equals(lf, tol);
  }
 
  void print(const std::string& s = "", const KeyFormatter& formatter =
      DefaultKeyFormatter) const override {
    if (active())
      Base::print(s + "  Active", formatter);
    else
      Base::print(s + "  Inactive", formatter);
  }
 
  GaussianFactor::shared_ptr clone() const override {
    return std::static_pointer_cast < GaussianFactor
        > (std::make_shared < LinearInequality > (*this));
  }
 
  Key dualKey() const {
    return dualKey_;
  }
 
  bool active() const {
    return active_;
  }
 
  void activate() {
    active_ = true;
  }
 
  void inactivate() {
    active_ = false;
  }
 
  Vector error_vector(const VectorValues& c) const {
    return unweighted_error(c);
  }
 
  double error(const VectorValues& c) const override {
    return error_vector(c)[0];
  }
 
  double dotProductRow(const VectorValues& p) const {
    double aTp = 0.0;
    for (const_iterator xj = begin(); xj != end(); ++xj) {
      Vector pj = p.at(*xj);
      Vector aj = getA(xj).transpose();
      aTp += aj.dot(pj);
    }
    return aTp;
  }
 
};
// \ LinearInequality
 
template<> struct traits<LinearInequality> : public Testable<LinearInequality> {
};
 
} // \ namespace gtsam