Loading...
Searching...
No Matches
NonlinearFactor.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
* -------------------------------------------------------------------------- */
// \callgraph
#pragma once
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/inference/Factor.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/utilities.h> // boost::index_sequence
#include <boost/serialization/base_object.hpp>
namespace gtsam {
/* ************************************************************************* */
class GTSAM_EXPORT NonlinearFactor: public Factor {
protected:
// Some handy typedefs
typedef Factor Base;
typedef NonlinearFactor This;
public:
typedef boost::shared_ptr<This> shared_ptr;
NonlinearFactor() {}
template<typename CONTAINER>
NonlinearFactor(const CONTAINER& keys) :
Base(keys) {}
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
virtual ~NonlinearFactor() {}
virtual double error(const Values& c) const;
double error(const HybridValues& c) const override;
virtual size_t dim() const = 0;
virtual bool active(const Values& /*c*/) const { return true; }
virtual boost::shared_ptr<GaussianFactor>
linearize(const Values& c) const = 0;
virtual shared_ptr clone() const {
// TODO: choose better exception to throw here
throw std::runtime_error("NonlinearFactor::clone(): Attempting to clone factor with no clone() implemented!");
return shared_ptr();
}
virtual shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const;
virtual shared_ptr rekey(const KeyVector& new_keys) const;
virtual bool sendable() const {
return true;
}
}; // \class NonlinearFactor
template<> struct traits<NonlinearFactor> : public Testable<NonlinearFactor> {
};
/* ************************************************************************* */
class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor {
protected:
// handy typedefs
typedef NonlinearFactor Base;
typedef NoiseModelFactor This;
SharedNoiseModel noiseModel_;
public:
typedef boost::shared_ptr<This> shared_ptr;
NoiseModelFactor() {}
~NoiseModelFactor() override {}
template<typename CONTAINER>
NoiseModelFactor(const SharedNoiseModel& noiseModel, const CONTAINER& keys) :
Base(keys), noiseModel_(noiseModel) {}
protected:
NoiseModelFactor(const SharedNoiseModel& noiseModel) : noiseModel_(noiseModel) {}
public:
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
bool equals(const NonlinearFactor& f, double tol = 1e-9) const override;
size_t dim() const override {
return noiseModel_->dim();
}
const SharedNoiseModel& noiseModel() const {
return noiseModel_;
}
virtual Vector unwhitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
Vector whitenedError(const Values& c) const;
Vector unweightedWhitenedError(const Values& c) const;
double weight(const Values& c) const;
double error(const Values& c) const override;
boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override;
shared_ptr cloneWithNewNoiseModel(const SharedNoiseModel newNoise) const;
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(noiseModel_);
}
}; // \class NoiseModelFactor
/* ************************************************************************* */
namespace detail {
template <typename, typename...>
struct NoiseModelFactorAliases {};
template <typename T1>
struct NoiseModelFactorAliases<T1> {
using X = T1;
using X1 = T1;
};
template <typename T1, typename T2>
struct NoiseModelFactorAliases<T1, T2> {
using X1 = T1;
using X2 = T2;
};
template <typename T1, typename T2, typename T3>
struct NoiseModelFactorAliases<T1, T2, T3> {
using X1 = T1;
using X2 = T2;
using X3 = T3;
};
template <typename T1, typename T2, typename T3, typename T4>
struct NoiseModelFactorAliases<T1, T2, T3, T4> {
using X1 = T1;
using X2 = T2;
using X3 = T3;
using X4 = T4;
};
template <typename T1, typename T2, typename T3, typename T4, typename T5>
struct NoiseModelFactorAliases<T1, T2, T3, T4, T5> {
using X1 = T1;
using X2 = T2;
using X3 = T3;
using X4 = T4;
using X5 = T5;
};
template <typename T1, typename T2, typename T3, typename T4, typename T5,
typename T6, typename... TExtra>
struct NoiseModelFactorAliases<T1, T2, T3, T4, T5, T6, TExtra...> {
using X1 = T1;
using X2 = T2;
using X3 = T3;
using X4 = T4;
using X5 = T5;
using X6 = T6;
};
} // namespace detail
/* ************************************************************************* */
template <class... ValueTypes>
class NoiseModelFactorN
: public NoiseModelFactor,
public detail::NoiseModelFactorAliases<ValueTypes...> {
public:
enum { N = sizeof...(ValueTypes) };
protected:
using Base = NoiseModelFactor;
using This = NoiseModelFactorN<ValueTypes...>;
template <typename From, typename To>
using IsConvertible =
typename std::enable_if<std::is_convertible<From, To>::value, void>::type;
template <int I>
using IndexIsValid = typename std::enable_if<(I >= 1) && (I <= N),
void>::type; // 1-indexed!
template <typename Container>
using ContainerElementType =
typename std::decay<decltype(*std::declval<Container>().begin())>::type;
template <typename Container>
using IsContainerOfKeys = IsConvertible<ContainerElementType<Container>, Key>;
/* Like std::void_t, except produces `boost::optional<Matrix&>` instead of
* `void`. Used to expand fixed-type parameter-packs with same length as
* ValueTypes. */
template <typename T>
using OptionalMatrix = boost::optional<Matrix&>;
/* Like std::void_t, except produces `Key` instead of `void`. Used to expand
* fixed-type parameter-packs with same length as ValueTypes. */
template <typename T>
using KeyType = Key;
public:
template <int I, typename = IndexIsValid<I>>
using ValueType =
typename std::tuple_element<I - 1, std::tuple<ValueTypes...>>::type;
public:
NoiseModelFactorN() {}
NoiseModelFactorN(const SharedNoiseModel& noiseModel,
KeyType<ValueTypes>... keys)
: Base(noiseModel, std::array<Key, N>{keys...}) {}
template <typename CONTAINER = std::initializer_list<Key>,
typename = IsContainerOfKeys<CONTAINER>>
NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys)
: Base(noiseModel, keys) {
if (keys.size() != N) {
throw std::invalid_argument(
"NoiseModelFactorN: wrong number of keys given");
}
}
~NoiseModelFactorN() override {}
template <int I = 1>
inline Key key() const {
static_assert(I <= N, "Index out of bounds");
return keys_[I - 1];
}
Vector unwhitenedError(
const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const override {
return unwhitenedError(boost::mp11::index_sequence_for<ValueTypes...>{}, x,
H);
}
virtual Vector evaluateError(const ValueTypes&... x,
OptionalMatrix<ValueTypes>... H) const = 0;
inline Vector evaluateError(const ValueTypes&... x) const {
return evaluateError(x..., OptionalMatrix<ValueTypes>()...);
}
template <typename... OptionalJacArgs,
typename = IndexIsValid<sizeof...(OptionalJacArgs) + 1>>
inline Vector evaluateError(const ValueTypes&... x,
OptionalJacArgs&&... H) const {
return evaluateError(x..., std::forward<OptionalJacArgs>(H)...,
boost::none);
}
private:
template <std::size_t... Indices>
inline Vector unwhitenedError(
boost::mp11::index_sequence<Indices...>, //
const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const {
if (this->active(x)) {
if (H) {
return evaluateError(x.at<ValueTypes>(keys_[Indices])...,
(*H)[Indices]...);
} else {
return evaluateError(x.at<ValueTypes>(keys_[Indices])...);
}
} else {
return Vector::Zero(this->dim());
}
}
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"NoiseModelFactor", boost::serialization::base_object<Base>(*this));
}
public:
inline Key key1() const {
return key<1>();
}
template <int I = 2>
inline Key key2() const {
static_assert(I <= N, "Index out of bounds");
return key<2>();
}
template <int I = 3>
inline Key key3() const {
static_assert(I <= N, "Index out of bounds");
return key<3>();
}
template <int I = 4>
inline Key key4() const {
static_assert(I <= N, "Index out of bounds");
return key<4>();
}
template <int I = 5>
inline Key key5() const {
static_assert(I <= N, "Index out of bounds");
return key<5>();
}
template <int I = 6>
inline Key key6() const {
static_assert(I <= N, "Index out of bounds");
return key<6>();
}
}; // \class NoiseModelFactorN
#define NoiseModelFactor1 NoiseModelFactorN
#define NoiseModelFactor2 NoiseModelFactorN
#define NoiseModelFactor3 NoiseModelFactorN
#define NoiseModelFactor4 NoiseModelFactorN
#define NoiseModelFactor5 NoiseModelFactorN
#define NoiseModelFactor6 NoiseModelFactorN
} // namespace gtsam
OptionalJacobian.h
Special class for optional Jacobian arguments.
Factor.h
The base class for all factors.
NoiseModel.h
JacobianFactor.h
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
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::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::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
boost::mp11::index_sequence
Definition utilities.h:42
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
gtsam::Factor::Factor
Factor()
Default constructor for I/O.
Definition Factor.h:91
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
gtsam::NonlinearFactor::linearize
virtual boost::shared_ptr< GaussianFactor > linearize(const Values &c) const =0
linearize to a GaussianFactor
gtsam::NonlinearFactor::dim
virtual size_t dim() const =0
get the dimension of the factor (number of rows on linearization)
gtsam::NonlinearFactor::NonlinearFactor
NonlinearFactor()
Default constructor for I/O only.
Definition NonlinearFactor.h:58
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition NonlinearFactor.h:118
gtsam::NonlinearFactor::NonlinearFactor
NonlinearFactor(const CONTAINER &keys)
Constructor from a collection of the keys involved in this factor.
Definition NonlinearFactor.h:64
gtsam::NonlinearFactor::sendable
virtual bool sendable() const
Should the factor be evaluated in the same thread as the caller This is to enable factors that has sh...
Definition NonlinearFactor.h:153
gtsam::NonlinearFactor::clone
virtual shared_ptr clone() const
Creates a shared_ptr clone of the factor - needs to be specialized to allow for subclasses.
Definition NonlinearFactor.h:130
gtsam::NonlinearFactor::~NonlinearFactor
virtual ~NonlinearFactor()
Destructor.
Definition NonlinearFactor.h:83
gtsam::NoiseModelFactor::NoiseModelFactor
NoiseModelFactor(const SharedNoiseModel &noiseModel, const CONTAINER &keys)
Constructor.
Definition NonlinearFactor.h:198
gtsam::NoiseModelFactor::~NoiseModelFactor
~NoiseModelFactor() override
Destructor.
Definition NonlinearFactor.h:192
gtsam::NoiseModelFactor::NoiseModelFactor
NoiseModelFactor(const SharedNoiseModel &noiseModel)
Constructor - only for subclasses, as this does not set keys.
Definition NonlinearFactor.h:206
gtsam::NoiseModelFactor::whitenedError
Vector whitenedError(const Values &c) const
Vector of errors, whitened This is the raw error, i.e., i.e.
Definition NonlinearFactor.cpp:109
gtsam::NoiseModelFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Linearize a non-linearFactorN to get a GaussianFactor, Hence .
Definition NonlinearFactor.cpp:152
gtsam::NoiseModelFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
Noise model.
Definition NonlinearFactor.h:186
gtsam::NoiseModelFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition NonlinearFactor.h:218
gtsam::NoiseModelFactor::unweightedWhitenedError
Vector unweightedWhitenedError(const Values &c) const
Vector of errors, whitened, but unweighted by any loss function.
Definition NonlinearFactor.cpp:116
gtsam::NoiseModelFactor::cloneWithNewNoiseModel
shared_ptr cloneWithNewNoiseModel(const SharedNoiseModel newNoise) const
Creates a shared_ptr clone of the factor with a new noise model.
Definition NonlinearFactor.cpp:91
gtsam::NoiseModelFactor::weight
double weight(const Values &c) const
Compute the effective weight of the factor from the noise model.
Definition NonlinearFactor.cpp:123
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
gtsam::NoiseModelFactor::NoiseModelFactor
NoiseModelFactor()
Default constructor for I/O only.
Definition NonlinearFactor.h:189
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition NonlinearFactor.h:223
gtsam::NoiseModelFactor::unwhitenedError
virtual Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const =0
Error function without the NoiseModel, .
gtsam::detail::NoiseModelFactorAliases
Convenience base class to add aliases X1, X2, ..., X6 -> ValueType<N>.
Definition NonlinearFactor.h:300
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
gtsam::NoiseModelFactorN::evaluateError
Vector evaluateError(const ValueTypes &... x) const
No-Jacobians requested function overload.
Definition NonlinearFactor.h:589
gtsam::NoiseModelFactorN::NoiseModelFactorN
NoiseModelFactorN(const SharedNoiseModel &noiseModel, CONTAINER keys)
Constructor.
Definition NonlinearFactor.h:491
gtsam::NoiseModelFactorN::NoiseModelFactorN
NoiseModelFactorN(const SharedNoiseModel &noiseModel, KeyType< ValueTypes >... keys)
Constructor.
Definition NonlinearFactor.h:478
gtsam::NoiseModelFactorN::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
This implements the unwhitenedError virtual function by calling the n-key specific version of evaluat...
Definition NonlinearFactor.h:542
gtsam::NoiseModelFactorN::NoiseModelFactorN
NoiseModelFactorN()
Default Constructor for I/O.
Definition NonlinearFactor.h:469
gtsam::NoiseModelFactorN::evaluateError
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrix< ValueTypes >... H) const =0
Override evaluateError to finish implementing an n-way factor.
gtsam::NoiseModelFactorN::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
gtsam::NoiseModelFactorN::evaluateError
Vector evaluateError(const ValueTypes &... x, OptionalJacArgs &&... H) const
Some (but not all) optional Jacobians are omitted (function overload).
Definition NonlinearFactor.h:599
gtsam::NoiseModelFactorN< Rot3, Rot3, Vector3 >::ValueType
typename std::tuple_element< I - 1, std::tuple< ValueTypes... > >::type ValueType
Definition NonlinearFactor.h:460
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
HybridValues
the error.
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...