nonlinear/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 NoiseModelFactorN<VALUE> {
31 
32  public:
33  typedef VALUE T;
34 
35  // Provide access to the Matrix& version of evaluateError:
36  using NoiseModelFactor1<VALUE>::evaluateError;
37 
38 
39  private:
40 
42 
47 
48  public:
49 
51  typedef typename std::shared_ptr<PriorFactor<VALUE> > shared_ptr;
52 
55 
58 
59  ~PriorFactor() override {}
60 
62  PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model = nullptr) :
63  Base(model, key), prior_(prior) {
64  }
65 
67  PriorFactor(Key key, const VALUE& prior, const Matrix& covariance) :
68  Base(noiseModel::Gaussian::Covariance(covariance), key), prior_(prior) {
69  }
70 
73  return std::static_pointer_cast<gtsam::NonlinearFactor>(
75 
79  void print(const std::string& s,
80  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
81  std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
82  traits<T>::Print(prior_, " prior mean: ");
83  if (this->noiseModel_)
84  this->noiseModel_->print(" noise model: ");
85  else
86  std::cout << "no noise model" << std::endl;
87  }
88 
90  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
91  const This* e = dynamic_cast<const This*> (&expected);
92  return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(prior_, e->prior_, tol);
93  }
94 
98  Vector evaluateError(const T& x, OptionalMatrixType H) const override {
99  if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
100  // manifold equivalent of z-x -> Local(x,z)
101  return -traits<T>::Local(x, prior_);
102  }
103 
104  const VALUE & prior() const { return prior_; }
105 
106  private:
107 
108 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
109 
110  friend class boost::serialization::access;
111  template<class ARCHIVE>
112  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
113  // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
114  ar & boost::serialization::make_nvp("NoiseModelFactor1",
115  boost::serialization::base_object<Base>(*this));
116  ar & BOOST_SERIALIZATION_NVP(prior_);
117  }
118 #endif
119 
120  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
121  enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
122  public:
124  };
125 
127  template<class VALUE>
128  struct traits<PriorFactor<VALUE> > : public Testable<PriorFactor<VALUE> > {};
129 
130 
131 }
Gaussian
double Gaussian(double mu, double sigma, double z)
Gaussian density function.
Definition: testGaussianMixture.cpp:47
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Testable.h
Concept check for values that can be used in unit tests.
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::PriorFactor
Definition: nonlinear/PriorFactor.h:30
gtsam::PriorFactor::NeedsToAlign
@ NeedsToAlign
Definition: nonlinear/PriorFactor.h:121
gtsam::PriorFactor::~PriorFactor
~PriorFactor() override
Definition: nonlinear/PriorFactor.h:59
gtsam::PriorFactor::T
VALUE T
Definition: nonlinear/PriorFactor.h:33
gtsam::PriorFactor::shared_ptr
std::shared_ptr< PriorFactor< VALUE > > shared_ptr
shorthand for a smart pointer to a factor
Definition: nonlinear/PriorFactor.h:51
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Factor
Definition: Factor.h:70
gtsam::PriorFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: nonlinear/PriorFactor.h:79
gtsam::NoiseModelFactor::noiseModel_
SharedNoiseModel noiseModel_
Definition: NonlinearFactor.h:205
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::PriorFactor::PriorFactor
PriorFactor(Key key, const VALUE &prior, const Matrix &covariance)
Definition: nonlinear/PriorFactor.h:67
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
Eigen::internal::VALUE
@ VALUE
Definition: SpecialFunctionsImpl.h:729
gtsam::PriorFactor::This
PriorFactor< VALUE > This
Typedef to this class.
Definition: nonlinear/PriorFactor.h:54
gtsam::PriorFactor::PriorFactor
PriorFactor()
Definition: nonlinear/PriorFactor.h:57
gtsam::PriorFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: nonlinear/PriorFactor.h:72
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:245
gtsam::KeyFormatter
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
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:431
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::PriorFactor::prior_
VALUE prior_
Definition: nonlinear/PriorFactor.h:43
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: NonlinearFactor.cpp:80
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
NonlinearFactor.h
Non-linear factor base classes.
gtsam
traits
Definition: SFMdata.h:40
gtsam::PriorFactor::evaluateError
Vector evaluateError(const T &x, OptionalMatrixType H) const override
Definition: nonlinear/PriorFactor.h:98
gtsam::Testable
Definition: Testable.h:152
gtsam::traits
Definition: Group.h:36
gtsam::PriorFactor::PriorFactor
PriorFactor(Key key, const VALUE &prior, const SharedNoiseModel &model=nullptr)
Definition: nonlinear/PriorFactor.h:62
gtsam::PriorFactor::Base
NoiseModelFactorN< VALUE > Base
Definition: nonlinear/PriorFactor.h:41
gtsam::PriorFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
Definition: nonlinear/PriorFactor.h:90
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
GTSAM_CONCEPT_TESTABLE_TYPE
#define GTSAM_CONCEPT_TESTABLE_TYPE(T)
Definition: Testable.h:177
gtsam::Print
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:65
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
Definition: types.h:288
gtsam::NoiseModelFactorN< VALUE >::key
Key key() const
Definition: NonlinearFactor.h:582
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::PriorFactor::prior
const VALUE & prior() const
Definition: nonlinear/PriorFactor.h:104


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:25