gtsam 4.1.1
gtsam
LinearInequality.h
Go to the documentation of this file.
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
20#pragma once
21
24
25namespace gtsam {
26
27typedef Eigen::RowVectorXd RowVector;
28
34public:
37 typedef boost::shared_ptr<This> shared_ptr;
38
39private:
40 Key dualKey_;
41 bool active_;
42
43public:
46 Base(), active_(true) {
47 }
48
50 explicit LinearInequality(const HessianFactor& hf) {
51 throw std::runtime_error(
52 "Cannot convert HessianFactor to LinearInequality");
53 }
54
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
69 LinearInequality(Key i1, const RowVector& A1, double b, Key dualKey) :
70 Base(i1, A1, (Vector(1) << b).finished(),
71 noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) {
72 }
73
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(1)), dualKey_(dualKey), active_(true) {
79 }
80
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(1)), dualKey_(dualKey), active_(true) {
86 }
87
92 template<typename TERMS>
93 LinearInequality(const TERMS& terms, double b, Key dualKey) :
94 Base(terms, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_(
95 dualKey), active_(true) {
96 }
97
99 ~LinearInequality() override {
100 }
101
103 bool equals(const GaussianFactor& lf, double tol = 1e-9) const override {
104 return Base::equals(lf, tol);
105 }
106
108 void print(const std::string& s = "", const KeyFormatter& formatter =
109 DefaultKeyFormatter) const override {
110 if (active())
111 Base::print(s + " Active", formatter);
112 else
113 Base::print(s + " Inactive", formatter);
114 }
115
118 return boost::static_pointer_cast < GaussianFactor
119 > (boost::make_shared < LinearInequality > (*this));
120 }
121
123 Key dualKey() const {
124 return dualKey_;
125 }
126
128 bool active() const {
129 return active_;
130 }
131
133 void activate() {
134 active_ = true;
135 }
136
138 void inactivate() {
139 active_ = false;
140 }
141
143 Vector error_vector(const VectorValues& c) const {
144 return unweighted_error(c);
145 }
146
148 double error(const VectorValues& c) const override {
149 return error_vector(c)[0];
150 }
151
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(*xj);
157 Vector aj = getA(xj).transpose();
158 aTp += aj.dot(pj);
159 }
160 return aTp;
161 }
162
163};
164// \ LinearInequality
165
167template<> struct traits<LinearInequality> : public Testable<LinearInequality> {
168};
169
170} // \ namespace gtsam
171
Factor Graph Values.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
This is the base class for all factor types.
Definition: Factor.h:56
const_iterator begin() const
Iterator at beginning of involved variable keys.
Definition: Factor.h:128
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:68
const_iterator end() const
Iterator at end of involved variable keys.
Definition: Factor.h:131
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition: GaussianFactor.h:39
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:101
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:91
const SharedDiagonal & get_model() const
get a copy of model
Definition: JacobianFactor.h:289
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition: JacobianFactor.cpp:414
bool isConstrained() const
is noise model constrained ?
Definition: JacobianFactor.h:267
constABlock getA() const
Get a view of the A matrix, not weighted by noise.
Definition: JacobianFactor.h:301
bool equals(const GaussianFactor &lf, double tol=1e-9) const override
Equals for testable.
Definition: JacobianFactor.cpp:431
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
Vector & at(Key j)
Read/write access to the vector value with key j, throws std::out_of_range if j does not exist,...
Definition: VectorValues.h:137
This class defines a linear inequality constraint Ax-b <= 0, inheriting JacobianFactor with the speci...
Definition: LinearInequality.h:33
bool equals(const GaussianFactor &lf, double tol=1e-9) const override
equals
Definition: LinearInequality.h:103
void inactivate()
Make this inequality constraint inactive.
Definition: LinearInequality.h:138
GaussianFactor::shared_ptr clone() const override
Clone this LinearInequality.
Definition: LinearInequality.h:117
LinearInequality()
default constructor for I/O
Definition: LinearInequality.h:45
LinearInequality(const TERMS &terms, double b, Key dualKey)
Construct an n-ary factor.
Definition: LinearInequality.h:93
~LinearInequality() override
Virtual destructor.
Definition: LinearInequality.h:99
JacobianFactor Base
Typedef to base class.
Definition: LinearInequality.h:36
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: LinearInequality.h:37
LinearInequality(Key i1, const RowVector &A1, double b, Key dualKey)
Construct unary factor.
Definition: LinearInequality.h:69
LinearInequality(const JacobianFactor &jf, Key dualKey)
Conversion from JacobianFactor.
Definition: LinearInequality.h:56
double dotProductRow(const VectorValues &p) const
dot product of row s with the corresponding vector in p
Definition: LinearInequality.h:153
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition: LinearInequality.h:108
Key dualKey() const
dual key
Definition: LinearInequality.h:123
void activate()
Make this inequality constraint active.
Definition: LinearInequality.h:133
double error(const VectorValues &c) const override
Special error for single-valued inequality constraints.
Definition: LinearInequality.h:148
bool active() const
return true if this constraint is active
Definition: LinearInequality.h:128
LinearInequality(const HessianFactor &hf)
Conversion from HessianFactor.
Definition: LinearInequality.h:50
Vector error_vector(const VectorValues &c) const
Special error_vector for constraints (A*x-b)
Definition: LinearInequality.h:143
LinearInequality(Key i1, const RowVector &A1, Key i2, const RowVector &A2, double b, Key dualKey)
Construct binary factor.
Definition: LinearInequality.h:75
LinearInequality(Key i1, const RowVector &A1, Key i2, const RowVector &A2, Key i3, const RowVector &A3, double b, Key dualKey)
Construct ternary factor.
Definition: LinearInequality.h:82
LinearInequality This
Typedef to this class.
Definition: LinearInequality.h:35