InequalityFactorGraph.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_unstable/linear/LinearInequality.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/FactorGraph.h>

namespace gtsam {

class InequalityFactorGraph: public FactorGraph<LinearInequality> {
private:
  typedef FactorGraph<LinearInequality> Base;

public:
  typedef std::shared_ptr<InequalityFactorGraph> shared_ptr;

  void print(
      const std::string& str = "",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
    Base::print(str, keyFormatter);
  }

  bool equals(const InequalityFactorGraph& other, double tol = 1e-9) const {
    return Base::equals(other, tol);
  }

  template <class... Args> void add(Args &&... args) {
    emplace_shared<LinearInequality>(std::forward<Args>(args)...);
  }

  double error(const VectorValues& x) const {
    for (const sharedFactor& factor : *this) {
      if (factor)
        if (factor->error(x) > 1e-7)
          return std::numeric_limits<double>::infinity();
    }
    return 0.0;
  }
};

template<>
struct traits<InequalityFactorGraph> : public Testable<InequalityFactorGraph> {
};

} // \ namespace gtsam