gtsam  3.2.0
gtsam
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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 <boost/serialization/base_object.hpp>
24 #include <boost/assign/list_of.hpp>
25 
26 #include <gtsam/nonlinear/Values.h>
29 #include <gtsam/inference/Factor.h>
30 
31 
36 #define ADD_CLONE_NONLINEAR_FACTOR(Derived) \
37  virtual gtsam::NonlinearFactor::shared_ptr clone() const { \
38  return boost::static_pointer_cast<gtsam::NonlinearFactor>( \
39  gtsam::NonlinearFactor::shared_ptr(new Derived(*this))); }
40 
41 namespace gtsam {
42 
43 using boost::assign::cref_list_of;
44 
45 /* ************************************************************************* */
54 class NonlinearFactor: public Factor {
55 
56 protected:
57 
58  // Some handy typedefs
59  typedef Factor Base;
60  typedef NonlinearFactor This;
61 
62 public:
63 
64  typedef boost::shared_ptr<This> shared_ptr;
65 
68 
71 
75  template<typename CONTAINER>
76  NonlinearFactor(const CONTAINER& keys) :
77  Base(keys) {}
78 
82 
84  virtual void print(const std::string& s = "",
85  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const
86  {
87  std::cout << s << " keys = { ";
88  BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; }
89  std::cout << "}" << std::endl;
90  }
91 
93  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
94  return Base::equals(f);
95  }
96 
100 
102  virtual ~NonlinearFactor() {}
103 
104 
110  virtual double error(const Values& c) const = 0;
111 
113  virtual size_t dim() const = 0;
114 
125  virtual bool active(const Values& c) const { return true; }
126 
128  virtual boost::shared_ptr<GaussianFactor>
129  linearize(const Values& c) const = 0;
130 
135  //virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
136  // std::vector<Index> indices(this->size());
137  // for(size_t j=0; j<this->size(); ++j)
138  // indices[j] = ordering[this->keys()[j]];
139  // return IndexFactor::shared_ptr(new IndexFactor(indices));
140  //}
141 
148  virtual shared_ptr clone() const {
149  // TODO: choose better exception to throw here
150  throw std::runtime_error("NonlinearFactor::clone(): Attempting to clone factor with no clone() implemented!");
151  return shared_ptr();
152  }
153 
158  shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const {
159  shared_ptr new_factor = clone();
160  for (size_t i=0; i<new_factor->size(); ++i) {
161  Key& cur_key = new_factor->keys()[i];
162  std::map<Key,Key>::const_iterator mapping = rekey_mapping.find(cur_key);
163  if (mapping != rekey_mapping.end())
164  cur_key = mapping->second;
165  }
166  return new_factor;
167  }
168 
173  shared_ptr rekey(const std::vector<Key>& new_keys) const {
174  assert(new_keys.size() == this->keys().size());
175  shared_ptr new_factor = clone();
176  new_factor->keys() = new_keys;
177  return new_factor;
178  }
179 
180 
181 }; // \class NonlinearFactor
182 
183 /* ************************************************************************* */
195 
196 protected:
197 
198  // handy typedefs
199  typedef NonlinearFactor Base;
200  typedef NoiseModelFactor This;
201 
202  SharedNoiseModel noiseModel_;
204 public:
205 
206  typedef boost::shared_ptr<This> shared_ptr;
207 
210 
212  virtual ~NoiseModelFactor() {}
213 
217  template<typename CONTAINER>
218  NoiseModelFactor(const SharedNoiseModel& noiseModel, const CONTAINER& keys) :
219  Base(keys), noiseModel_(noiseModel) {}
220 
221 protected:
222 
226  NoiseModelFactor(const SharedNoiseModel& noiseModel) : noiseModel_(noiseModel) {}
227 
228 public:
229 
231  virtual void print(const std::string& s = "",
232  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const
233  {
234  Base::print(s, keyFormatter);
235  this->noiseModel_->print(" noise model: ");
236  }
237 
239  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
240  const NoiseModelFactor* e = dynamic_cast<const NoiseModelFactor*>(&f);
241  return e && Base::equals(f, tol) &&
242  ((!noiseModel_ && !e->noiseModel_) ||
243  (noiseModel_ && e->noiseModel_ && noiseModel_->equals(*e->noiseModel_, tol)));
244  }
245 
247  virtual size_t dim() const {
248  return noiseModel_->dim();
249  }
250 
253  return noiseModel_;
254  }
255 
262  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
263 
268  Vector whitenedError(const Values& c) const {
269  const Vector unwhitenedErrorVec = unwhitenedError(c);
270  if((size_t) unwhitenedErrorVec.size() != noiseModel_->dim())
271  throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension.");
272  return noiseModel_->whiten(unwhitenedErrorVec);
273  }
274 
281  virtual double error(const Values& c) const {
282  if (this->active(c)) {
283  const Vector unwhitenedErrorVec = unwhitenedError(c);
284  if((size_t) unwhitenedErrorVec.size() != noiseModel_->dim())
285  throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension.");
286  return 0.5 * noiseModel_->distance(unwhitenedErrorVec);
287  } else {
288  return 0.0;
289  }
290  }
291 
297  boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
298  // Only linearize if the factor is active
299  if (!this->active(x))
300  return boost::shared_ptr<JacobianFactor>();
301 
302  // Create the set of terms - Jacobians for each index
303  Vector b;
304  // Call evaluate error to get Jacobians and b vector
305  std::vector<Matrix> A(this->size());
306  b = -unwhitenedError(x, A);
307  if(noiseModel_)
308  {
309  if((size_t) b.size() != noiseModel_->dim())
310  throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension.");
311 
312  this->noiseModel_->WhitenSystem(A,b);
313  }
314 
315  std::vector<std::pair<Key, Matrix> > terms(this->size());
316  // Fill in terms
317  for(size_t j=0; j<this->size(); ++j) {
318  terms[j].first = this->keys()[j];
319  terms[j].second.swap(A[j]);
320  }
321 
322  if(noiseModel_)
323  {
324  // TODO pass unwhitened + noise model to Gaussian factor
325  noiseModel::Constrained::shared_ptr constrained =
326  boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
327  if(constrained)
328  return GaussianFactor::shared_ptr(new JacobianFactor(terms, b, constrained->unit()));
329  else
330  return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
331  }
332  else
333  return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
334  }
335 
336 private:
337 
340  template<class ARCHIVE>
341  void serialize(ARCHIVE & ar, const unsigned int version) {
342  ar & boost::serialization::make_nvp("NonlinearFactor",
343  boost::serialization::base_object<Base>(*this));
344  ar & BOOST_SERIALIZATION_NVP(noiseModel_);
345  }
346 
347 }; // \class NoiseModelFactor
348 
349 
350 /* ************************************************************************* */
353 template<class VALUE>
355 
356 public:
357 
358  // typedefs for value types pulled from keys
359  typedef VALUE X;
360 
361 protected:
362 
363  typedef NoiseModelFactor Base;
365 
366 public:
367 
370 
371  virtual ~NoiseModelFactor1() {}
372 
373  inline Key key() const { return keys_[0]; }
374 
380  NoiseModelFactor1(const SharedNoiseModel& noiseModel, Key key1) :
381  Base(noiseModel, cref_list_of<1>(key1)) {}
382 
386  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
387  if(this->active(x)) {
388  const X& x1 = x.at<X>(keys_[0]);
389  if(H) {
390  return evaluateError(x1, (*H)[0]);
391  } else {
392  return evaluateError(x1);
393  }
394  } else {
395  return zero(this->dim());
396  }
397  }
398 
404  virtual Vector evaluateError(const X& x, boost::optional<Matrix&> H =
405  boost::none) const = 0;
406 
407 private:
408 
410  friend class boost::serialization::access;
411  template<class ARCHIVE>
412  void serialize(ARCHIVE & ar, const unsigned int version) {
413  ar & boost::serialization::make_nvp("NoiseModelFactor",
414  boost::serialization::base_object<Base>(*this));
415  }
416 };// \class NoiseModelFactor1
417 
418 
419 /* ************************************************************************* */
422 template<class VALUE1, class VALUE2>
424 
425 public:
426 
427  // typedefs for value types pulled from keys
428  typedef VALUE1 X1;
429  typedef VALUE2 X2;
430 
431 protected:
432 
433  typedef NoiseModelFactor Base;
435 
436 public:
437 
442 
449  NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) :
450  Base(noiseModel, cref_list_of<2>(j1)(j2)) {}
451 
452  virtual ~NoiseModelFactor2() {}
453 
455  inline Key key1() const { return keys_[0]; }
456  inline Key key2() const { return keys_[1]; }
457 
460  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
461  if(this->active(x)) {
462  const X1& x1 = x.at<X1>(keys_[0]);
463  const X2& x2 = x.at<X2>(keys_[1]);
464  if(H) {
465  return evaluateError(x1, x2, (*H)[0], (*H)[1]);
466  } else {
467  return evaluateError(x1, x2);
468  }
469  } else {
470  return zero(this->dim());
471  }
472  }
473 
479  virtual Vector
480  evaluateError(const X1&, const X2&, boost::optional<Matrix&> H1 =
481  boost::none, boost::optional<Matrix&> H2 = boost::none) const = 0;
482 
483 private:
484 
486  friend class boost::serialization::access;
487  template<class ARCHIVE>
488  void serialize(ARCHIVE & ar, const unsigned int version) {
489  ar & boost::serialization::make_nvp("NoiseModelFactor",
490  boost::serialization::base_object<Base>(*this));
491  }
492 }; // \class NoiseModelFactor2
493 
494 /* ************************************************************************* */
497 template<class VALUE1, class VALUE2, class VALUE3>
499 
500 public:
501 
502  // typedefs for value types pulled from keys
503  typedef VALUE1 X1;
504  typedef VALUE2 X2;
505  typedef VALUE3 X3;
506 
507 protected:
508 
509  typedef NoiseModelFactor Base;
511 
512 public:
513 
518 
526  NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) :
527  Base(noiseModel, cref_list_of<3>(j1)(j2)(j3)) {}
528 
529  virtual ~NoiseModelFactor3() {}
530 
532  inline Key key1() const { return keys_[0]; }
533  inline Key key2() const { return keys_[1]; }
534  inline Key key3() const { return keys_[2]; }
535 
538  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
539  if(this->active(x)) {
540  if(H)
541  return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), (*H)[0], (*H)[1], (*H)[2]);
542  else
543  return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]));
544  } else {
545  return zero(this->dim());
546  }
547  }
548 
554  virtual Vector
555  evaluateError(const X1&, const X2&, const X3&,
556  boost::optional<Matrix&> H1 = boost::none,
557  boost::optional<Matrix&> H2 = boost::none,
558  boost::optional<Matrix&> H3 = boost::none) const = 0;
559 
560 private:
561 
563  friend class boost::serialization::access;
564  template<class ARCHIVE>
565  void serialize(ARCHIVE & ar, const unsigned int version) {
566  ar & boost::serialization::make_nvp("NoiseModelFactor",
567  boost::serialization::base_object<Base>(*this));
568  }
569 }; // \class NoiseModelFactor3
570 
571 /* ************************************************************************* */
574 template<class VALUE1, class VALUE2, class VALUE3, class VALUE4>
576 
577 public:
578 
579  // typedefs for value types pulled from keys
580  typedef VALUE1 X1;
581  typedef VALUE2 X2;
582  typedef VALUE3 X3;
583  typedef VALUE4 X4;
584 
585 protected:
586 
587  typedef NoiseModelFactor Base;
589 
590 public:
591 
596 
605  NoiseModelFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4) :
606  Base(noiseModel, cref_list_of<4>(j1)(j2)(j3)(j4)) {}
607 
608  virtual ~NoiseModelFactor4() {}
609 
611  inline Key key1() const { return keys_[0]; }
612  inline Key key2() const { return keys_[1]; }
613  inline Key key3() const { return keys_[2]; }
614  inline Key key4() const { return keys_[3]; }
615 
618  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
619  if(this->active(x)) {
620  if(H)
621  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]);
622  else
623  return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]));
624  } else {
625  return zero(this->dim());
626  }
627  }
628 
634  virtual Vector
635  evaluateError(const X1&, const X2&, const X3&, const X4&,
636  boost::optional<Matrix&> H1 = boost::none,
637  boost::optional<Matrix&> H2 = boost::none,
638  boost::optional<Matrix&> H3 = boost::none,
639  boost::optional<Matrix&> H4 = boost::none) const = 0;
640 
641 private:
642 
645  template<class ARCHIVE>
646  void serialize(ARCHIVE & ar, const unsigned int version) {
647  ar & boost::serialization::make_nvp("NoiseModelFactor",
648  boost::serialization::base_object<Base>(*this));
649  }
650 }; // \class NoiseModelFactor4
651 
652 /* ************************************************************************* */
655 template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5>
657 
658 public:
659 
660  // typedefs for value types pulled from keys
661  typedef VALUE1 X1;
662  typedef VALUE2 X2;
663  typedef VALUE3 X3;
664  typedef VALUE4 X4;
665  typedef VALUE5 X5;
666 
667 protected:
668 
669  typedef NoiseModelFactor Base;
671 
672 public:
673 
678 
688  NoiseModelFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) :
689  Base(noiseModel, cref_list_of<5>(j1)(j2)(j3)(j4)(j5)) {}
690 
691  virtual ~NoiseModelFactor5() {}
692 
694  inline Key key1() const { return keys_[0]; }
695  inline Key key2() const { return keys_[1]; }
696  inline Key key3() const { return keys_[2]; }
697  inline Key key4() const { return keys_[3]; }
698  inline Key key5() const { return keys_[4]; }
699 
702  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
703  if(this->active(x)) {
704  if(H)
705  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]);
706  else
707  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]));
708  } else {
709  return zero(this->dim());
710  }
711  }
712 
718  virtual Vector
719  evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&,
720  boost::optional<Matrix&> H1 = boost::none,
721  boost::optional<Matrix&> H2 = boost::none,
722  boost::optional<Matrix&> H3 = boost::none,
723  boost::optional<Matrix&> H4 = boost::none,
724  boost::optional<Matrix&> H5 = boost::none) const = 0;
725 
726 private:
727 
730  template<class ARCHIVE>
731  void serialize(ARCHIVE & ar, const unsigned int version) {
732  ar & boost::serialization::make_nvp("NoiseModelFactor",
733  boost::serialization::base_object<Base>(*this));
734  }
735 }; // \class NoiseModelFactor5
736 
737 /* ************************************************************************* */
740 template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5, class VALUE6>
742 
743 public:
744 
745  // typedefs for value types pulled from keys
746  typedef VALUE1 X1;
747  typedef VALUE2 X2;
748  typedef VALUE3 X3;
749  typedef VALUE4 X4;
750  typedef VALUE5 X5;
751  typedef VALUE6 X6;
752 
753 protected:
754 
755  typedef NoiseModelFactor Base;
757 
758 public:
759 
764 
775  NoiseModelFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) :
776  Base(noiseModel, cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6)) {}
777 
778  virtual ~NoiseModelFactor6() {}
779 
781  inline Key key1() const { return keys_[0]; }
782  inline Key key2() const { return keys_[1]; }
783  inline Key key3() const { return keys_[2]; }
784  inline Key key4() const { return keys_[3]; }
785  inline Key key5() const { return keys_[4]; }
786  inline Key key6() const { return keys_[5]; }
787 
790  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
791  if(this->active(x)) {
792  if(H)
793  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]);
794  else
795  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]));
796  } else {
797  return zero(this->dim());
798  }
799  }
800 
806  virtual Vector
807  evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&,
808  boost::optional<Matrix&> H1 = boost::none,
809  boost::optional<Matrix&> H2 = boost::none,
810  boost::optional<Matrix&> H3 = boost::none,
811  boost::optional<Matrix&> H4 = boost::none,
812  boost::optional<Matrix&> H5 = boost::none,
813  boost::optional<Matrix&> H6 = boost::none) const = 0;
814 
815 private:
816 
819  template<class ARCHIVE>
820  void serialize(ARCHIVE & ar, const unsigned int version) {
821  ar & boost::serialization::make_nvp("NoiseModelFactor",
822  boost::serialization::base_object<Base>(*this));
823  }
824 }; // \class NoiseModelFactor6
825 
826 /* ************************************************************************* */
827 
828 } // \namespace gtsam
shared_ptr rekey(const std::vector< Key > &new_keys) const
Clones a factor and fully replaces its keys.
Definition: NonlinearFactor.h:173
virtual Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const
Calls the 6-key specific version of evaluateError, which is pure virtual so must be implemented in th...
Definition: NonlinearFactor.h:790
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
Check if two factors are equal.
Definition: NonlinearFactor.h:239
NoiseModelFactor1()
Default constructor for I/O only.
Definition: NonlinearFactor.h:369
virtual double error(const Values &c) const =0
Calculate the error of the factor This is typically equal to log-likelihood, e.g. ...
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition: NonlinearFactor.h:194
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print
Definition: NonlinearFactor.h:84
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.
virtual size_t dim() const =0
get the dimension of the factor (number of rows on linearization)
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:611
virtual boost::shared_ptr< GaussianFactor > linearize(const Values &c) const =0
linearize to a GaussianFactor
virtual size_t dim() const
get the dimension of the factor (number of rows on linearization)
Definition: NonlinearFactor.h:247
NoiseModelFactor(const SharedNoiseModel &noiseModel, const CONTAINER &keys)
Constructor.
Definition: NonlinearFactor.h:218
Vector whitenedError(const Values &c) const
Vector of errors, whitened This is the raw error, i.e., i.e.
Definition: NonlinearFactor.h:268
friend class boost::serialization::access
Serialization function.
Definition: NonlinearFactor.h:339
NoiseModelFactor1(const SharedNoiseModel &noiseModel, Key key1)
Constructor.
Definition: NonlinearFactor.h:380
virtual Vector evaluateError(const X &x, boost::optional< Matrix & > H=boost::none) const =0
Override this method to finish implementing a unary factor.
NonlinearFactor()
Default constructor for I/O only.
Definition: NonlinearFactor.h:70
NoiseModelFactor4(const SharedNoiseModel &noiseModel, Key j1, Key j2, Key j3, Key j4)
Constructor.
Definition: NonlinearFactor.h:605
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.
FastVector< Key > keys_
The keys involved in this factor.
Definition: Factor.h:69
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const
Linearize a non-linearFactorN to get a GaussianFactor, Hence .
Definition: NonlinearFactor.h:297
A non-templated config holding any types of Manifold-group elements.
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:423
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:82
const FastVector< Key > & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:115
NoiseModelFactor5()
Default Constructor for I/O.
Definition: NonlinearFactor.h:677
NoiseModelFactor6()
Default Constructor for I/O.
Definition: NonlinearFactor.h:763
This is the base class for all factor types.
Definition: Factor.h:51
NoiseModelFactor6(const SharedNoiseModel &noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6)
Constructor.
Definition: NonlinearFactor.h:775
virtual Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const
Calls the 1-key specific version of evaluateError, which is pure virtual so must be implemented in th...
Definition: NonlinearFactor.h:386
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.
NoiseModelFactor4()
Default Constructor for I/O.
Definition: NonlinearFactor.h:595
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:75
bool zero(const Vector &v)
check if all zero
Definition: Vector.cpp:39
virtual ~NoiseModelFactor()
Destructor.
Definition: NonlinearFactor.h:212
NonlinearFactor(const CONTAINER &keys)
Constructor from a collection of the keys involved in this factor.
Definition: NonlinearFactor.h:76
A Constrained constrained model is a specialization of Diagonal which allows some or all of the sigma...
Definition: NoiseModel.h:361
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:455
const ValueType & at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:219
The base class for all factors.
NoiseModelFactor(const SharedNoiseModel &noiseModel)
Constructor - only for subclasses, as this does not set keys.
Definition: NonlinearFactor.h:226
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
Check if two factors are equal.
Definition: NonlinearFactor.h:93
NoiseModelFactor()
Default constructor for I/O only.
Definition: NonlinearFactor.h:209
shared_ptr rekey(const std::map< Key, Key > &rekey_mapping) const
Creates a shared_ptr clone of the factor with different keys using a map from old->new keys...
Definition: NonlinearFactor.h:158
boost::shared_ptr< This > shared_ptr
Noise model.
Definition: NonlinearFactor.h:206
size_t Key
Integer nonlinear key type.
Definition: types.h:59
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:43
NoiseModelFactor3()
Default Constructor for I/O.
Definition: NonlinearFactor.h:517
size_t size() const
Definition: Factor.h:126
friend class boost::serialization::access
Serialization function.
Definition: NonlinearFactor.h:729
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:39
virtual Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const =0
Error function without the NoiseModel, .
SharedNoiseModel get_noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:252
virtual Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const
Calls the 5-key specific version of evaluateError, which is pure virtual so must be implemented in th...
Definition: NonlinearFactor.h:702
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:354
friend class boost::serialization::access
Serialization function.
Definition: NonlinearFactor.h:818
virtual shared_ptr clone() const
Create a symbolic factor using the given ordering to determine the variable indices.
Definition: NonlinearFactor.h:148
A convenient base class for creating your own NoiseModelFactor with 6 variables.
Definition: NonlinearFactor.h:741
NoiseModelFactor3(const SharedNoiseModel &noiseModel, Key j1, Key j2, Key j3)
Constructor.
Definition: NonlinearFactor.h:526
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:532
A convenient base class for creating your own NoiseModelFactor with 4 variables.
Definition: NonlinearFactor.h:575
virtual Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const
Calls the 4-key specific version of evaluateError, which is pure virtual so must be implemented in th...
Definition: NonlinearFactor.h:618
virtual Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const
Calls the 3-key specific version of evaluateError, which is pure virtual so must be implemented in th...
Definition: NonlinearFactor.h:538
virtual double error(const Values &c) const
Calculate the error of the factor.
Definition: NonlinearFactor.h:281
virtual bool active(const Values &c) const
Checks whether a factor should be used based on a set of values.
Definition: NonlinearFactor.h:125
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.
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:694
virtual ~NonlinearFactor()
Destructor.
Definition: NonlinearFactor.h:102
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Print.
Definition: NonlinearFactor.h:231
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:884
Nonlinear factor base class.
Definition: NonlinearFactor.h:54
friend class boost::serialization::access
Serialization function.
Definition: NonlinearFactor.h:644
NoiseModelFactor2(const SharedNoiseModel &noiseModel, Key j1, Key j2)
Constructor.
Definition: NonlinearFactor.h:449
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:781
Definition: Point3.h:39
boost::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: types.h:62
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.
virtual Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const
Calls the 2-key specific version of evaluateError, which is pure virtual so must be implemented in th...
Definition: NonlinearFactor.h:460
A convenient base class for creating your own NoiseModelFactor with 5 variables.
Definition: NonlinearFactor.h:656
NoiseModelFactor2()
Default Constructor for I/O.
Definition: NonlinearFactor.h:441
A convenient base class for creating your own NoiseModelFactor with 3 variables.
Definition: NonlinearFactor.h:498
NoiseModelFactor5(const SharedNoiseModel &noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5)
Constructor.
Definition: NonlinearFactor.h:688