58template <
typename R,
typename T>
65 std::function<R(T, boost::optional<Matrix &>)> func_;
79 const std::function<R(T, boost::optional<Matrix &>)> func)
80 :
Base(model,
key), measured_(z), noiseModel_(model), func_(func) {}
85 NonlinearFactor::shared_ptr
clone()
const override {
86 return boost::static_pointer_cast<NonlinearFactor>(
90 Vector evaluateError(
const T ¶ms, boost::optional<Matrix &> H =
91 boost::none)
const override {
92 R x = func_(params, H);
100 const std::string &s =
"",
101 const KeyFormatter &keyFormatter = DefaultKeyFormatter)
const override {
103 std::cout << s << (s !=
"" ?
" " :
"") <<
"FunctorizedFactor("
104 << keyFormatter(this->key1()) <<
")" << std::endl;
106 std::cout <<
" noise model sigmas: " << noiseModel_->sigmas().transpose()
121 template <
class ARCHIVE>
122 void serialize(ARCHIVE &ar,
const unsigned int ) {
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_);
132template <
typename R,
typename T>
134 :
public Testable<FunctorizedFactor<R, T>> {};
142template <
typename T,
typename R,
typename FUNC>
158template <
typename R,
typename T1,
typename T2>
165 using FunctionType = std::function<R(T1, T2, boost::optional<Matrix &>,
166 boost::optional<Matrix &>)>;
182 :
Base(model, key1, key2),
190 NonlinearFactor::shared_ptr
clone()
const override {
191 return boost::static_pointer_cast<NonlinearFactor>(
195 Vector evaluateError(
196 const T1 ¶ms1,
const T2 ¶ms2,
197 boost::optional<Matrix &> H1 = boost::none,
198 boost::optional<Matrix &> H2 = boost::none)
const override {
199 R x = func_(params1, params2, H1, H2);
207 const std::string &s =
"",
208 const KeyFormatter &keyFormatter = DefaultKeyFormatter)
const override {
210 std::cout << s << (s !=
"" ?
" " :
"") <<
"FunctorizedFactor2("
211 << keyFormatter(this->key1()) <<
", "
212 << keyFormatter(this->key2()) <<
")" << std::endl;
214 std::cout <<
" noise model sigmas: " << noiseModel_->sigmas().transpose()
229 template <
class ARCHIVE>
230 void serialize(ARCHIVE &ar,
const unsigned int ) {
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_);
240template <
typename R,
typename T1,
typename T2>
242 :
public Testable<FunctorizedFactor2<R, T1, T2>> {};
250template <
typename T1,
typename T2,
typename R,
typename FUNC>
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