gtsam  4.1.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 
19 // \callgraph
20 
21 #pragma once
22 
23 #include <gtsam/nonlinear/Values.h>
26 #include <gtsam/inference/Factor.h>
28 
29 #include <boost/serialization/base_object.hpp>
30 #include <boost/assign/list_of.hpp>
31 
32 namespace gtsam {
33 
34 using boost::assign::cref_list_of;
35 
36 /* ************************************************************************* */
37 
43 class GTSAM_EXPORT NonlinearFactor: public Factor {
44 
45 protected:
46 
47  // Some handy typedefs
48  typedef Factor Base;
49  typedef NonlinearFactor This;
50 
51 public:
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  virtual void print(const std::string& s = "",
74  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
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  shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const;
130 
135  shared_ptr rekey(const KeyVector& new_keys) const;
136 
137 }; // \class NonlinearFactor
138 
140 template<> struct traits<NonlinearFactor> : public Testable<NonlinearFactor> {
141 };
142 
143 /* ************************************************************************* */
154 class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor {
155 
156 protected:
157 
158  // handy typedefs
159  typedef NonlinearFactor Base;
160  typedef NoiseModelFactor This;
161 
162  SharedNoiseModel noiseModel_;
164 public:
165 
166  typedef boost::shared_ptr<This> shared_ptr;
167 
170 
172  virtual ~NoiseModelFactor() {}
173 
177  template<typename CONTAINER>
178  NoiseModelFactor(const SharedNoiseModel& noiseModel, const CONTAINER& keys) :
179  Base(keys), noiseModel_(noiseModel) {}
180 
181 protected:
182 
186  NoiseModelFactor(const SharedNoiseModel& noiseModel) : noiseModel_(noiseModel) {}
187 
188 public:
189 
191  void print(const std::string& s = "",
192  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
193 
195  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override;
196 
198  size_t dim() const override {
199  return noiseModel_->dim();
200  }
201 
203  const SharedNoiseModel& noiseModel() const {
204  return noiseModel_;
205  }
206 
213  virtual Vector unwhitenedError(const Values& x,
214  boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
215 
220  Vector whitenedError(const Values& c) const;
221 
225  Vector unweightedWhitenedError(const Values& c) const;
226 
230  double weight(const Values& c) const;
231 
238  double error(const Values& c) const override;
239 
245  boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override;
246 
247  private:
249  friend class boost::serialization::access;
250  template<class ARCHIVE>
251  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
252  ar & boost::serialization::make_nvp("NonlinearFactor",
253  boost::serialization::base_object<Base>(*this));
254  ar & BOOST_SERIALIZATION_NVP(noiseModel_);
255  }
256 
257 }; // \class NoiseModelFactor
258 
259 
260 /* ************************************************************************* */
261 
270 template<class VALUE>
272 
273 public:
274 
275  // typedefs for value types pulled from keys
276  typedef VALUE X;
277 
278 protected:
279 
280  typedef NoiseModelFactor Base;
282 
283 public:
286 
289 
290  virtual ~NoiseModelFactor1() {}
291 
292  inline Key key() const { return keys_[0]; }
293 
300  : Base(noiseModel, cref_list_of<1>(key1)) {}
301 
305 
311  const Values &x,
312  boost::optional<std::vector<Matrix> &> H = boost::none) const override {
313  if (this->active(x)) {
314  const X &x1 = x.at<X>(keys_[0]);
315  if (H) {
316  return evaluateError(x1, (*H)[0]);
317  } else {
318  return evaluateError(x1);
319  }
320  } else {
321  return Vector::Zero(this->dim());
322  }
323  }
324 
328 
334  virtual Vector
335  evaluateError(const X &x,
336  boost::optional<Matrix &> H = boost::none) const = 0;
337 
339 
340 private:
342  friend class boost::serialization::access;
343  template<class ARCHIVE>
344  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
345  ar & boost::serialization::make_nvp("NoiseModelFactor",
346  boost::serialization::base_object<Base>(*this));
347  }
348 };// \class NoiseModelFactor1
349 
350 
351 /* ************************************************************************* */
354 template<class VALUE1, class VALUE2>
356 
357 public:
358 
359  // typedefs for value types pulled from keys
360  typedef VALUE1 X1;
361  typedef VALUE2 X2;
362 
363 protected:
364 
365  typedef NoiseModelFactor Base;
367 
368 public:
369 
374 
382  Base(noiseModel, cref_list_of<2>(j1)(j2)) {}
383 
384  virtual ~NoiseModelFactor2() {}
385 
387  inline Key key1() const { return keys_[0]; }
388  inline Key key2() const { return keys_[1]; }
389 
392  Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
393  if(this->active(x)) {
394  const X1& x1 = x.at<X1>(keys_[0]);
395  const X2& x2 = x.at<X2>(keys_[1]);
396  if(H) {
397  return evaluateError(x1, x2, (*H)[0], (*H)[1]);
398  } else {
399  return evaluateError(x1, x2);
400  }
401  } else {
402  return Vector::Zero(this->dim());
403  }
404  }
405 
411  virtual Vector
412  evaluateError(const X1&, const X2&, boost::optional<Matrix&> H1 =
413  boost::none, boost::optional<Matrix&> H2 = boost::none) const = 0;
414 
415 private:
416 
418  friend class boost::serialization::access;
419  template<class ARCHIVE>
420  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
421  ar & boost::serialization::make_nvp("NoiseModelFactor",
422  boost::serialization::base_object<Base>(*this));
423  }
424 }; // \class NoiseModelFactor2
425 
426 /* ************************************************************************* */
429 template<class VALUE1, class VALUE2, class VALUE3>
431 
432 public:
433 
434  // typedefs for value types pulled from keys
435  typedef VALUE1 X1;
436  typedef VALUE2 X2;
437  typedef VALUE3 X3;
438 
439 protected:
440 
441  typedef NoiseModelFactor Base;
443 
444 public:
445 
450 
459  Base(noiseModel, cref_list_of<3>(j1)(j2)(j3)) {}
460 
461  virtual ~NoiseModelFactor3() {}
462 
464  inline Key key1() const { return keys_[0]; }
465  inline Key key2() const { return keys_[1]; }
466  inline Key key3() const { return keys_[2]; }
467 
470  Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
471  if(this->active(x)) {
472  if(H)
473  return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), (*H)[0], (*H)[1], (*H)[2]);
474  else
475  return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]));
476  } else {
477  return Vector::Zero(this->dim());
478  }
479  }
480 
486  virtual Vector
487  evaluateError(const X1&, const X2&, const X3&,
488  boost::optional<Matrix&> H1 = boost::none,
489  boost::optional<Matrix&> H2 = boost::none,
490  boost::optional<Matrix&> H3 = boost::none) const = 0;
491 
492 private:
493 
495  friend class boost::serialization::access;
496  template<class ARCHIVE>
497  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
498  ar & boost::serialization::make_nvp("NoiseModelFactor",
499  boost::serialization::base_object<Base>(*this));
500  }
501 }; // \class NoiseModelFactor3
502 
503 /* ************************************************************************* */
506 template<class VALUE1, class VALUE2, class VALUE3, class VALUE4>
508 
509 public:
510 
511  // typedefs for value types pulled from keys
512  typedef VALUE1 X1;
513  typedef VALUE2 X2;
514  typedef VALUE3 X3;
515  typedef VALUE4 X4;
516 
517 protected:
518 
519  typedef NoiseModelFactor Base;
521 
522 public:
523 
528 
538  Base(noiseModel, cref_list_of<4>(j1)(j2)(j3)(j4)) {}
539 
540  virtual ~NoiseModelFactor4() {}
541 
543  inline Key key1() const { return keys_[0]; }
544  inline Key key2() const { return keys_[1]; }
545  inline Key key3() const { return keys_[2]; }
546  inline Key key4() const { return keys_[3]; }
547 
550  Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
551  if(this->active(x)) {
552  if(H)
553  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]);
554  else
555  return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]));
556  } else {
557  return Vector::Zero(this->dim());
558  }
559  }
560 
566  virtual Vector
567  evaluateError(const X1&, const X2&, const X3&, const X4&,
568  boost::optional<Matrix&> H1 = boost::none,
569  boost::optional<Matrix&> H2 = boost::none,
570  boost::optional<Matrix&> H3 = boost::none,
571  boost::optional<Matrix&> H4 = boost::none) const = 0;
572 
573 private:
574 
576  friend class boost::serialization::access;
577  template<class ARCHIVE>
578  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
579  ar & boost::serialization::make_nvp("NoiseModelFactor",
580  boost::serialization::base_object<Base>(*this));
581  }
582 }; // \class NoiseModelFactor4
583 
584 /* ************************************************************************* */
587 template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5>
589 
590 public:
591 
592  // typedefs for value types pulled from keys
593  typedef VALUE1 X1;
594  typedef VALUE2 X2;
595  typedef VALUE3 X3;
596  typedef VALUE4 X4;
597  typedef VALUE5 X5;
598 
599 protected:
600 
601  typedef NoiseModelFactor Base;
603 
604 public:
605 
610 
621  Base(noiseModel, cref_list_of<5>(j1)(j2)(j3)(j4)(j5)) {}
622 
623  virtual ~NoiseModelFactor5() {}
624 
626  inline Key key1() const { return keys_[0]; }
627  inline Key key2() const { return keys_[1]; }
628  inline Key key3() const { return keys_[2]; }
629  inline Key key4() const { return keys_[3]; }
630  inline Key key5() const { return keys_[4]; }
631 
634  Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
635  if(this->active(x)) {
636  if(H)
637  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]);
638  else
639  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]));
640  } else {
641  return Vector::Zero(this->dim());
642  }
643  }
644 
650  virtual Vector
651  evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&,
652  boost::optional<Matrix&> H1 = boost::none,
653  boost::optional<Matrix&> H2 = boost::none,
654  boost::optional<Matrix&> H3 = boost::none,
655  boost::optional<Matrix&> H4 = boost::none,
656  boost::optional<Matrix&> H5 = boost::none) const = 0;
657 
658 private:
659 
661  friend class boost::serialization::access;
662  template<class ARCHIVE>
663  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
664  ar & boost::serialization::make_nvp("NoiseModelFactor",
665  boost::serialization::base_object<Base>(*this));
666  }
667 }; // \class NoiseModelFactor5
668 
669 /* ************************************************************************* */
672 template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5, class VALUE6>
674 
675 public:
676 
677  // typedefs for value types pulled from keys
678  typedef VALUE1 X1;
679  typedef VALUE2 X2;
680  typedef VALUE3 X3;
681  typedef VALUE4 X4;
682  typedef VALUE5 X5;
683  typedef VALUE6 X6;
684 
685 protected:
686 
687  typedef NoiseModelFactor Base;
689 
690 public:
691 
696 
707  NoiseModelFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) :
708  Base(noiseModel, cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6)) {}
709 
710  virtual ~NoiseModelFactor6() {}
711 
713  inline Key key1() const { return keys_[0]; }
714  inline Key key2() const { return keys_[1]; }
715  inline Key key3() const { return keys_[2]; }
716  inline Key key4() const { return keys_[3]; }
717  inline Key key5() const { return keys_[4]; }
718  inline Key key6() const { return keys_[5]; }
719 
722  Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
723  if(this->active(x)) {
724  if(H)
725  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]);
726  else
727  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]));
728  } else {
729  return Vector::Zero(this->dim());
730  }
731  }
732 
738  virtual Vector
739  evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&,
740  boost::optional<Matrix&> H1 = boost::none,
741  boost::optional<Matrix&> H2 = boost::none,
742  boost::optional<Matrix&> H3 = boost::none,
743  boost::optional<Matrix&> H4 = boost::none,
744  boost::optional<Matrix&> H5 = boost::none,
745  boost::optional<Matrix&> H6 = boost::none) const = 0;
746 
747 private:
748 
750  friend class boost::serialization::access;
751  template<class ARCHIVE>
752  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
753  ar & boost::serialization::make_nvp("NoiseModelFactor",
754  boost::serialization::base_object<Base>(*this));
755  }
756 }; // \class NoiseModelFactor6
757 
758 /* ************************************************************************* */
759 
760 } // \namespace gtsam
gtsam::NoiseModelFactor4::unwhitenedError
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:550
JacobianFactor.h
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::NoiseModelFactor::NoiseModelFactor
NoiseModelFactor()
Default constructor for I/O only.
Definition: NonlinearFactor.h:169
gtsam::NoiseModelFactor1::NoiseModelFactor1
NoiseModelFactor1()
Default constructor for I/O only.
Definition: NonlinearFactor.h:288
gtsam::NoiseModelFactor6
A convenient base class for creating your own NoiseModelFactor with 6 variables.
Definition: NonlinearFactor.h:673
gtsam::NoiseModelFactor5::NoiseModelFactor5
NoiseModelFactor5(const SharedNoiseModel &noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5)
Constructor.
Definition: NonlinearFactor.h:620
gtsam::NoiseModelFactor3
A convenient base class for creating your own NoiseModelFactor with 3 variables.
Definition: NonlinearFactor.h:430
gtsam::NoiseModelFactor5::unwhitenedError
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:634
gtsam::NoiseModelFactor1::unwhitenedError
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:310
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:30
gtsam::NoiseModelFactor5::key1
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:626
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
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::NoiseModelFactor1
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:271
gtsam::NoiseModelFactor4::NoiseModelFactor4
NoiseModelFactor4()
Default Constructor for I/O.
Definition: NonlinearFactor.h:527
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:203
gtsam::NonlinearFactor::NonlinearFactor
NonlinearFactor(const CONTAINER &keys)
Constructor from a collection of the keys involved in this factor.
Definition: NonlinearFactor.h:65
gtsam::NoiseModelFactor5
A convenient base class for creating your own NoiseModelFactor with 5 variables.
Definition: NonlinearFactor.h:588
gtsam::NoiseModelFactor2::key1
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:387
gtsam::NonlinearFactor::dim
virtual size_t dim() const =0
get the dimension of the factor (number of rows on linearization)
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::NonlinearFactor::NonlinearFactor
NonlinearFactor()
Default constructor for I/O only.
Definition: NonlinearFactor.h:59
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:72
gtsam::NoiseModelFactor2::NoiseModelFactor2
NoiseModelFactor2(const SharedNoiseModel &noiseModel, Key j1, Key j2)
Constructor.
Definition: NonlinearFactor.h:381
gtsam::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition: NonlinearFactor.h:154
gtsam::NoiseModelFactor::NoiseModelFactor
NoiseModelFactor(const SharedNoiseModel &noiseModel)
Constructor - only for subclasses, as this does not set keys.
Definition: NonlinearFactor.h:186
gtsam::NoiseModelFactor3::key1
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:464
gtsam::NoiseModelFactor6::unwhitenedError
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:722
gtsam::NoiseModelFactor6::NoiseModelFactor6
NoiseModelFactor6(const SharedNoiseModel &noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6)
Constructor.
Definition: NonlinearFactor.h:707
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
OptionalJacobian.h
Special class for optional Jacobian arguments.
gtsam::NoiseModelFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition: NonlinearFactor.h:198
gtsam::NoiseModelFactor6::key1
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:713
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::NoiseModelFactor2::NoiseModelFactor2
NoiseModelFactor2()
Default Constructor for I/O.
Definition: NonlinearFactor.h:373
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
Factor.h
The base class for all factors.
gtsam::NoiseModelFactor5::NoiseModelFactor5
NoiseModelFactor5()
Default Constructor for I/O.
Definition: NonlinearFactor.h:609
gtsam::NoiseModelFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
Noise model.
Definition: NonlinearFactor.h:166
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:106
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::Values::at
ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:342
gtsam::NoiseModelFactor5::evaluateError
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.
gtsam::NoiseModelFactor3::evaluateError
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.
gtsam::NoiseModelFactor1::evaluateError
virtual Vector evaluateError(const X &x, boost::optional< Matrix & > H=boost::none) const =0
Override this method to finish implementing a unary factor.
gtsam::NoiseModelFactor2::unwhitenedError
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:392
gtsam::NoiseModelFactor1::NoiseModelFactor1
NoiseModelFactor1(const SharedNoiseModel &noiseModel, Key key1)
Constructor.
Definition: NonlinearFactor.h:299
gtsam::NoiseModelFactor2::evaluateError
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.
gtsam::NoiseModelFactor4::key1
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:543
gtsam::Pose3
Definition: Pose3.h:37
gtsam::KeyFormatter
boost::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::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::NoiseModelFactor2
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:355
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::NonlinearFactor::linearize
virtual boost::shared_ptr< GaussianFactor > linearize(const Values &c) const =0
linearize to a GaussianFactor
gtsam::NonlinearFactor::~NonlinearFactor
virtual ~NonlinearFactor()
Destructor.
Definition: NonlinearFactor.h:83
gtsam::NoiseModelFactor3::NoiseModelFactor3
NoiseModelFactor3()
Default Constructor for I/O.
Definition: NonlinearFactor.h:449
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:118
gtsam::NoiseModelFactor4::evaluateError
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.
gtsam::NoiseModelFactor4::NoiseModelFactor4
NoiseModelFactor4(const SharedNoiseModel &noiseModel, Key j1, Key j2, Key j3, Key j4)
Constructor.
Definition: NonlinearFactor.h:537
gtsam::NoiseModelFactor3::NoiseModelFactor3
NoiseModelFactor3(const SharedNoiseModel &noiseModel, Key j1, Key j2, Key j3)
Constructor.
Definition: NonlinearFactor.h:458
gtsam::NoiseModelFactor::NoiseModelFactor
NoiseModelFactor(const SharedNoiseModel &noiseModel, const CONTAINER &keys)
Constructor.
Definition: NonlinearFactor.h:178
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::NoiseModelFactor4
A convenient base class for creating your own NoiseModelFactor with 4 variables.
Definition: NonlinearFactor.h:507
gtsam::NonlinearFactor::error
virtual double error(const Values &c) const =0
Calculate the error of the factor This is typically equal to log-likelihood, e.g.
gtsam::NoiseModelFactor6::NoiseModelFactor6
NoiseModelFactor6()
Default Constructor for I/O.
Definition: NonlinearFactor.h:695
NoiseModel.h
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::NoiseModelFactor6::evaluateError
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.
gtsam::NoiseModelFactor3::unwhitenedError
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:470
gtsam::NoiseModelFactor::~NoiseModelFactor
virtual ~NoiseModelFactor()
Destructor.
Definition: NonlinearFactor.h:172