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 LinearCost.h
14 * @brief LinearCost derived from JacobianFactor to support linear cost functions c'x
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
25typedef Eigen::RowVectorXd RowVector;
26
27/**
28 * This class defines a linear cost function c'x
29 * which is a JacobianFactor with only one row
30 */
31class LinearCost: public JacobianFactor {
32public:
33 typedef LinearCost This; ///< Typedef to this class
34 typedef JacobianFactor Base; ///< Typedef to base class
35 typedef std::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
36
37public:
38 /** default constructor for I/O */
39 LinearCost() :
40 Base() {
41 }
42
43 /** Conversion from HessianFactor */
44 explicit LinearCost(const HessianFactor& hf) {
45 throw std::runtime_error("Cannot convert HessianFactor to LinearCost");
46 }
47
48 /** Conversion from JacobianFactor */
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
62 /** Construct unary factor */
63 LinearCost(Key i1, const RowVector& A1) :
64 Base(i1, A1, Vector1::Zero()) {
65 }
66
67 /** Construct binary factor */
68 LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b) :
69 Base(i1, A1, i2, A2, Vector1::Zero()) {
70 }
71
72 /** Construct ternary factor */
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
78 /** Construct an n-ary factor
79 * @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
80 * collection of keys and matrices making up the factor. */
81 template<typename TERMS>
82 LinearCost(const TERMS& terms) :
83 Base(terms, Vector1::Zero()) {
84 }
85
86 /** Virtual destructor */
87 ~LinearCost() override {
88 }
89
90 /** equals */
91 bool equals(const GaussianFactor& lf, double tol = 1e-9) const override {
92 return Base::equals(lf, tol);
93 }
94
95 /** print */
96 void print(const std::string& s = "", const KeyFormatter& formatter =
97 DefaultKeyFormatter) const override {
98 Base::print(s: s + " LinearCost: ", formatter);
99 }
100
101 /** Clone this LinearCost */
102 GaussianFactor::shared_ptr clone() const override {
103 return std::static_pointer_cast < GaussianFactor
104 > (r: std::make_shared < LinearCost > (args: *this));
105 }
106
107 /** Special error_vector for constraints (A*x-b) */
108 Vector error_vector(const VectorValues& c) const {
109 return unweighted_error(c);
110 }
111
112 /** Special error for single-valued inequality constraints. */
113 double error(const VectorValues& c) const override {
114 return error_vector(c)[0];
115 }
116};
117// \ LinearCost
118
119/// traits
120template<> struct traits<LinearCost> : public Testable<LinearCost> {
121};
122
123} // \ namespace gtsam
124
125