1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file InequalityFactorGraph.h
14 * @brief Factor graph of all LinearInequality factors
15 * @date Dec 8, 2014
16 * @author Duy-Nguyen Ta
17 */
18
19#pragma once
20
21#include <gtsam_unstable/linear/LinearInequality.h>
22#include <gtsam/inference/FactorGraph-inst.h>
23#include <gtsam/linear/VectorValues.h>
24#include <gtsam/inference/FactorGraph.h>
25
26namespace gtsam {
27
28/**
29 * Collection of all Linear Inequality constraints Ax-b <= 0 of
30 * a Programming problem as a Factor Graph
31 */
32class InequalityFactorGraph: public FactorGraph<LinearInequality> {
33private:
34 typedef FactorGraph<LinearInequality> Base;
35
36public:
37 typedef std::shared_ptr<InequalityFactorGraph> shared_ptr;
38
39 /** print */
40 void print(
41 const std::string& str = "",
42 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
43 Base::print(s: str, formatter: keyFormatter);
44 }
45
46 /** equals */
47 bool equals(const InequalityFactorGraph& other, double tol = 1e-9) const {
48 return Base::equals(fg: other, tol);
49 }
50
51 /// Add a linear inequality, forwards arguments to LinearInequality.
52 template <class... Args> void add(Args &&... args) {
53 emplace_shared<LinearInequality>(std::forward<Args>(args)...);
54 }
55
56 /**
57 * Compute error of a guess.
58 * Infinity error if it violates an inequality; zero otherwise. */
59 double error(const VectorValues& x) const {
60 for (const sharedFactor& factor : *this) {
61 if (factor)
62 if (factor->error(c: x) > 1e-7)
63 return std::numeric_limits<double>::infinity();
64 }
65 return 0.0;
66 }
67};
68
69/// traits
70template<>
71struct traits<InequalityFactorGraph> : public Testable<InequalityFactorGraph> {
72};
73
74} // \ namespace gtsam
75
76