gtsam  4.0.0
gtsam
LinearEquality.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 
19 #pragma once
20 
22 
23 namespace gtsam {
24 
30 public:
31  typedef LinearEquality This;
32  typedef JacobianFactor Base;
33  typedef boost::shared_ptr<This> shared_ptr;
34 
35 private:
36  Key dualKey_;
37 
38 public:
41  Base() {
42  }
43 
47  explicit LinearEquality(const JacobianFactor& jf, Key dualKey) :
48  Base(jf), dualKey_(dualKey) {
49  if (!jf.isConstrained()) {
50  throw std::runtime_error(
51  "Cannot convert an unconstrained JacobianFactor to LinearEquality");
52  }
53  }
54 
56  explicit LinearEquality(const HessianFactor& hf) {
57  throw std::runtime_error("Cannot convert HessianFactor to LinearEquality");
58  }
59 
61  LinearEquality(Key i1, const Matrix& A1, const Vector& b, Key dualKey) :
62  Base(i1, A1, b, noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) {
63  }
64 
66  LinearEquality(Key i1, const Matrix& A1, Key i2, const Matrix& A2,
67  const Vector& b, Key dualKey) :
68  Base(i1, A1, i2, A2, b, noiseModel::Constrained::All(b.rows())), dualKey_(
69  dualKey) {
70  }
71 
73  LinearEquality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3,
74  const Matrix& A3, const Vector& b, Key dualKey) :
75  Base(i1, A1, i2, A2, i3, A3, b, noiseModel::Constrained::All(b.rows())), dualKey_(
76  dualKey) {
77  }
78 
82  template<typename TERMS>
83  LinearEquality(const TERMS& terms, const Vector& b, Key dualKey) :
84  Base(terms, b, noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) {
85  }
86 
88  virtual ~LinearEquality() {
89  }
90 
92  virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const {
93  return Base::equals(lf, tol);
94  }
95 
97  virtual void print(const std::string& s = "", const KeyFormatter& formatter =
98  DefaultKeyFormatter) const {
99  Base::print(s, formatter);
100  }
101 
104  return boost::static_pointer_cast < GaussianFactor
105  > (boost::make_shared < LinearEquality > (*this));
106  }
107 
109  Key dualKey() const {
110  return dualKey_;
111  }
112 
114  bool active() const {
115  return true;
116  }
117 
119  Vector error_vector(const VectorValues& c) const {
120  return unweighted_error(c);
121  }
122 
127  virtual double error(const VectorValues& c) const {
128  return 0.0;
129  }
130 
131 };
132 // \ LinearEquality
133 
135 template<> struct traits<LinearEquality> : public Testable<LinearEquality> {
136 };
137 
138 } // \ namespace gtsam
139 
This is the base class for all factor types.
Definition: Factor.h:54
bool isConstrained() const
is noise model constrained ?
Definition: JacobianFactor.h:238
virtual bool equals(const GaussianFactor &lf, double tol=1e-9) const
equals
Definition: LinearEquality.h:92
LinearEquality(const JacobianFactor &jf, Key dualKey)
Construct from a constrained noisemodel JacobianFactor with a dual key.
Definition: LinearEquality.h:47
LinearEquality(Key i1, const Matrix &A1, Key i2, const Matrix &A2, Key i3, const Matrix &A3, const Vector &b, Key dualKey)
Construct ternary factor.
Definition: LinearEquality.h:73
LinearEquality This
Typedef to this class.
Definition: LinearEquality.h:31
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:57
JacobianFactor Base
Typedef to base class.
Definition: LinearEquality.h:32
LinearEquality(Key i1, const Matrix &A1, const Vector &b, Key dualKey)
Construct unary factor.
Definition: LinearEquality.h:61
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition: GaussianFactor.h:38
size_t rows() const
return the number of rows in the corresponding linear system
Definition: JacobianFactor.h:250
bool active() const
for active set method: equality constraints are always active
Definition: LinearEquality.h:114
virtual double error(const VectorValues &c) const
Special error for constraints.
Definition: LinearEquality.h:127
boost::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:33
virtual void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition: LinearEquality.h:97
Vector error_vector(const VectorValues &c) const
Special error_vector for constraints (A*x-b)
Definition: LinearEquality.h:119
virtual GaussianFactor::shared_ptr clone() const
Clone this LinearEquality.
Definition: LinearEquality.h:103
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:73
virtual ~LinearEquality()
Virtual destructor.
Definition: LinearEquality.h:88
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: LinearEquality.h:33
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:87
This class defines a linear equality constraints, inheriting JacobianFactor with the special Constrai...
Definition: LinearEquality.h:29
LinearEquality(const TERMS &terms, const Vector &b, Key dualKey)
Construct an n-ary factor.
Definition: LinearEquality.h:83
LinearEquality(Key i1, const Matrix &A1, Key i2, const Matrix &A2, const Vector &b, Key dualKey)
Construct binary factor.
Definition: LinearEquality.h:66
virtual bool equals(const GaussianFactor &lf, double tol=1e-9) const
Equals for testable.
Definition: JacobianFactor.cpp:375
LinearEquality(const HessianFactor &hf)
Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix)
Definition: LinearEquality.h:56
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
LinearEquality()
default constructor for I/O
Definition: LinearEquality.h:40
Key dualKey() const
dual key
Definition: LinearEquality.h:109
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:101
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42