gtsam 4.1.1
gtsam
FunctorizedFactor.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
18#pragma once
19
20#include <gtsam/base/Testable.h>
22
23#include <cmath>
24
25namespace gtsam {
26
58template <typename R, typename T>
59class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
60 private:
62
63 R measured_;
64 SharedNoiseModel noiseModel_;
65 std::function<R(T, boost::optional<Matrix &>)> func_;
66
67 public:
70
78 FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model,
79 const std::function<R(T, boost::optional<Matrix &>)> func)
80 : Base(model, key), measured_(z), noiseModel_(model), func_(func) {}
81
82 ~FunctorizedFactor() override {}
83
85 NonlinearFactor::shared_ptr clone() const override {
86 return boost::static_pointer_cast<NonlinearFactor>(
87 NonlinearFactor::shared_ptr(new FunctorizedFactor<R, T>(*this)));
88 }
89
90 Vector evaluateError(const T &params, boost::optional<Matrix &> H =
91 boost::none) const override {
92 R x = func_(params, H);
93 Vector error = traits<R>::Local(measured_, x);
94 return error;
95 }
96
99 void print(
100 const std::string &s = "",
101 const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
102 Base::print(s, keyFormatter);
103 std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
104 << keyFormatter(this->key()) << ")" << std::endl;
105 traits<R>::Print(measured_, " measurement: ");
106 std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
107 << std::endl;
108 }
109
110 bool equals(const NonlinearFactor &other, double tol = 1e-9) const override {
111 const FunctorizedFactor<R, T> *e =
112 dynamic_cast<const FunctorizedFactor<R, T> *>(&other);
113 return e != nullptr && Base::equals(other, tol) &&
114 traits<R>::Equals(this->measured_, e->measured_, tol);
115 }
117
118 private:
120 friend class boost::serialization::access;
121 template <class ARCHIVE>
122 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
123 ar &boost::serialization::make_nvp(
124 "NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
125 ar &BOOST_SERIALIZATION_NVP(measured_);
126 ar &BOOST_SERIALIZATION_NVP(func_);
127 }
128};
129
131template <typename R, typename T>
133 : public Testable<FunctorizedFactor<R, T>> {};
134
141template <typename T, typename R, typename FUNC>
143 const SharedNoiseModel &model,
144 const FUNC func) {
145 return FunctorizedFactor<R, T>(key, z, model, func);
146}
147
157template <typename R, typename T1, typename T2>
158class GTSAM_EXPORT FunctorizedFactor2 : public NoiseModelFactor2<T1, T2> {
159 private:
161
162 R measured_;
163 SharedNoiseModel noiseModel_;
164 using FunctionType = std::function<R(T1, T2, boost::optional<Matrix &>,
165 boost::optional<Matrix &>)>;
166 FunctionType func_;
167
168 public:
171
179 FunctorizedFactor2(Key key1, Key key2, const R &z,
180 const SharedNoiseModel &model, const FunctionType func)
181 : Base(model, key1, key2),
182 measured_(z),
183 noiseModel_(model),
184 func_(func) {}
185
186 ~FunctorizedFactor2() override {}
187
189 NonlinearFactor::shared_ptr clone() const override {
190 return boost::static_pointer_cast<NonlinearFactor>(
191 NonlinearFactor::shared_ptr(new FunctorizedFactor2<R, T1, T2>(*this)));
192 }
193
195 const T1 &params1, const T2 &params2,
196 boost::optional<Matrix &> H1 = boost::none,
197 boost::optional<Matrix &> H2 = boost::none) const override {
198 R x = func_(params1, params2, H1, H2);
199 Vector error = traits<R>::Local(measured_, x);
200 return error;
201 }
202
205 void print(
206 const std::string &s = "",
207 const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
208 Base::print(s, keyFormatter);
209 std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor2("
210 << keyFormatter(this->key1()) << ", "
211 << keyFormatter(this->key2()) << ")" << std::endl;
212 traits<R>::Print(measured_, " measurement: ");
213 std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
214 << std::endl;
215 }
216
217 bool equals(const NonlinearFactor &other, double tol = 1e-9) const override {
219 dynamic_cast<const FunctorizedFactor2<R, T1, T2> *>(&other);
220 return e && Base::equals(other, tol) &&
221 traits<R>::Equals(this->measured_, e->measured_, tol);
222 }
224
225 private:
227 friend class boost::serialization::access;
228 template <class ARCHIVE>
229 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
230 ar &boost::serialization::make_nvp(
231 "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
232 ar &BOOST_SERIALIZATION_NVP(measured_);
233 ar &BOOST_SERIALIZATION_NVP(func_);
234 }
235};
236
238template <typename R, typename T1, typename T2>
239struct traits<FunctorizedFactor2<R, T1, T2>>
240 : public Testable<FunctorizedFactor2<R, T1, T2>> {};
241
248template <typename T1, typename T2, typename R, typename FUNC>
250 Key key1, Key key2, const R &z, const SharedNoiseModel &model,
251 const FUNC func) {
252 return FunctorizedFactor2<R, T1, T2>(key1, key2, z, model, func);
253}
254
255} // namespace gtsam
Concept check for values that can be used in unit tests.
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
FunctorizedFactor2< R, T1, T2 > MakeFunctorizedFactor2(Key key1, Key key2, const R &z, const SharedNoiseModel &model, const FUNC func)
Helper function to create a functorized factor.
Definition: FunctorizedFactor.h:249
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:112
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
FunctorizedFactor< R, T > MakeFunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model, const FUNC func)
Helper function to create a functorized factor.
Definition: FunctorizedFactor.h:142
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:736
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
Factor which evaluates provided unary functor and uses the result to compute error with respect to th...
Definition: FunctorizedFactor.h:59
FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model, const std::function< R(T, boost::optional< Matrix & >)> func)
Construct with given x and the parameters of the basis.
Definition: FunctorizedFactor.h:78
bool equals(const NonlinearFactor &other, double tol=1e-9) const override
Check if two factors are equal.
Definition: FunctorizedFactor.h:110
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: FunctorizedFactor.h:99
Vector evaluateError(const T &params, boost::optional< Matrix & > H=boost::none) const override
Override this method to finish implementing a unary factor.
Definition: FunctorizedFactor.h:90
FunctorizedFactor()
default constructor - only use for serialization
Definition: FunctorizedFactor.h:69
NonlinearFactor::shared_ptr clone() const override
Definition: FunctorizedFactor.h:85
Factor which evaluates provided binary functor and uses the result to compute error with respect to t...
Definition: FunctorizedFactor.h:158
FunctorizedFactor2()
default constructor - only use for serialization
Definition: FunctorizedFactor.h:170
Vector evaluateError(const T1 &params1, const T2 &params2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Override this method to finish implementing a binary factor.
Definition: FunctorizedFactor.h:194
NonlinearFactor::shared_ptr clone() const override
Definition: FunctorizedFactor.h:189
FunctorizedFactor2(Key key1, Key key2, const R &z, const SharedNoiseModel &model, const FunctionType func)
Construct with given x and the parameters of the basis.
Definition: FunctorizedFactor.h:179
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: FunctorizedFactor.h:205
bool equals(const NonlinearFactor &other, double tol=1e-9) const override
Check if two factors are equal.
Definition: FunctorizedFactor.h:217
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:285
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:369