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 LinearInequality.h
14 * @brief LinearInequality derived from Base with constrained noise model
15 * @date Nov 27, 2014
16 * @author Duy-Nguyen Ta
17 * @author Ivan Dario Jimenez
18 */
19
20#pragma once
21
22#include <gtsam/linear/JacobianFactor.h>
23#include <gtsam/linear/VectorValues.h>
24
25namespace gtsam {
26
27typedef Eigen::RowVectorXd RowVector;
28
29/**
30 * This class defines a linear inequality constraint Ax-b <= 0,
31 * inheriting JacobianFactor with the special Constrained noise model
32 */
33class LinearInequality: public JacobianFactor {
34public:
35 typedef LinearInequality This; ///< Typedef to this class
36 typedef JacobianFactor Base; ///< Typedef to base class
37 typedef std::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
38
39private:
40 Key dualKey_;
41 bool active_;
42
43public:
44 /** default constructor for I/O */
45 LinearInequality() :
46 Base(), active_(true) {
47 }
48
49 /** Conversion from HessianFactor */
50 explicit LinearInequality(const HessianFactor& hf) {
51 throw std::runtime_error(
52 "Cannot convert HessianFactor to LinearInequality");
53 }
54
55 /** Conversion from JacobianFactor */
56 explicit LinearInequality(const JacobianFactor& jf, Key dualKey) :
57 Base(jf), dualKey_(dualKey), active_(true) {
58 if (!jf.isConstrained()) {
59 throw std::runtime_error(
60 "Cannot convert an unconstrained JacobianFactor to LinearInequality");
61 }
62
63 if (jf.get_model()->dim() != 1) {
64 throw std::runtime_error("Only support single-valued inequality factor!");
65 }
66 }
67
68 /** Construct unary factor */
69 LinearInequality(Key i1, const RowVector& A1, double b, Key dualKey) :
70 Base(i1, A1, (Vector(1) << b).finished(),
71 noiseModel::Constrained::All(dim: 1)), dualKey_(dualKey), active_(true) {
72 }
73
74 /** Construct binary factor */
75 LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2,
76 double b, Key dualKey) :
77 Base(i1, A1, i2, A2, (Vector(1) << b).finished(),
78 noiseModel::Constrained::All(dim: 1)), dualKey_(dualKey), active_(true) {
79 }
80
81 /** Construct ternary factor */
82 LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2,
83 Key i3, const RowVector& A3, double b, Key dualKey) :
84 Base(i1, A1, i2, A2, i3, A3, (Vector(1) << b).finished(),
85 noiseModel::Constrained::All(dim: 1)), dualKey_(dualKey), active_(true) {
86 }
87
88 /** Construct an n-ary factor
89 * @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
90 * collection of keys and matrices making up the factor.
91 * In this inequality factor, each matrix must have only one row!! */
92 template<typename TERMS>
93 LinearInequality(const TERMS& terms, double b, Key dualKey) :
94 Base(terms, (Vector(1) << b).finished(), noiseModel::Constrained::All(dim: 1)), dualKey_(
95 dualKey), active_(true) {
96 }
97
98 /** Virtual destructor */
99 ~LinearInequality() override {
100 }
101
102 /** equals */
103 bool equals(const GaussianFactor& lf, double tol = 1e-9) const override {
104 return Base::equals(lf, tol);
105 }
106
107 /** print */
108 void print(const std::string& s = "", const KeyFormatter& formatter =
109 DefaultKeyFormatter) const override {
110 if (active())
111 Base::print(s: s + " Active", formatter);
112 else
113 Base::print(s: s + " Inactive", formatter);
114 }
115
116 /** Clone this LinearInequality */
117 GaussianFactor::shared_ptr clone() const override {
118 return std::static_pointer_cast < GaussianFactor
119 > (r: std::make_shared < LinearInequality > (args: *this));
120 }
121
122 /// dual key
123 Key dualKey() const {
124 return dualKey_;
125 }
126
127 /// return true if this constraint is active
128 bool active() const {
129 return active_;
130 }
131
132 /// Make this inequality constraint active
133 void activate() {
134 active_ = true;
135 }
136
137 /// Make this inequality constraint inactive
138 void inactivate() {
139 active_ = false;
140 }
141
142 /** Special error_vector for constraints (A*x-b) */
143 Vector error_vector(const VectorValues& c) const {
144 return unweighted_error(c);
145 }
146
147 /** Special error for single-valued inequality constraints. */
148 double error(const VectorValues& c) const override {
149 return error_vector(c)[0];
150 }
151
152 /** dot product of row s with the corresponding vector in p */
153 double dotProductRow(const VectorValues& p) const {
154 double aTp = 0.0;
155 for (const_iterator xj = begin(); xj != end(); ++xj) {
156 Vector pj = p.at(j: *xj);
157 Vector aj = getA(variable: xj).transpose();
158 aTp += aj.dot(other: pj);
159 }
160 return aTp;
161 }
162
163};
164// \ LinearInequality
165
166/// traits
167template<> struct traits<LinearInequality> : public Testable<LinearInequality> {
168};
169
170} // \ namespace gtsam
171
172