gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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>
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->key1()) << ")" << 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:
121 template <class ARCHIVE>
122 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
123 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
124 ar &boost::serialization::make_nvp(
125 "NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
126 ar &BOOST_SERIALIZATION_NVP(measured_);
127 ar &BOOST_SERIALIZATION_NVP(func_);
128 }
129};
130
132template <typename R, typename T>
134 : public Testable<FunctorizedFactor<R, T>> {};
135
142template <typename T, typename R, typename FUNC>
144 const SharedNoiseModel &model,
145 const FUNC func) {
146 return FunctorizedFactor<R, T>(key, z, model, func);
147}
148
158template <typename R, typename T1, typename T2>
159class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
160 private:
162
163 R measured_;
164 SharedNoiseModel noiseModel_;
165 using FunctionType = std::function<R(T1, T2, boost::optional<Matrix &>,
166 boost::optional<Matrix &>)>;
167 FunctionType func_;
168
169 public:
172
180 FunctorizedFactor2(Key key1, Key key2, const R &z,
181 const SharedNoiseModel &model, const FunctionType func)
182 : Base(model, key1, key2),
183 measured_(z),
184 noiseModel_(model),
185 func_(func) {}
186
187 ~FunctorizedFactor2() override {}
188
190 NonlinearFactor::shared_ptr clone() const override {
191 return boost::static_pointer_cast<NonlinearFactor>(
192 NonlinearFactor::shared_ptr(new FunctorizedFactor2<R, T1, T2>(*this)));
193 }
194
195 Vector evaluateError(
196 const T1 &params1, const T2 &params2,
197 boost::optional<Matrix &> H1 = boost::none,
198 boost::optional<Matrix &> H2 = boost::none) const override {
199 R x = func_(params1, params2, H1, H2);
200 Vector error = traits<R>::Local(measured_, x);
201 return error;
202 }
203
206 void print(
207 const std::string &s = "",
208 const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
209 Base::print(s, keyFormatter);
210 std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor2("
211 << keyFormatter(this->key1()) << ", "
212 << keyFormatter(this->key2()) << ")" << std::endl;
213 traits<R>::Print(measured_, " measurement: ");
214 std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
215 << std::endl;
216 }
217
218 bool equals(const NonlinearFactor &other, double tol = 1e-9) const override {
220 dynamic_cast<const FunctorizedFactor2<R, T1, T2> *>(&other);
221 return e && Base::equals(other, tol) &&
222 traits<R>::Equals(this->measured_, e->measured_, tol);
223 }
225
226 private:
229 template <class ARCHIVE>
230 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
231 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
232 ar &boost::serialization::make_nvp(
233 "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
234 ar &BOOST_SERIALIZATION_NVP(measured_);
235 ar &BOOST_SERIALIZATION_NVP(func_);
236 }
237};
238
240template <typename R, typename T1, typename T2>
241struct traits<FunctorizedFactor2<R, T1, T2>>
242 : public Testable<FunctorizedFactor2<R, T1, T2>> {};
243
250template <typename T1, typename T2, typename R, typename FUNC>
252 Key key1, Key key2, const R &z, const SharedNoiseModel &model,
253 const FUNC func) {
254 return FunctorizedFactor2<R, T1, T2>(key1, key2, z, model, func);
255}
256
257} // 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:251
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:143
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
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
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
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
FunctorizedFactor()
default constructor - only use for serialization
Definition FunctorizedFactor.h:69
friend class boost::serialization::access
Serialization function.
Definition FunctorizedFactor.h:120
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:159
FunctorizedFactor2()
default constructor - only use for serialization
Definition FunctorizedFactor.h:171
NonlinearFactor::shared_ptr clone() const override
Definition FunctorizedFactor.h:190
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:180
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition FunctorizedFactor.h:206
bool equals(const NonlinearFactor &other, double tol=1e-9) const override
Check if two factors are equal.
Definition FunctorizedFactor.h:218
friend class boost::serialization::access
Serialization function.
Definition FunctorizedFactor.h:228
Nonlinear factor base class.
Definition NonlinearFactor.h:42
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
Key key() const
Returns a key.
Definition NonlinearFactor.h:518