gtsam  4.1.0
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 
25 namespace gtsam {
26 
58 template <typename R, typename T>
59 class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
60  private:
61  using Base = NoiseModelFactor1<T>;
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  virtual ~FunctorizedFactor() {}
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,
91  boost::optional<Matrix &> H = 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(const std::string &s = "",
100  const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
101  Base::print(s, keyFormatter);
102  std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
103  << keyFormatter(this->key()) << ")" << std::endl;
104  traits<R>::Print(measured_, " measurement: ");
105  std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
106  << std::endl;
107  }
108 
109  bool equals(const NonlinearFactor &other, double tol = 1e-9) const override {
110  const FunctorizedFactor<R, T> *e =
111  dynamic_cast<const FunctorizedFactor<R, T> *>(&other);
112  return e && Base::equals(other, tol) &&
113  traits<R>::Equals(this->measured_, e->measured_, tol);
114  }
116 
117  private:
119  friend class boost::serialization::access;
120  template <class ARCHIVE>
121  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
122  ar &boost::serialization::make_nvp(
123  "NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
124  ar &BOOST_SERIALIZATION_NVP(measured_);
125  ar &BOOST_SERIALIZATION_NVP(func_);
126  }
127 };
128 
130 template <typename R, typename T>
132  : public Testable<FunctorizedFactor<R, T>> {};
133 
140 template <typename T, typename R, typename FUNC>
142  const SharedNoiseModel &model,
143  const FUNC func) {
144  return FunctorizedFactor<R, T>(key, z, model, func);
145 }
146 
147 } // namespace gtsam
Testable.h
Concept check for values that can be used in unit tests.
gtsam::FunctorizedFactor::equals
bool equals(const NonlinearFactor &other, double tol=1e-9) const override
Check if two factors are equal.
Definition: FunctorizedFactor.h:109
gtsam::FunctorizedFactor::evaluateError
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
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::NoiseModelFactor1
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:271
gtsam::FunctorizedFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: FunctorizedFactor.h:99
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::FunctorizedFactor::clone
NonlinearFactor::shared_ptr clone() const override
Definition: FunctorizedFactor.h:85
gtsam::FunctorizedFactor::FunctorizedFactor
FunctorizedFactor()
default constructor - only use for serialization
Definition: FunctorizedFactor.h:69
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::MakeFunctorizedFactor
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:141
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::FunctorizedFactor
Factor which evaluates provided unary functor and uses the result to compute error with respect to th...
Definition: FunctorizedFactor.h:59
gtsam::KeyFormatter
boost::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::FunctorizedFactor
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