gtsam 4.2.0
gtsam
NonlinearFactor.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
20// \callgraph
21
22#pragma once
23
29#include <gtsam/base/utilities.h> // boost::index_sequence
30
31#include <boost/serialization/base_object.hpp>
32
33namespace gtsam {
34
35/* ************************************************************************* */
36
42class GTSAM_EXPORT NonlinearFactor: public Factor {
43
44protected:
45
46 // Some handy typedefs
47 typedef Factor Base;
48 typedef NonlinearFactor This;
49
50public:
51
52 typedef boost::shared_ptr<This> shared_ptr;
53
56
59
63 template<typename CONTAINER>
64 NonlinearFactor(const CONTAINER& keys) :
65 Base(keys) {}
66
70
72 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
73 DefaultKeyFormatter) const override;
74
76 virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
77
81
83 virtual ~NonlinearFactor() {}
84
97 virtual double error(const Values& c) const;
98
103 double error(const HybridValues& c) const override;
104
106 virtual size_t dim() const = 0;
107
118 virtual bool active(const Values& /*c*/) const { return true; }
119
121 virtual boost::shared_ptr<GaussianFactor>
122 linearize(const Values& c) const = 0;
123
130 virtual shared_ptr clone() const {
131 // TODO: choose better exception to throw here
132 throw std::runtime_error("NonlinearFactor::clone(): Attempting to clone factor with no clone() implemented!");
133 return shared_ptr();
134 }
135
141 virtual shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const;
142
147 virtual shared_ptr rekey(const KeyVector& new_keys) const;
148
153 virtual bool sendable() const {
154 return true;
155 }
156
157}; // \class NonlinearFactor
158
160template<> struct traits<NonlinearFactor> : public Testable<NonlinearFactor> {
161};
162
163/* ************************************************************************* */
174class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor {
175
176protected:
177
178 // handy typedefs
179 typedef NonlinearFactor Base;
180 typedef NoiseModelFactor This;
181
182 SharedNoiseModel noiseModel_;
184public:
185
186 typedef boost::shared_ptr<This> shared_ptr;
187
190
192 ~NoiseModelFactor() override {}
193
197 template<typename CONTAINER>
198 NoiseModelFactor(const SharedNoiseModel& noiseModel, const CONTAINER& keys) :
199 Base(keys), noiseModel_(noiseModel) {}
200
201protected:
202
206 NoiseModelFactor(const SharedNoiseModel& noiseModel) : noiseModel_(noiseModel) {}
207
208public:
209
211 void print(const std::string& s = "",
212 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
213
215 bool equals(const NonlinearFactor& f, double tol = 1e-9) const override;
216
218 size_t dim() const override {
219 return noiseModel_->dim();
220 }
221
224 return noiseModel_;
225 }
226
233 virtual Vector unwhitenedError(const Values& x,
234 boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
235
240 Vector whitenedError(const Values& c) const;
241
245 Vector unweightedWhitenedError(const Values& c) const;
246
250 double weight(const Values& c) const;
251
258 double error(const Values& c) const override;
259
265 boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override;
266
271 shared_ptr cloneWithNewNoiseModel(const SharedNoiseModel newNoise) const;
272
273 private:
275 friend class boost::serialization::access;
276 template<class ARCHIVE>
277 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
278 ar & boost::serialization::make_nvp("NonlinearFactor",
279 boost::serialization::base_object<Base>(*this));
280 ar & BOOST_SERIALIZATION_NVP(noiseModel_);
281 }
282
283}; // \class NoiseModelFactor
284
285/* ************************************************************************* */
286namespace detail {
299template <typename, typename...>
301template <typename T1>
303 using X = T1;
304 using X1 = T1;
305};
306template <typename T1, typename T2>
308 using X1 = T1;
309 using X2 = T2;
310};
311template <typename T1, typename T2, typename T3>
313 using X1 = T1;
314 using X2 = T2;
315 using X3 = T3;
316};
317template <typename T1, typename T2, typename T3, typename T4>
319 using X1 = T1;
320 using X2 = T2;
321 using X3 = T3;
322 using X4 = T4;
323};
324template <typename T1, typename T2, typename T3, typename T4, typename T5>
326 using X1 = T1;
327 using X2 = T2;
328 using X3 = T3;
329 using X4 = T4;
330 using X5 = T5;
331};
332template <typename T1, typename T2, typename T3, typename T4, typename T5,
333 typename T6, typename... TExtra>
334struct NoiseModelFactorAliases<T1, T2, T3, T4, T5, T6, TExtra...> {
335 using X1 = T1;
336 using X2 = T2;
337 using X3 = T3;
338 using X4 = T4;
339 using X5 = T5;
340 using X6 = T6;
341};
342} // namespace detail
343
344/* ************************************************************************* */
397template <class... ValueTypes>
399 : public NoiseModelFactor,
400 public detail::NoiseModelFactorAliases<ValueTypes...> {
401 public:
403 enum { N = sizeof...(ValueTypes) };
404
405 protected:
406 using Base = NoiseModelFactor;
407 using This = NoiseModelFactorN<ValueTypes...>;
408
411
412 template <typename From, typename To>
413 using IsConvertible =
414 typename std::enable_if<std::is_convertible<From, To>::value, void>::type;
415
416 template <int I>
417 using IndexIsValid = typename std::enable_if<(I >= 1) && (I <= N),
418 void>::type; // 1-indexed!
419
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>;
425
427
428 /* Like std::void_t, except produces `boost::optional<Matrix&>` instead of
429 * `void`. Used to expand fixed-type parameter-packs with same length as
430 * ValueTypes. */
431 template <typename T>
432 using OptionalMatrix = boost::optional<Matrix&>;
433
434 /* Like std::void_t, except produces `Key` instead of `void`. Used to expand
435 * fixed-type parameter-packs with same length as ValueTypes. */
436 template <typename T>
437 using KeyType = Key;
438
439 public:
459 template <int I, typename = IndexIsValid<I>>
460 using ValueType =
461 typename std::tuple_element<I - 1, std::tuple<ValueTypes...>>::type;
462
463 public:
464
467
470
479 KeyType<ValueTypes>... keys)
480 : Base(noiseModel, std::array<Key, N>{keys...}) {}
481
489 template <typename CONTAINER = std::initializer_list<Key>,
490 typename = IsContainerOfKeys<CONTAINER>>
492 : Base(noiseModel, keys) {
493 if (keys.size() != N) {
494 throw std::invalid_argument(
495 "NoiseModelFactorN: wrong number of keys given");
496 }
497 }
498
500
501 ~NoiseModelFactorN() override {}
502
517 template <int I = 1>
518 inline Key key() const {
519 static_assert(I <= N, "Index out of bounds");
520 return keys_[I - 1];
521 }
522
525
543 const Values& x,
544 boost::optional<std::vector<Matrix>&> H = boost::none) const override {
546 H);
547 }
548
552
575 virtual Vector evaluateError(const ValueTypes&... x,
576 OptionalMatrix<ValueTypes>... H) const = 0;
577
579
582
589 inline Vector evaluateError(const ValueTypes&... x) const {
590 return evaluateError(x..., OptionalMatrix<ValueTypes>()...);
591 }
592
597 template <typename... OptionalJacArgs,
598 typename = IndexIsValid<sizeof...(OptionalJacArgs) + 1>>
599 inline Vector evaluateError(const ValueTypes&... x,
600 OptionalJacArgs&&... H) const {
601 return evaluateError(x..., std::forward<OptionalJacArgs>(H)...,
602 boost::none);
603 }
604
606
607 private:
614 template <std::size_t... Indices>
615 inline Vector unwhitenedError(
617 const Values& x,
618 boost::optional<std::vector<Matrix>&> H = boost::none) const {
619 if (this->active(x)) {
620 if (H) {
621 return evaluateError(x.at<ValueTypes>(keys_[Indices])...,
622 (*H)[Indices]...);
623 } else {
624 return evaluateError(x.at<ValueTypes>(keys_[Indices])...);
625 }
626 } else {
627 return Vector::Zero(this->dim());
628 }
629 }
630
633 template <class ARCHIVE>
634 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
635 ar& boost::serialization::make_nvp(
636 "NoiseModelFactor", boost::serialization::base_object<Base>(*this));
637 }
638
639 public:
642
643 inline Key key1() const {
644 return key<1>();
645 }
646 template <int I = 2>
647 inline Key key2() const {
648 static_assert(I <= N, "Index out of bounds");
649 return key<2>();
650 }
651 template <int I = 3>
652 inline Key key3() const {
653 static_assert(I <= N, "Index out of bounds");
654 return key<3>();
655 }
656 template <int I = 4>
657 inline Key key4() const {
658 static_assert(I <= N, "Index out of bounds");
659 return key<4>();
660 }
661 template <int I = 5>
662 inline Key key5() const {
663 static_assert(I <= N, "Index out of bounds");
664 return key<5>();
665 }
666 template <int I = 6>
667 inline Key key6() const {
668 static_assert(I <= N, "Index out of bounds");
669 return key<6>();
670 }
671
673
674}; // \class NoiseModelFactorN
675
676#define NoiseModelFactor1 NoiseModelFactorN
677#define NoiseModelFactor2 NoiseModelFactorN
678#define NoiseModelFactor3 NoiseModelFactorN
679#define NoiseModelFactor4 NoiseModelFactorN
680#define NoiseModelFactor5 NoiseModelFactorN
681#define NoiseModelFactor6 NoiseModelFactorN
682
683} // namespace gtsam
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
Definition: Factor.h:68
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
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