gtsam  4.1.0
gtsam
PriorFactor.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 
16 #pragma once
17 
19 #include <gtsam/base/Testable.h>
20 
21 #include <string>
22 
23 namespace gtsam {
24 
29  template<class VALUE>
30  class PriorFactor: public NoiseModelFactor1<VALUE> {
31 
32  public:
33  typedef VALUE T;
34 
35  private:
36 
38 
39  VALUE prior_;
42  GTSAM_CONCEPT_TESTABLE_TYPE(T)
43 
44  public:
45 
47  typedef typename boost::shared_ptr<PriorFactor<VALUE> > shared_ptr;
48 
51 
54 
55  virtual ~PriorFactor() {}
56 
58  PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model = nullptr) :
59  Base(model, key), prior_(prior) {
60  }
61 
63  PriorFactor(Key key, const VALUE& prior, const Matrix& covariance) :
64  Base(noiseModel::Gaussian::Covariance(covariance), key), prior_(prior) {
65  }
66 
68  gtsam::NonlinearFactor::shared_ptr clone() const override {
69  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
70  gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
71 
75  void print(const std::string& s,
76  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
77  std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
78  traits<T>::Print(prior_, " prior mean: ");
79  if (this->noiseModel_)
80  this->noiseModel_->print(" noise model: ");
81  else
82  std::cout << "no noise model" << std::endl;
83  }
84 
86  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
87  const This* e = dynamic_cast<const This*> (&expected);
88  return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(prior_, e->prior_, tol);
89  }
90 
94  Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const override {
95  if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
96  // manifold equivalent of z-x -> Local(x,z)
97  // TODO(ASL) Add Jacobians.
98  return -traits<T>::Local(x, prior_);
99  }
100 
101  const VALUE & prior() const { return prior_; }
102 
103  private:
104 
107  template<class ARCHIVE>
108  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
109  ar & boost::serialization::make_nvp("NoiseModelFactor1",
110  boost::serialization::base_object<Base>(*this));
111  ar & BOOST_SERIALIZATION_NVP(prior_);
112  }
113 
114  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
115  enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
116  public:
118  };
119 
121  template<class VALUE>
122  struct traits<PriorFactor<VALUE> > : public Testable<PriorFactor<VALUE> > {};
123 
124 
125 }
Testable.h
Concept check for values that can be used in unit tests.
gtsam::PriorFactor::PriorFactor
PriorFactor(Key key, const VALUE &prior, const Matrix &covariance)
Convenience constructor that takes a full covariance argument.
Definition: PriorFactor.h:63
gtsam::PriorFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
implement functions needed for Testable
Definition: PriorFactor.h:75
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::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::PriorFactor::evaluateError
Vector evaluateError(const T &x, boost::optional< Matrix & > H=boost::none) const override
implement functions needed to derive from Factor
Definition: PriorFactor.h:94
gtsam::PriorFactor::shared_ptr
boost::shared_ptr< PriorFactor< VALUE > > shared_ptr
The measurement.
Definition: PriorFactor.h:47
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: NonlinearFactor.cpp:71
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
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
NonlinearFactor.h
Non-linear factor base classes.
gtsam::PriorFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
equals
Definition: PriorFactor.h:86
gtsam::PriorFactor::PriorFactor
PriorFactor(Key key, const VALUE &prior, const SharedNoiseModel &model=nullptr)
Constructor.
Definition: PriorFactor.h:58
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::PriorFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: PriorFactor.h:68
gtsam::PriorFactor
Definition: PriorFactor.h:30
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition: types.h:278
gtsam::PriorFactor::PriorFactor
PriorFactor()
default constructor - only use for serialization
Definition: PriorFactor.h:53
gtsam::PriorFactor::This
PriorFactor< VALUE > This
Typedef to this class.
Definition: PriorFactor.h:50
gtsam::PriorFactor::access
friend class boost::serialization::access
Serialization function.
Definition: PriorFactor.h:106