Loading...
Searching...
No Matches
FunctorizedFactor.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/base/Testable.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <cmath>
namespace gtsam {
template <typename R, typename T>
class FunctorizedFactor : public NoiseModelFactorN<T> {
private:
using Base = NoiseModelFactorN<T>;
R measured_;
SharedNoiseModel noiseModel_;
std::function<R(T, boost::optional<Matrix &>)> func_;
public:
FunctorizedFactor() {}
FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model,
const std::function<R(T, boost::optional<Matrix &>)> func)
: Base(model, key), measured_(z), noiseModel_(model), func_(func) {}
~FunctorizedFactor() override {}
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new FunctorizedFactor<R, T>(*this)));
}
Vector evaluateError(const T &params, boost::optional<Matrix &> H =
boost::none) const override {
R x = func_(params, H);
Vector error = traits<R>::Local(measured_, x);
return error;
}
void print(
const std::string &s = "",
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
Base::print(s, keyFormatter);
std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
<< keyFormatter(this->key1()) << ")" << std::endl;
traits<R>::Print(measured_, " measurement: ");
std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
<< std::endl;
}
bool equals(const NonlinearFactor &other, double tol = 1e-9) const override {
const FunctorizedFactor<R, T> *e =
dynamic_cast<const FunctorizedFactor<R, T> *>(&other);
return e != nullptr && Base::equals(other, tol) &&
traits<R>::Equals(this->measured_, e->measured_, tol);
}
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar &boost::serialization::make_nvp(
"NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
ar &BOOST_SERIALIZATION_NVP(measured_);
ar &BOOST_SERIALIZATION_NVP(func_);
}
};
template <typename R, typename T>
struct traits<FunctorizedFactor<R, T>>
: public Testable<FunctorizedFactor<R, T>> {};
template <typename T, typename R, typename FUNC>
FunctorizedFactor<R, T> MakeFunctorizedFactor(Key key, const R &z,
const SharedNoiseModel &model,
const FUNC func) {
return FunctorizedFactor<R, T>(key, z, model, func);
}
template <typename R, typename T1, typename T2>
class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
private:
using Base = NoiseModelFactorN<T1, T2>;
R measured_;
SharedNoiseModel noiseModel_;
using FunctionType = std::function<R(T1, T2, boost::optional<Matrix &>,
boost::optional<Matrix &>)>;
FunctionType func_;
public:
FunctorizedFactor2() {}
FunctorizedFactor2(Key key1, Key key2, const R &z,
const SharedNoiseModel &model, const FunctionType func)
: Base(model, key1, key2),
measured_(z),
noiseModel_(model),
func_(func) {}
~FunctorizedFactor2() override {}
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new FunctorizedFactor2<R, T1, T2>(*this)));
}
Vector evaluateError(
const T1 &params1, const T2 &params2,
boost::optional<Matrix &> H1 = boost::none,
boost::optional<Matrix &> H2 = boost::none) const override {
R x = func_(params1, params2, H1, H2);
Vector error = traits<R>::Local(measured_, x);
return error;
}
void print(
const std::string &s = "",
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
Base::print(s, keyFormatter);
std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor2("
<< keyFormatter(this->key1()) << ", "
<< keyFormatter(this->key2()) << ")" << std::endl;
traits<R>::Print(measured_, " measurement: ");
std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
<< std::endl;
}
bool equals(const NonlinearFactor &other, double tol = 1e-9) const override {
const FunctorizedFactor2<R, T1, T2> *e =
dynamic_cast<const FunctorizedFactor2<R, T1, T2> *>(&other);
return e && Base::equals(other, tol) &&
traits<R>::Equals(this->measured_, e->measured_, tol);
}
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar &boost::serialization::make_nvp(
"NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
ar &BOOST_SERIALIZATION_NVP(measured_);
ar &BOOST_SERIALIZATION_NVP(func_);
}
};
template <typename R, typename T1, typename T2>
struct traits<FunctorizedFactor2<R, T1, T2>>
: public Testable<FunctorizedFactor2<R, T1, T2>> {};
template <typename T1, typename T2, typename R, typename FUNC>
FunctorizedFactor2<R, T1, T2> MakeFunctorizedFactor2(
Key key1, Key key2, const R &z, const SharedNoiseModel &model,
const FUNC func) {
return FunctorizedFactor2<R, T1, T2>(key1, key2, z, model, func);
}
} // namespace gtsam
Testable.h
Concept check for values that can be used in unit tests.
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::MakeFunctorizedFactor2
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
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::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:143
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
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::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
gtsam::FunctorizedFactor
Factor which evaluates provided unary functor and uses the result to compute error with respect to th...
Definition FunctorizedFactor.h:59
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
gtsam::FunctorizedFactor::equals
bool equals(const NonlinearFactor &other, double tol=1e-9) const override
Check if two factors are equal.
Definition FunctorizedFactor.h:110
gtsam::FunctorizedFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition FunctorizedFactor.h:99
gtsam::FunctorizedFactor::FunctorizedFactor
FunctorizedFactor()
default constructor - only use for serialization
Definition FunctorizedFactor.h:69
gtsam::FunctorizedFactor::clone
NonlinearFactor::shared_ptr clone() const override
Definition FunctorizedFactor.h:85
gtsam::FunctorizedFactor2
Factor which evaluates provided binary functor and uses the result to compute error with respect to t...
Definition FunctorizedFactor.h:159
gtsam::FunctorizedFactor2::FunctorizedFactor2
FunctorizedFactor2()
default constructor - only use for serialization
Definition FunctorizedFactor.h:171
gtsam::FunctorizedFactor2::clone
NonlinearFactor::shared_ptr clone() const override
Definition FunctorizedFactor.h:190
gtsam::FunctorizedFactor2::FunctorizedFactor2
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
gtsam::FunctorizedFactor2::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition FunctorizedFactor.h:206
gtsam::FunctorizedFactor2::equals
bool equals(const NonlinearFactor &other, double tol=1e-9) const override
Check if two factors are equal.
Definition FunctorizedFactor.h:218
gtsam::FunctorizedFactor2::access
friend class boost::serialization::access
Serialization function.
Definition FunctorizedFactor.h:228
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
gtsam::NoiseModelFactorN< T >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
gtsam::NoiseModelFactorN::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518