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
12/**
13 * @file LinearEquality.h
14 * @brief LinearEquality derived from Base with constrained noise model
15 * @date Nov 27, 2014
16 * @author Duy-Nguyen Ta
17 */
18
19#pragma once
20
21#include <gtsam/linear/JacobianFactor.h>
22
23namespace gtsam {
24
25/**
26 * This class defines a linear equality constraints, inheriting JacobianFactor
27 * with the special Constrained noise model
28 */
29class LinearEquality: public JacobianFactor {
30public:
31 typedef LinearEquality This; ///< Typedef to this class
32 typedef JacobianFactor Base; ///< Typedef to base class
33 typedef std::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
34
35private:
36 Key dualKey_;
37
38public:
39 /** default constructor for I/O */
40 LinearEquality() :
41 Base() {
42 }
43
44 /**
45 * Construct from a constrained noisemodel JacobianFactor with a dual key.
46 */
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
55 /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */
56 explicit LinearEquality(const HessianFactor& hf) {
57 throw std::runtime_error("Cannot convert HessianFactor to LinearEquality");
58 }
59
60 /** Construct unary factor */
61 LinearEquality(Key i1, const Matrix& A1, const Vector& b, Key dualKey) :
62 Base(i1, A1, b, noiseModel::Constrained::All(dim: b.rows())), dualKey_(dualKey) {
63 }
64
65 /** Construct binary factor */
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(dim: b.rows())), dualKey_(
69 dualKey) {
70 }
71
72 /** Construct ternary factor */
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(dim: b.rows())), dualKey_(
76 dualKey) {
77 }
78
79 /** Construct an n-ary factor
80 * @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
81 * collection of keys and matrices making up the factor. */
82 template<typename TERMS>
83 LinearEquality(const TERMS& terms, const Vector& b, Key dualKey) :
84 Base(terms, b, noiseModel::Constrained::All(dim: b.rows())), dualKey_(dualKey) {
85 }
86
87 /** Virtual destructor */
88 ~LinearEquality() override {
89 }
90
91 /** equals */
92 bool equals(const GaussianFactor& lf, double tol = 1e-9) const override {
93 return Base::equals(lf, tol);
94 }
95
96 /** print */
97 void print(const std::string& s = "", const KeyFormatter& formatter =
98 DefaultKeyFormatter) const override {
99 Base::print(s, formatter);
100 }
101
102 /** Clone this LinearEquality */
103 GaussianFactor::shared_ptr clone() const override {
104 return std::static_pointer_cast < GaussianFactor
105 > (r: std::make_shared < LinearEquality > (args: *this));
106 }
107
108 /// dual key
109 Key dualKey() const {
110 return dualKey_;
111 }
112
113 /// for active set method: equality constraints are always active
114 bool active() const {
115 return true;
116 }
117
118 /** Special error_vector for constraints (A*x-b) */
119 Vector error_vector(const VectorValues& c) const {
120 return unweighted_error(c);
121 }
122
123 /** Special error for constraints.
124 * I think it should be zero, as this function is meant for objective cost.
125 * But the name "error" can be misleading.
126 * TODO: confirm with Frank!! */
127 double error(const VectorValues& c) const override {
128 return 0.0;
129 }
130
131};
132// \ LinearEquality
133
134/// traits
135template<> struct traits<LinearEquality> : public Testable<LinearEquality> {
136};
137
138} // \ namespace gtsam
139
140