29#include <gtsam/base/utilities.h>
31#include <boost/serialization/base_object.hpp>
52 typedef boost::shared_ptr<This> shared_ptr;
63 template<
typename CONTAINER>
73 DefaultKeyFormatter)
const override;
97 virtual double error(
const Values& c)
const;
106 virtual size_t dim()
const = 0;
121 virtual boost::shared_ptr<GaussianFactor>
132 throw std::runtime_error(
"NonlinearFactor::clone(): Attempting to clone factor with no clone() implemented!");
141 virtual shared_ptr rekey(
const std::map<Key,Key>& rekey_mapping)
const;
147 virtual shared_ptr rekey(
const KeyVector& new_keys)
const;
197 template<
typename CONTAINER>
199 Base(keys), noiseModel_(noiseModel) {}
211 void print(
const std::string& s =
"",
212 const KeyFormatter& keyFormatter = DefaultKeyFormatter)
const override;
218 size_t dim()
const override {
219 return noiseModel_->dim();
234 boost::optional<std::vector<Matrix>&> H = boost::none)
const = 0;
240 Vector whitenedError(
const Values& c)
const;
245 Vector unweightedWhitenedError(
const Values& c)
const;
250 double weight(
const Values& c)
const;
258 double error(
const Values& c)
const override;
265 boost::shared_ptr<GaussianFactor> linearize(
const Values& x)
const override;
275 friend class boost::serialization::access;
276 template<
class ARCHIVE>
277 void serialize(ARCHIVE & ar,
const unsigned int ) {
278 ar & boost::serialization::make_nvp(
"NonlinearFactor",
279 boost::serialization::base_object<Base>(*
this));
280 ar & BOOST_SERIALIZATION_NVP(noiseModel_);
299template <
typename,
typename...>
301template <
typename T1>
306template <
typename T1,
typename T2>
311template <
typename T1,
typename T2,
typename T3>
317template <
typename T1,
typename T2,
typename T3,
typename T4>
324template <
typename T1,
typename T2,
typename T3,
typename T4,
typename T5>
332template <
typename T1,
typename T2,
typename T3,
typename T4,
typename T5,
333 typename T6,
typename... TExtra>
397template <
class... ValueTypes>
403 enum { N =
sizeof...(ValueTypes) };
412 template <
typename From,
typename To>
413 using IsConvertible =
414 typename std::enable_if<std::is_convertible<From, To>::value,
void>::type;
417 using IndexIsValid =
typename std::enable_if<(I >= 1) && (I <= N),
420 template <
typename Container>
421 using ContainerElementType =
422 typename std::decay<decltype(*std::declval<Container>().
begin())>::type;
423 template <
typename Container>
424 using IsContainerOfKeys = IsConvertible<ContainerElementType<Container>,
Key>;
431 template <
typename T>
432 using OptionalMatrix = boost::optional<Matrix&>;
436 template <
typename T>
459 template <
int I,
typename = IndexIsVal
id<I>>
461 typename std::tuple_element<I - 1, std::tuple<ValueTypes...>>::type;
479 KeyType<ValueTypes>...
keys)
489 template <
typename CONTAINER = std::initializer_list<Key>,
490 typename = IsContainerOfKeys<CONTAINER>>
493 if (
keys.size() != N) {
494 throw std::invalid_argument(
495 "NoiseModelFactorN: wrong number of keys given");
519 static_assert(I <= N,
"Index out of bounds");
544 boost::optional<std::vector<Matrix>&> H = boost::none)
const override {
576 OptionalMatrix<ValueTypes>... H)
const = 0;
597 template <
typename... OptionalJacArgs,
598 typename = IndexIsValid<
sizeof...(OptionalJacArgs) + 1>>
600 OptionalJacArgs&&... H)
const {
601 return evaluateError(x..., std::forward<OptionalJacArgs>(H)...,
614 template <std::size_t... Indices>
618 boost::optional<std::vector<Matrix>&> H = boost::none)
const {
627 return Vector::Zero(this->
dim());
633 template <
class ARCHIVE>
634 void serialize(ARCHIVE& ar,
const unsigned int ) {
635 ar& boost::serialization::make_nvp(
636 "NoiseModelFactor", boost::serialization::base_object<Base>(*
this));
643 inline Key key1()
const {
647 inline Key key2()
const {
648 static_assert(I <= N,
"Index out of bounds");
652 inline Key key3()
const {
653 static_assert(I <= N,
"Index out of bounds");
657 inline Key key4()
const {
658 static_assert(I <= N,
"Index out of bounds");
662 inline Key key5()
const {
663 static_assert(I <= N,
"Index out of bounds");
667 inline Key key6()
const {
668 static_assert(I <= N,
"Index out of bounds");
676#define NoiseModelFactor1 NoiseModelFactorN
677#define NoiseModelFactor2 NoiseModelFactorN
678#define NoiseModelFactor3 NoiseModelFactorN
679#define NoiseModelFactor4 NoiseModelFactorN
680#define NoiseModelFactor5 NoiseModelFactorN
681#define NoiseModelFactor6 NoiseModelFactorN
Special class for optional Jacobian arguments.
The base class for all factors.
A non-templated config holding any types of Manifold-group elements.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:156
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
Template to create a binary predicate.
Definition: Testable.h:111
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
Definition: utilities.h:42
Definition: utilities.h:59
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition: HybridValues.h:38
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:140
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:85
const_iterator begin() const
Iterator at beginning of involved variable keys.
Definition: Factor.h:143
Nonlinear factor base class.
Definition: NonlinearFactor.h:42
virtual boost::shared_ptr< GaussianFactor > linearize(const Values &c) const =0
linearize to a GaussianFactor
virtual size_t dim() const =0
get the dimension of the factor (number of rows on linearization)
NonlinearFactor()
Default constructor for I/O only.
Definition: NonlinearFactor.h:58
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition: NonlinearFactor.h:118
NonlinearFactor(const CONTAINER &keys)
Constructor from a collection of the keys involved in this factor.
Definition: NonlinearFactor.h:64
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
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
virtual ~NonlinearFactor()
Destructor.
Definition: NonlinearFactor.h:83
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition: NonlinearFactor.h:174
NoiseModelFactor(const SharedNoiseModel &noiseModel, const CONTAINER &keys)
Constructor.
Definition: NonlinearFactor.h:198
~NoiseModelFactor() override
Destructor.
Definition: NonlinearFactor.h:192
NoiseModelFactor(const SharedNoiseModel &noiseModel)
Constructor - only for subclasses, as this does not set keys.
Definition: NonlinearFactor.h:206
boost::shared_ptr< This > shared_ptr
Noise model.
Definition: NonlinearFactor.h:186
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition: NonlinearFactor.h:218
NoiseModelFactor()
Default constructor for I/O only.
Definition: NonlinearFactor.h:189
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:223
virtual Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const =0
Error function without the NoiseModel, .
Convenience base class to add aliases X1, X2, ..., X6 -> ValueType<N>.
Definition: NonlinearFactor.h:300
Definition: NonlinearFactor.h:302
Definition: NonlinearFactor.h:307
Definition: NonlinearFactor.h:312
Definition: NonlinearFactor.h:318
Definition: NonlinearFactor.h:325
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition: NonlinearFactor.h:400
Vector evaluateError(const ValueTypes &... x) const
No-Jacobians requested function overload.
Definition: NonlinearFactor.h:589
NoiseModelFactorN(const SharedNoiseModel &noiseModel, CONTAINER keys)
Constructor.
Definition: NonlinearFactor.h:491
NoiseModelFactorN(const SharedNoiseModel &noiseModel, KeyType< ValueTypes >... keys)
Constructor.
Definition: NonlinearFactor.h:478
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
NoiseModelFactorN()
Default Constructor for I/O.
Definition: NonlinearFactor.h:469
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrix< ValueTypes >... H) const =0
Override evaluateError to finish implementing an n-way factor.
typename std::tuple_element< I - 1, std::tuple< ValueTypes... > >::type ValueType
The type of the I'th template param can be obtained as ValueType.
Definition: NonlinearFactor.h:461
friend class boost::serialization::access
Serialization function.
Definition: NonlinearFactor.h:632
Key key() const
Returns a key.
Definition: NonlinearFactor.h:518
Vector evaluateError(const ValueTypes &... x, OptionalJacArgs &&... H) const
Some (but not all) optional Jacobians are omitted (function overload)
Definition: NonlinearFactor.h:599
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:65
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:361