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 
25 namespace gtsam {
26 
27 typedef Eigen::RowVectorXd RowVector;
28 
34 public:
36  typedef JacobianFactor Base;
37  typedef std::shared_ptr<This> shared_ptr;
38 
39 private:
41  bool active_;
42 
43 public:
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 
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 
70  Base(i1, A1, (Vector(1) << b).finished(),
71  noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) {
72  }
73 
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 
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 std::static_pointer_cast < GaussianFactor
119  > (std::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 
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 
167 template<> struct traits<LinearInequality> : public Testable<LinearInequality> {
168 };
169 
170 } // \ namespace gtsam
171 
gtsam::LinearInequality::dualKey
Key dualKey() const
dual key
Definition: LinearInequality.h:123
gtsam::LinearInequality::active
bool active() const
return true if this constraint is active
Definition: LinearInequality.h:128
gtsam::LinearInequality::error_vector
Vector error_vector(const VectorValues &c) const
Definition: LinearInequality.h:143
gtsam::JacobianFactor::unweighted_error
Vector unweighted_error(const VectorValues &c) const
Definition: JacobianFactor.cpp:471
A3
static const double A3[]
Definition: expn.h:8
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:100
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::LinearInequality::print
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
Definition: LinearInequality.h:108
gtsam::JacobianFactor::get_model
const SharedDiagonal & get_model() const
Definition: JacobianFactor.h:292
gtsam::LinearInequality::LinearInequality
LinearInequality()
Definition: LinearInequality.h:45
gtsam::RowVector
Eigen::RowVectorXd RowVector
Definition: LinearCost.h:25
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::JacobianFactor::isConstrained
bool isConstrained() const
Definition: JacobianFactor.h:270
gtsam::LinearInequality::LinearInequality
LinearInequality(Key i1, const RowVector &A1, Key i2, const RowVector &A2, double b, Key dualKey)
Definition: LinearInequality.h:75
gtsam::LinearInequality::LinearInequality
LinearInequality(Key i1, const RowVector &A1, Key i2, const RowVector &A2, Key i3, const RowVector &A3, double b, Key dualKey)
Definition: LinearInequality.h:82
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::GaussianFactor
Definition: GaussianFactor.h:38
gtsam::LinearInequality::active_
bool active_
Definition: LinearInequality.h:41
formatter
const KeyFormatter & formatter
Definition: treeTraversal-inst.h:204
gtsam::LinearInequality::LinearInequality
LinearInequality(const JacobianFactor &jf, Key dualKey)
Definition: LinearInequality.h:56
gtsam::Factor
Definition: Factor.h:70
gtsam::LinearInequality::LinearInequality
LinearInequality(const TERMS &terms, double b, Key dualKey)
Definition: LinearInequality.h:93
gtsam::Factor::begin
const_iterator begin() const
Definition: Factor.h:146
gtsam::LinearInequality
Definition: LinearInequality.h:33
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::LinearInequality::equals
bool equals(const GaussianFactor &lf, double tol=1e-9) const override
Definition: LinearInequality.h:103
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::KeyFormatter
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
gtsam::LinearInequality::Base
JacobianFactor Base
Typedef to base class.
Definition: LinearInequality.h:36
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::LinearInequality::activate
void activate()
Make this inequality constraint active.
Definition: LinearInequality.h:133
A2
static const double A2[]
Definition: expn.h:7
gtsam::LinearInequality::error
double error(const VectorValues &c) const override
Definition: LinearInequality.h:148
VectorValues.h
Factor Graph Values.
gtsam::LinearInequality::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: LinearInequality.h:37
gtsam::Factor::end
const_iterator end() const
Definition: Factor.h:149
gtsam::LinearInequality::LinearInequality
LinearInequality(const HessianFactor &hf)
Definition: LinearInequality.h:50
JacobianFactor.h
gtsam::b
const G & b
Definition: Group.h:79
gtsam::LinearInequality::~LinearInequality
~LinearInequality() override
Definition: LinearInequality.h:99
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:83
gtsam::LinearInequality::inactivate
void inactivate()
Make this inequality constraint inactive.
Definition: LinearInequality.h:138
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
gtsam::traits
Definition: Group.h:36
i1
double i1(double x)
Definition: i1.c:150
gtsam::JacobianFactor::equals
bool equals(const GaussianFactor &lf, double tol=1e-9) const override
assert equality up to a tolerance
Definition: JacobianFactor.cpp:420
p
float * p
Definition: Tutorial_Map_using.cpp:9
A1
static const double A1[]
Definition: expn.h:6
gtsam::JacobianFactor::print
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print with optional string
Definition: JacobianFactor.cpp:403
gtsam::LinearInequality::dualKey_
Key dualKey_
Definition: LinearInequality.h:40
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::LinearInequality::LinearInequality
LinearInequality(Key i1, const RowVector &A1, double b, Key dualKey)
Definition: LinearInequality.h:69
gtsam::LinearInequality::clone
GaussianFactor::shared_ptr clone() const override
Definition: LinearInequality.h:117
gtsam::JacobianFactor::getA
constABlock getA() const
Definition: JacobianFactor.h:304
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::LinearInequality::dotProductRow
double dotProductRow(const VectorValues &p) const
Definition: LinearInequality.h:153
gtsam::LinearInequality::This
LinearInequality This
Typedef to this class.
Definition: LinearInequality.h:35


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:07