gtsam 4.1.1
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
19// \callgraph
20
21#pragma once
22
28
29#include <boost/serialization/base_object.hpp>
30#include <boost/assign/list_of.hpp>
31
32namespace gtsam {
33
34using boost::assign::cref_list_of;
35
36/* ************************************************************************* */
37
43class GTSAM_EXPORT NonlinearFactor: public Factor {
44
45protected:
46
47 // Some handy typedefs
48 typedef Factor Base;
49 typedef NonlinearFactor This;
50
51public:
52
53 typedef boost::shared_ptr<This> shared_ptr;
54
57
60
64 template<typename CONTAINER>
65 NonlinearFactor(const CONTAINER& keys) :
66 Base(keys) {}
67
71
73 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
74 DefaultKeyFormatter) const override;
75
77 virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
81
83 virtual ~NonlinearFactor() {}
84
85
91 virtual double error(const Values& c) const = 0;
92
94 virtual size_t dim() const = 0;
95
106 virtual bool active(const Values& /*c*/) const { return true; }
107
109 virtual boost::shared_ptr<GaussianFactor>
110 linearize(const Values& c) const = 0;
111
118 virtual shared_ptr clone() const {
119 // TODO: choose better exception to throw here
120 throw std::runtime_error("NonlinearFactor::clone(): Attempting to clone factor with no clone() implemented!");
121 return shared_ptr();
122 }
123
129 virtual shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const;
130
135 virtual shared_ptr rekey(const KeyVector& new_keys) const;
136
141 virtual bool sendable() const {
142 return true;
143 }
144
145}; // \class NonlinearFactor
146
148template<> struct traits<NonlinearFactor> : public Testable<NonlinearFactor> {
149};
150
151/* ************************************************************************* */
162class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor {
163
164protected:
165
166 // handy typedefs
167 typedef NonlinearFactor Base;
168 typedef NoiseModelFactor This;
169
170 SharedNoiseModel noiseModel_;
172public:
173
174 typedef boost::shared_ptr<This> shared_ptr;
175
178
180 ~NoiseModelFactor() override {}
181
185 template<typename CONTAINER>
186 NoiseModelFactor(const SharedNoiseModel& noiseModel, const CONTAINER& keys) :
187 Base(keys), noiseModel_(noiseModel) {}
188
189protected:
190
194 NoiseModelFactor(const SharedNoiseModel& noiseModel) : noiseModel_(noiseModel) {}
195
196public:
197
199 void print(const std::string& s = "",
200 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
201
203 bool equals(const NonlinearFactor& f, double tol = 1e-9) const override;
204
206 size_t dim() const override {
207 return noiseModel_->dim();
208 }
209
212 return noiseModel_;
213 }
214
221 virtual Vector unwhitenedError(const Values& x,
222 boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
223
228 Vector whitenedError(const Values& c) const;
229
233 Vector unweightedWhitenedError(const Values& c) const;
234
238 double weight(const Values& c) const;
239
246 double error(const Values& c) const override;
247
253 boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override;
254
259 shared_ptr cloneWithNewNoiseModel(const SharedNoiseModel newNoise) const;
260
261 private:
263 friend class boost::serialization::access;
264 template<class ARCHIVE>
265 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
266 ar & boost::serialization::make_nvp("NonlinearFactor",
267 boost::serialization::base_object<Base>(*this));
268 ar & BOOST_SERIALIZATION_NVP(noiseModel_);
269 }
270
271}; // \class NoiseModelFactor
272
273
274/* ************************************************************************* */
275
284template<class VALUE>
286
287public:
288
289 // typedefs for value types pulled from keys
290 typedef VALUE X;
291
292protected:
293
294 typedef NoiseModelFactor Base;
296
297public:
300
303
304 ~NoiseModelFactor1() override {}
305
306 inline Key key() const { return keys_[0]; }
307
314 : Base(noiseModel, cref_list_of<1>(key1)) {}
315
319
325 const Values &x,
326 boost::optional<std::vector<Matrix> &> H = boost::none) const override {
327 if (this->active(x)) {
328 const X &x1 = x.at<X>(keys_[0]);
329 if (H) {
330 return evaluateError(x1, (*H)[0]);
331 } else {
332 return evaluateError(x1);
333 }
334 } else {
335 return Vector::Zero(this->dim());
336 }
337 }
338
342
348 virtual Vector
349 evaluateError(const X &x,
350 boost::optional<Matrix &> H = boost::none) const = 0;
351
353
354private:
357 template<class ARCHIVE>
358 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
359 ar & boost::serialization::make_nvp("NoiseModelFactor",
360 boost::serialization::base_object<Base>(*this));
361 }
362};// \class NoiseModelFactor1
363
364
365/* ************************************************************************* */
368template<class VALUE1, class VALUE2>
370
371public:
372
373 // typedefs for value types pulled from keys
374 typedef VALUE1 X1;
375 typedef VALUE2 X2;
376
377protected:
378
379 typedef NoiseModelFactor Base;
381
382public:
383
388
396 Base(noiseModel, cref_list_of<2>(j1)(j2)) {}
397
398 ~NoiseModelFactor2() override {}
399
401 inline Key key1() const { return keys_[0]; }
402 inline Key key2() const { return keys_[1]; }
403
406 Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
407 if(this->active(x)) {
408 const X1& x1 = x.at<X1>(keys_[0]);
409 const X2& x2 = x.at<X2>(keys_[1]);
410 if(H) {
411 return evaluateError(x1, x2, (*H)[0], (*H)[1]);
412 } else {
413 return evaluateError(x1, x2);
414 }
415 } else {
416 return Vector::Zero(this->dim());
417 }
418 }
419
425 virtual Vector
426 evaluateError(const X1&, const X2&, boost::optional<Matrix&> H1 =
427 boost::none, boost::optional<Matrix&> H2 = boost::none) const = 0;
428
429private:
430
433 template<class ARCHIVE>
434 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
435 ar & boost::serialization::make_nvp("NoiseModelFactor",
436 boost::serialization::base_object<Base>(*this));
437 }
438}; // \class NoiseModelFactor2
439
440/* ************************************************************************* */
443template<class VALUE1, class VALUE2, class VALUE3>
445
446public:
447
448 // typedefs for value types pulled from keys
449 typedef VALUE1 X1;
450 typedef VALUE2 X2;
451 typedef VALUE3 X3;
452
453protected:
454
455 typedef NoiseModelFactor Base;
457
458public:
459
464
473 Base(noiseModel, cref_list_of<3>(j1)(j2)(j3)) {}
474
475 ~NoiseModelFactor3() override {}
476
478 inline Key key1() const { return keys_[0]; }
479 inline Key key2() const { return keys_[1]; }
480 inline Key key3() const { return keys_[2]; }
481
484 Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
485 if(this->active(x)) {
486 if(H)
487 return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), (*H)[0], (*H)[1], (*H)[2]);
488 else
489 return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]));
490 } else {
491 return Vector::Zero(this->dim());
492 }
493 }
494
500 virtual Vector
501 evaluateError(const X1&, const X2&, const X3&,
502 boost::optional<Matrix&> H1 = boost::none,
503 boost::optional<Matrix&> H2 = boost::none,
504 boost::optional<Matrix&> H3 = boost::none) const = 0;
505
506private:
507
510 template<class ARCHIVE>
511 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
512 ar & boost::serialization::make_nvp("NoiseModelFactor",
513 boost::serialization::base_object<Base>(*this));
514 }
515}; // \class NoiseModelFactor3
516
517/* ************************************************************************* */
520template<class VALUE1, class VALUE2, class VALUE3, class VALUE4>
522
523public:
524
525 // typedefs for value types pulled from keys
526 typedef VALUE1 X1;
527 typedef VALUE2 X2;
528 typedef VALUE3 X3;
529 typedef VALUE4 X4;
530
531protected:
532
533 typedef NoiseModelFactor Base;
535
536public:
537
542
552 Base(noiseModel, cref_list_of<4>(j1)(j2)(j3)(j4)) {}
553
554 ~NoiseModelFactor4() override {}
555
557 inline Key key1() const { return keys_[0]; }
558 inline Key key2() const { return keys_[1]; }
559 inline Key key3() const { return keys_[2]; }
560 inline Key key4() const { return keys_[3]; }
561
564 Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
565 if(this->active(x)) {
566 if(H)
567 return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
568 else
569 return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]));
570 } else {
571 return Vector::Zero(this->dim());
572 }
573 }
574
580 virtual Vector
581 evaluateError(const X1&, const X2&, const X3&, const X4&,
582 boost::optional<Matrix&> H1 = boost::none,
583 boost::optional<Matrix&> H2 = boost::none,
584 boost::optional<Matrix&> H3 = boost::none,
585 boost::optional<Matrix&> H4 = boost::none) const = 0;
586
587private:
588
591 template<class ARCHIVE>
592 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
593 ar & boost::serialization::make_nvp("NoiseModelFactor",
594 boost::serialization::base_object<Base>(*this));
595 }
596}; // \class NoiseModelFactor4
597
598/* ************************************************************************* */
601template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5>
603
604public:
605
606 // typedefs for value types pulled from keys
607 typedef VALUE1 X1;
608 typedef VALUE2 X2;
609 typedef VALUE3 X3;
610 typedef VALUE4 X4;
611 typedef VALUE5 X5;
612
613protected:
614
615 typedef NoiseModelFactor Base;
617
618public:
619
624
635 Base(noiseModel, cref_list_of<5>(j1)(j2)(j3)(j4)(j5)) {}
636
637 ~NoiseModelFactor5() override {}
638
640 inline Key key1() const { return keys_[0]; }
641 inline Key key2() const { return keys_[1]; }
642 inline Key key3() const { return keys_[2]; }
643 inline Key key4() const { return keys_[3]; }
644 inline Key key5() const { return keys_[4]; }
645
648 Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
649 if(this->active(x)) {
650 if(H)
651 return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
652 else
653 return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]));
654 } else {
655 return Vector::Zero(this->dim());
656 }
657 }
658
664 virtual Vector
665 evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&,
666 boost::optional<Matrix&> H1 = boost::none,
667 boost::optional<Matrix&> H2 = boost::none,
668 boost::optional<Matrix&> H3 = boost::none,
669 boost::optional<Matrix&> H4 = boost::none,
670 boost::optional<Matrix&> H5 = boost::none) const = 0;
671
672private:
673
676 template<class ARCHIVE>
677 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
678 ar & boost::serialization::make_nvp("NoiseModelFactor",
679 boost::serialization::base_object<Base>(*this));
680 }
681}; // \class NoiseModelFactor5
682
683/* ************************************************************************* */
686template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5, class VALUE6>
688
689public:
690
691 // typedefs for value types pulled from keys
692 typedef VALUE1 X1;
693 typedef VALUE2 X2;
694 typedef VALUE3 X3;
695 typedef VALUE4 X4;
696 typedef VALUE5 X5;
697 typedef VALUE6 X6;
698
699protected:
700
701 typedef NoiseModelFactor Base;
703
704public:
705
710
722 Base(noiseModel, cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6)) {}
723
724 ~NoiseModelFactor6() override {}
725
727 inline Key key1() const { return keys_[0]; }
728 inline Key key2() const { return keys_[1]; }
729 inline Key key3() const { return keys_[2]; }
730 inline Key key4() const { return keys_[3]; }
731 inline Key key5() const { return keys_[4]; }
732 inline Key key6() const { return keys_[5]; }
733
736 Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
737 if(this->active(x)) {
738 if(H)
739 return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
740 else
741 return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]));
742 } else {
743 return Vector::Zero(this->dim());
744 }
745 }
746
752 virtual Vector
753 evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&,
754 boost::optional<Matrix&> H1 = boost::none,
755 boost::optional<Matrix&> H2 = boost::none,
756 boost::optional<Matrix&> H3 = boost::none,
757 boost::optional<Matrix&> H4 = boost::none,
758 boost::optional<Matrix&> H5 = boost::none,
759 boost::optional<Matrix&> H6 = boost::none) const = 0;
760
761private:
762
765 template<class ARCHIVE>
766 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
767 ar & boost::serialization::make_nvp("NoiseModelFactor",
768 boost::serialization::base_object<Base>(*this));
769 }
770}; // \class NoiseModelFactor6
771
772/* ************************************************************************* */
773
774} // \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:112
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:736
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
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
This is the base class for all factor types.
Definition: Factor.h:56
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:73
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
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:59
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition: NonlinearFactor.h:106
NonlinearFactor(const CONTAINER &keys)
Constructor from a collection of the keys involved in this factor.
Definition: NonlinearFactor.h:65
virtual double error(const Values &c) const =0
Calculate the error of the factor This is typically equal to log-likelihood, e.g.
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:141
virtual shared_ptr clone() const
Creates a shared_ptr clone of the factor - needs to be specialized to allow for subclasses.
Definition: NonlinearFactor.h:118
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:162
NoiseModelFactor(const SharedNoiseModel &noiseModel, const CONTAINER &keys)
Constructor.
Definition: NonlinearFactor.h:186
~NoiseModelFactor() override
Destructor.
Definition: NonlinearFactor.h:180
NoiseModelFactor(const SharedNoiseModel &noiseModel)
Constructor - only for subclasses, as this does not set keys.
Definition: NonlinearFactor.h:194
boost::shared_ptr< This > shared_ptr
Noise model.
Definition: NonlinearFactor.h:174
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition: NonlinearFactor.h:206
NoiseModelFactor()
Default constructor for I/O only.
Definition: NonlinearFactor.h:177
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:211
virtual Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const =0
Error function without the NoiseModel, .
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:285
virtual Vector evaluateError(const X &x, boost::optional< Matrix & > H=boost::none) const =0
Override this method to finish implementing a unary factor.
NoiseModelFactor1(const SharedNoiseModel &noiseModel, Key key1)
Constructor.
Definition: NonlinearFactor.h:313
NoiseModelFactor1()
Default constructor for I/O only.
Definition: NonlinearFactor.h:302
friend class boost::serialization::access
Serialization function.
Definition: NonlinearFactor.h:356
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Calls the 1-key specific version of evaluateError below, which is pure virtual so must be implemented...
Definition: NonlinearFactor.h:324
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:369
NoiseModelFactor2()
Default Constructor for I/O.
Definition: NonlinearFactor.h:387
NoiseModelFactor2(const SharedNoiseModel &noiseModel, Key j1, Key j2)
Constructor.
Definition: NonlinearFactor.h:395
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:401
virtual Vector evaluateError(const X1 &, const X2 &, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const =0
Override this method to finish implementing a binary factor.
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Calls the 2-key specific version of evaluateError, which is pure virtual so must be implemented in th...
Definition: NonlinearFactor.h:406
friend class boost::serialization::access
Serialization function.
Definition: NonlinearFactor.h:432
A convenient base class for creating your own NoiseModelFactor with 3 variables.
Definition: NonlinearFactor.h:444
NoiseModelFactor3()
Default Constructor for I/O.
Definition: NonlinearFactor.h:463
virtual Vector evaluateError(const X1 &, const X2 &, const X3 &, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const =0
Override this method to finish implementing a trinary factor.
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:478
NoiseModelFactor3(const SharedNoiseModel &noiseModel, Key j1, Key j2, Key j3)
Constructor.
Definition: NonlinearFactor.h:472
friend class boost::serialization::access
Serialization function.
Definition: NonlinearFactor.h:509
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Calls the 3-key specific version of evaluateError, which is pure virtual so must be implemented in th...
Definition: NonlinearFactor.h:484
A convenient base class for creating your own NoiseModelFactor with 4 variables.
Definition: NonlinearFactor.h:521
virtual Vector evaluateError(const X1 &, const X2 &, const X3 &, const X4 &, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none, boost::optional< Matrix & > H4=boost::none) const =0
Override this method to finish implementing a 4-way factor.
NoiseModelFactor4()
Default Constructor for I/O.
Definition: NonlinearFactor.h:541
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Calls the 4-key specific version of evaluateError, which is pure virtual so must be implemented in th...
Definition: NonlinearFactor.h:564
NoiseModelFactor4(const SharedNoiseModel &noiseModel, Key j1, Key j2, Key j3, Key j4)
Constructor.
Definition: NonlinearFactor.h:551
friend class boost::serialization::access
Serialization function.
Definition: NonlinearFactor.h:590
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:557
A convenient base class for creating your own NoiseModelFactor with 5 variables.
Definition: NonlinearFactor.h:602
NoiseModelFactor5(const SharedNoiseModel &noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5)
Constructor.
Definition: NonlinearFactor.h:634
NoiseModelFactor5()
Default Constructor for I/O.
Definition: NonlinearFactor.h:623
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:640
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Calls the 5-key specific version of evaluateError, which is pure virtual so must be implemented in th...
Definition: NonlinearFactor.h:648
friend class boost::serialization::access
Serialization function.
Definition: NonlinearFactor.h:675
virtual Vector evaluateError(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none, boost::optional< Matrix & > H4=boost::none, boost::optional< Matrix & > H5=boost::none) const =0
Override this method to finish implementing a 5-way factor.
A convenient base class for creating your own NoiseModelFactor with 6 variables.
Definition: NonlinearFactor.h:687
NoiseModelFactor6()
Default Constructor for I/O.
Definition: NonlinearFactor.h:709
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Calls the 6-key specific version of evaluateError, which is pure virtual so must be implemented in th...
Definition: NonlinearFactor.h:736
virtual Vector evaluateError(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none, boost::optional< Matrix & > H4=boost::none, boost::optional< Matrix & > H5=boost::none, boost::optional< Matrix & > H6=boost::none) const =0
Override this method to finish implementing a 6-way factor.
friend class boost::serialization::access
Serialization function.
Definition: NonlinearFactor.h:764
NoiseModelFactor6(const SharedNoiseModel &noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6)
Constructor.
Definition: NonlinearFactor.h:721
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:727
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:63
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:346