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 
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 
gtsam::LinearEquality::dualKey
Key dualKey() const
dual key
Definition: LinearEquality.h:109
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.)
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::JacobianFactor::isConstrained
bool isConstrained() const
Definition: JacobianFactor.h:270
gtsam::LinearEquality::LinearEquality
LinearEquality(Key i1, const Matrix &A1, const Vector &b, Key dualKey)
Definition: LinearEquality.h:61
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::LinearEquality::LinearEquality
LinearEquality(const TERMS &terms, const Vector &b, Key dualKey)
Definition: LinearEquality.h:83
gtsam::GaussianFactor
Definition: GaussianFactor.h:38
formatter
const KeyFormatter & formatter
Definition: treeTraversal-inst.h:204
gtsam::LinearEquality::LinearEquality
LinearEquality()
Definition: LinearEquality.h:40
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Factor
Definition: Factor.h:70
gtsam::LinearEquality::error
double error(const VectorValues &c) const override
Definition: LinearEquality.h:127
gtsam::JacobianFactor::rows
size_t rows() const
Definition: JacobianFactor.h:284
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::LinearEquality::LinearEquality
LinearEquality(const JacobianFactor &jf, Key dualKey)
Definition: LinearEquality.h:47
gtsam::LinearEquality
Definition: LinearEquality.h:29
gtsam::LinearEquality::LinearEquality
LinearEquality(Key i1, const Matrix &A1, Key i2, const Matrix &A2, const Vector &b, Key dualKey)
Definition: LinearEquality.h:66
gtsam::LinearEquality::error_vector
Vector error_vector(const VectorValues &c) const
Definition: LinearEquality.h:119
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::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
A2
static const double A2[]
Definition: expn.h:7
gtsam::LinearEquality::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: LinearEquality.h:33
gtsam::LinearEquality::active
bool active() const
for active set method: equality constraints are always active
Definition: LinearEquality.h:114
gtsam::LinearEquality::This
LinearEquality This
Typedef to this class.
Definition: LinearEquality.h:31
gtsam::LinearEquality::clone
GaussianFactor::shared_ptr clone() const override
Definition: LinearEquality.h:103
JacobianFactor.h
gtsam::b
const G & b
Definition: Group.h:79
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
gtsam::LinearEquality::equals
bool equals(const GaussianFactor &lf, double tol=1e-9) const override
Definition: LinearEquality.h:92
gtsam::LinearEquality::LinearEquality
LinearEquality(Key i1, const Matrix &A1, Key i2, const Matrix &A2, Key i3, const Matrix &A3, const Vector &b, Key dualKey)
Definition: LinearEquality.h:73
A1
static const double A1[]
Definition: expn.h:6
gtsam::LinearEquality::LinearEquality
LinearEquality(const HessianFactor &hf)
Definition: LinearEquality.h:56
gtsam::LinearEquality::dualKey_
Key dualKey_
Definition: LinearEquality.h:36
gtsam::JacobianFactor::print
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print with optional string
Definition: JacobianFactor.cpp:403
gtsam::LinearEquality::~LinearEquality
~LinearEquality() override
Definition: LinearEquality.h:88
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::LinearEquality::print
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
Definition: LinearEquality.h:97
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::LinearEquality::Base
JacobianFactor Base
Typedef to base class.
Definition: LinearEquality.h:32


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:42