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 
25 namespace gtsam {
26 
58 template <typename R, typename T>
60  private:
62 
65  std::function<R(T, OptionalMatrixType)> func_;
66 
67  public:
68 
69  // Provide access to the Matrix& version of evaluateError:
70  using Base::evaluateError;
71 
74 
83  const std::function<R(T, OptionalMatrixType)> func)
85 
86  ~FunctorizedFactor() override {}
87 
90  return std::static_pointer_cast<NonlinearFactor>(
92  }
93 
94  Vector evaluateError(const T &params, OptionalMatrixType H) const override {
95  R x = func_(params, H);
97  return error;
98  }
99 
102  void print(
103  const std::string &s = "",
104  const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
105  Base::print(s, keyFormatter);
106  std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
107  << keyFormatter(this->key1()) << ")" << std::endl;
108  traits<R>::Print(measured_, " measurement: ");
109  std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
110  << std::endl;
111  }
112 
113  bool equals(const NonlinearFactor &other, double tol = 1e-9) const override {
114  const FunctorizedFactor<R, T> *e =
115  dynamic_cast<const FunctorizedFactor<R, T> *>(&other);
116  return e != nullptr && Base::equals(other, tol) &&
117  traits<R>::Equals(this->measured_, e->measured_, tol);
118  }
120 
121  private:
122 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
123 
124  friend class boost::serialization::access;
125  template <class ARCHIVE>
126  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
127  // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
128  ar &boost::serialization::make_nvp(
129  "NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
130  ar &BOOST_SERIALIZATION_NVP(measured_);
131  ar &BOOST_SERIALIZATION_NVP(func_);
132  }
133 #endif
134 };
135 
137 template <typename R, typename T>
139  : public Testable<FunctorizedFactor<R, T>> {};
140 
147 template <typename T, typename R, typename FUNC>
149  const SharedNoiseModel &model,
150  const FUNC func) {
152 }
153 
163 template <typename R, typename T1, typename T2>
164 class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
165  private:
167 
172 
173  public:
174 
175  // Provide access to the Matrix& version of evaluateError:
176  using Base::evaluateError;
177 
180 
189  const SharedNoiseModel &model, const FunctionType func)
190  : Base(model, key1, key2),
191  measured_(z),
193  func_(func) {}
194 
195  ~FunctorizedFactor2() override {}
196 
199  return std::static_pointer_cast<NonlinearFactor>(
201  }
202 
204  const T1 &params1, const T2 &params2,
205  OptionalMatrixType H1, OptionalMatrixType H2) const override {
206  R x = func_(params1, params2, H1, H2);
208  return error;
209  }
210 
213  void print(
214  const std::string &s = "",
215  const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
216  Base::print(s, keyFormatter);
217  std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor2("
218  << keyFormatter(this->key1()) << ", "
219  << keyFormatter(this->key2()) << ")" << std::endl;
220  traits<R>::Print(measured_, " measurement: ");
221  std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
222  << std::endl;
223  }
224 
225  bool equals(const NonlinearFactor &other, double tol = 1e-9) const override {
227  dynamic_cast<const FunctorizedFactor2<R, T1, T2> *>(&other);
228  return e && Base::equals(other, tol) &&
229  traits<R>::Equals(this->measured_, e->measured_, tol);
230  }
232 
233  private:
234 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
235 
236  friend class boost::serialization::access;
237  template <class ARCHIVE>
238  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
239  // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
240  ar &boost::serialization::make_nvp(
241  "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
242  ar &BOOST_SERIALIZATION_NVP(measured_);
243  ar &BOOST_SERIALIZATION_NVP(func_);
244  }
245 #endif
246 };
247 
249 template <typename R, typename T1, typename T2>
251  : public Testable<FunctorizedFactor2<R, T1, T2>> {};
252 
259 template <typename T1, typename T2, typename R, typename FUNC>
261  Key key1, Key key2, const R &z, const SharedNoiseModel &model,
262  const FUNC func) {
264 }
265 
266 } // namespace gtsam
key1
const Symbol key1('v', 1)
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
Eigen
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
gtsam::FunctorizedFactor2::equals
bool equals(const NonlinearFactor &other, double tol=1e-9) const override
Definition: FunctorizedFactor.h:225
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Definition: NonlinearFactor.cpp:136
Testable.h
Concept check for values that can be used in unit tests.
gtsam::FunctorizedFactor2::FunctorizedFactor2
FunctorizedFactor2(Key key1, Key key2, const R &z, const SharedNoiseModel &model, const FunctionType func)
Definition: FunctorizedFactor.h:188
gtsam::FunctorizedFactor2::FunctionType
std::function< R(T1, T2, OptionalMatrixType, OptionalMatrixType)> FunctionType
Definition: FunctorizedFactor.h:170
gtsam::NoiseModelFactorN< T >::evaluateError
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::MakeFunctorizedFactor
FunctorizedFactor< R, T > MakeFunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model, const FUNC func)
Definition: FunctorizedFactor.h:148
gtsam::FunctorizedFactor::equals
bool equals(const NonlinearFactor &other, double tol=1e-9) const override
Definition: FunctorizedFactor.h:113
gtsam::FunctorizedFactor::~FunctorizedFactor
~FunctorizedFactor() override
Definition: FunctorizedFactor.h:86
gtsam::Factor
Definition: Factor.h:70
gtsam::FunctorizedFactor::noiseModel_
SharedNoiseModel noiseModel_
noise model
Definition: FunctorizedFactor.h:64
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::FunctorizedFactor::FunctorizedFactor
FunctorizedFactor()
Definition: FunctorizedFactor.h:73
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::FunctorizedFactor2::func_
FunctionType func_
functor instance
Definition: FunctorizedFactor.h:171
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::FunctorizedFactor::evaluateError
Vector evaluateError(const T &params, OptionalMatrixType H) const override
Definition: FunctorizedFactor.h:94
gtsam::MakeFunctorizedFactor2
FunctorizedFactor2< R, T1, T2 > MakeFunctorizedFactor2(Key key1, Key key2, const R &z, const SharedNoiseModel &model, const FUNC func)
Definition: FunctorizedFactor.h:260
gtsam::FunctorizedFactor2::~FunctorizedFactor2
~FunctorizedFactor2() override
Definition: FunctorizedFactor.h:195
gtsam::FunctorizedFactor2::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: FunctorizedFactor.h:213
key2
const Symbol key2('v', 2)
gtsam::FunctorizedFactor2::noiseModel_
SharedNoiseModel noiseModel_
noise model
Definition: FunctorizedFactor.h:169
gtsam::KeyFormatter
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
gtsam::FunctorizedFactor::measured_
R measured_
value that is compared with functor return value
Definition: FunctorizedFactor.h:63
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:431
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactor.cpp:72
gtsam::FunctorizedFactor::FunctorizedFactor
FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model, const std::function< R(T, OptionalMatrixType)> func)
Definition: FunctorizedFactor.h:82
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: NonlinearFactor.cpp:80
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
T2
static const Pose3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2)
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
Eigen::Triplet< double >
gtsam::FunctorizedFactor::func_
std::function< R(T, OptionalMatrixType)> func_
functor instance
Definition: FunctorizedFactor.h:65
gtsam::NoiseModelFactorN< T >::key1
Key key1() const
Definition: NonlinearFactor.h:731
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::FunctorizedFactor2::FunctorizedFactor2
FunctorizedFactor2()
Definition: FunctorizedFactor.h:179
key
const gtsam::Symbol key('X', 0)
NonlinearFactor.h
Non-linear factor base classes.
gtsam::NoiseModelFactorN< T1, T2 >::key2
Key key2() const
Definition: NonlinearFactor.h:735
gtsam::FunctorizedFactor2
Definition: FunctorizedFactor.h:164
gtsam::FunctorizedFactor
Definition: FunctorizedFactor.h:59
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
gtsam::traits
Definition: Group.h:36
gtsam::FunctorizedFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: FunctorizedFactor.h:102
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
gtsam::Print
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:65
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
gtsam::NoiseModelFactorN< T >::key
Key key() const
Definition: NonlinearFactor.h:582
T1
static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1)
gtsam::FunctorizedFactor::clone
NonlinearFactor::shared_ptr clone() const override
Definition: FunctorizedFactor.h:89
func
Definition: benchGeometry.cpp:23
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::FunctorizedFactor2::clone
NonlinearFactor::shared_ptr clone() const override
Definition: FunctorizedFactor.h:198
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::FunctorizedFactor2::measured_
R measured_
value that is compared with functor return value
Definition: FunctorizedFactor.h:168
gtsam::FunctorizedFactor2::evaluateError
Vector evaluateError(const T1 &params1, const T2 &params2, OptionalMatrixType H1, OptionalMatrixType H2) const override
Definition: FunctorizedFactor.h:203


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:33