gtsam 4.1.1
gtsam
LinearCost.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
23namespace gtsam {
24
25typedef Eigen::RowVectorXd RowVector;
26
32public:
33 typedef LinearCost This;
35 typedef boost::shared_ptr<This> shared_ptr;
36
37public:
40 Base() {
41 }
42
44 explicit LinearCost(const HessianFactor& hf) {
45 throw std::runtime_error("Cannot convert HessianFactor to LinearCost");
46 }
47
49 explicit LinearCost(const JacobianFactor& jf) :
50 Base(jf) {
51 if (jf.isConstrained()) {
52 throw std::runtime_error(
53 "Cannot convert a constrained JacobianFactor to LinearCost");
54 }
55
56 if (jf.get_model()->dim() != 1) {
57 throw std::runtime_error(
58 "Only support single-valued linear cost factor!");
59 }
60 }
61
63 LinearCost(Key i1, const RowVector& A1) :
64 Base(i1, A1, Vector1::Zero()) {
65 }
66
68 LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b) :
69 Base(i1, A1, i2, A2, Vector1::Zero()) {
70 }
71
73 LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3,
74 const RowVector& A3) :
75 Base(i1, A1, i2, A2, i3, A3, Vector1::Zero()) {
76 }
77
81 template<typename TERMS>
82 LinearCost(const TERMS& terms) :
83 Base(terms, Vector1::Zero()) {
84 }
85
87 ~LinearCost() override {
88 }
89
91 bool equals(const GaussianFactor& lf, double tol = 1e-9) const override {
92 return Base::equals(lf, tol);
93 }
94
96 void print(const std::string& s = "", const KeyFormatter& formatter =
97 DefaultKeyFormatter) const override {
98 Base::print(s + " LinearCost: ", formatter);
99 }
100
103 return boost::static_pointer_cast < GaussianFactor
104 > (boost::make_shared < LinearCost > (*this));
105 }
106
108 Vector error_vector(const VectorValues& c) const {
109 return unweighted_error(c);
110 }
111
113 double error(const VectorValues& c) const override {
114 return error_vector(c)[0];
115 }
116};
117// \ LinearCost
118
120template<> struct traits<LinearCost> : public Testable<LinearCost> {
121};
122
123} // \ namespace gtsam
124
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
This is the base class for all factor types.
Definition: Factor.h:56
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition: GaussianFactor.h:39
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:101
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:91
const SharedDiagonal & get_model() const
get a copy of model
Definition: JacobianFactor.h:289
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition: JacobianFactor.cpp:414
bool isConstrained() const
is noise model constrained ?
Definition: JacobianFactor.h:267
bool equals(const GaussianFactor &lf, double tol=1e-9) const override
Equals for testable.
Definition: JacobianFactor.cpp:431
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
This class defines a linear cost function c'x which is a JacobianFactor with only one row.
Definition: LinearCost.h:31
~LinearCost() override
Virtual destructor.
Definition: LinearCost.h:87
LinearCost(Key i1, const RowVector &A1)
Construct unary factor.
Definition: LinearCost.h:63
bool equals(const GaussianFactor &lf, double tol=1e-9) const override
equals
Definition: LinearCost.h:91
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: LinearCost.h:35
LinearCost(Key i1, const RowVector &A1, Key i2, const RowVector &A2, Key i3, const RowVector &A3)
Construct ternary factor.
Definition: LinearCost.h:73
JacobianFactor Base
Typedef to base class.
Definition: LinearCost.h:34
Vector error_vector(const VectorValues &c) const
Special error_vector for constraints (A*x-b)
Definition: LinearCost.h:108
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition: LinearCost.h:96
LinearCost()
default constructor for I/O
Definition: LinearCost.h:39
double error(const VectorValues &c) const override
Special error for single-valued inequality constraints.
Definition: LinearCost.h:113
LinearCost This
Typedef to this class.
Definition: LinearCost.h:33
LinearCost(const TERMS &terms)
Construct an n-ary factor.
Definition: LinearCost.h:82
LinearCost(const HessianFactor &hf)
Conversion from HessianFactor.
Definition: LinearCost.h:44
LinearCost(const JacobianFactor &jf)
Conversion from JacobianFactor.
Definition: LinearCost.h:49
LinearCost(Key i1, const RowVector &A1, Key i2, const RowVector &A2, double b)
Construct binary factor.
Definition: LinearCost.h:68
GaussianFactor::shared_ptr clone() const override
Clone this LinearCost.
Definition: LinearCost.h:102