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 std::shared_ptr<This> shared_ptr;
34 
35 private:
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  ~LinearEquality() override {
89  }
90 
92  bool equals(const GaussianFactor& lf, double tol = 1e-9) const override {
93  return Base::equals(lf, tol);
94  }
95 
97  void print(const std::string& s = "", const KeyFormatter& formatter =
98  DefaultKeyFormatter) const override {
100  }
101 
104  return std::static_pointer_cast < GaussianFactor
105  > (std::make_shared < LinearEquality > (*this));
106  }
107 
109  Key dualKey() const {
110  return dualKey_;
111  }
112 
114  bool active() const {
115  return true;
116  }
117 
120  return unweighted_error(c);
121  }
122 
127  double error(const VectorValues& c) const override {
128  return 0.0;
129  }
130 
131 };
132 // \ LinearEquality
133 
135 template<> struct traits<LinearEquality> : public Testable<LinearEquality> {
136 };
137 
138 } // \ namespace gtsam
139 
JacobianFactor Base
Typedef to base class.
static Matrix A1
LinearEquality(Key i1, const Matrix &A1, const Vector &b, Key dualKey)
LinearEquality(const TERMS &terms, const Vector &b, Key dualKey)
double error(const VectorValues &c) const override
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
Vector unweighted_error(const VectorValues &c) const
const KeyFormatter & formatter
bool isConstrained() const
LinearEquality(const JacobianFactor &jf, Key dualKey)
Eigen::VectorXd Vector
Definition: Vector.h:38
Vector error_vector(const VectorValues &c) const
LinearEquality(Key i1, const Matrix &A1, Key i2, const Matrix &A2, const Vector &b, Key dualKey)
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print with optional string
bool equals(const GaussianFactor &lf, double tol=1e-9) const override
assert equality up to a tolerance
GaussianFactor::shared_ptr clone() const override
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
LinearEquality(Key i1, const Matrix &A1, Key i2, const Matrix &A2, Key i3, const Matrix &A3, const Vector &b, Key dualKey)
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
const G & b
Definition: Group.h:86
std::shared_ptr< This > shared_ptr
shared_ptr to this class
bool active() const
for active set method: equality constraints are always active
traits
Definition: chartTesting.h:28
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
std::shared_ptr< This > shared_ptr
shared_ptr to this class
LinearEquality This
Typedef to this class.
A Gaussian factor using the canonical parameters (information form)
LinearEquality(const HessianFactor &hf)
const G double tol
Definition: Group.h:86
Key dualKey() const
dual key
bool equals(const GaussianFactor &lf, double tol=1e-9) const override
~LinearEquality() override
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:33