GaussMarkov1stOrderFactor.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 
17 #pragma once
18 
22 #include <gtsam/base/Testable.h>
23 #include <gtsam/base/Lie.h>
24 
25 #include <ostream>
26 
27 namespace gtsam {
28 
29 /*
30  * - The 1st order GaussMarkov factor relates two keys of the same type. This relation is given via
31  * key_2 = exp(-1/tau*delta_t) * key1 + w_d
32  * where tau is the time constant and delta_t is the time difference between the two keys.
33  * w_d is the equivalent discrete noise, whose covariance is calculated from the continuous noise model and delta_t.
34  * - w_d is approximated as a Gaussian noise.
35  * - In the multi-dimensional case, tau is a vector, and the above equation is applied on each element
36  * in the state (represented by keys), using the appropriate time constant in the vector tau.
37  */
38 
39 /*
40  * A class for a measurement predicted by "GaussMarkov1stOrderFactor(config[key1],config[key2])"
41  * KEY1::Value is the Lie Group type
42  * T is the measurement type, by default the same
43  */
44 template<class VALUE>
45 class GaussMarkov1stOrderFactor: public NoiseModelFactorN<VALUE, VALUE> {
46 
47 private:
48 
51 
52  double dt_;
54 
55 public:
56 
57  // Provide access to the Matrix& version of evaluateError:
58  using Base::evaluateError;
59 
60  // shorthand for a smart pointer to a factor
61  typedef typename std::shared_ptr<GaussMarkov1stOrderFactor> shared_ptr;
62 
65 
67  GaussMarkov1stOrderFactor(const Key& key1, const Key& key2, double delta_t, Vector tau,
68  const SharedGaussian& model) :
69  Base(calcDiscreteNoiseModel(model, delta_t), key1, key2), dt_(delta_t), tau_(tau) {
70  }
71 
73 
77  void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
78  std::cout << s << "GaussMarkov1stOrderFactor("
79  << keyFormatter(this->key1()) << ","
80  << keyFormatter(this->key2()) << ")\n";
81  this->noiseModel_->print(" noise model");
82  }
83 
85  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
86  const This *e = dynamic_cast<const This*> (&expected);
87  return e != nullptr && Base::equals(*e, tol);
88  }
89 
93  Vector evaluateError(const VALUE& p1, const VALUE& p2,
94  OptionalMatrixType H1, OptionalMatrixType H2) const override {
95 
98 
99  Vector alpha(tau_.size());
100  Vector alpha_v1(tau_.size());
101  for(int i=0; i<tau_.size(); i++){
102  alpha(i) = exp(- 1/tau_(i)*dt_ );
103  alpha_v1(i) = alpha(i) * v1(i);
104  }
105 
106  Vector hx(v2 - alpha_v1);
107 
108  if(H1) *H1 = -1 * alpha.asDiagonal();
109  if(H2) *H2 = Matrix::Identity(v2.size(),v2.size());
110 
111  return hx;
112  }
113 
114 private:
115 
116 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
117 
118  friend class boost::serialization::access;
119  template<class ARCHIVE>
120  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
121  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
122  ar & BOOST_SERIALIZATION_NVP(dt_);
123  ar & BOOST_SERIALIZATION_NVP(tau_);
124  }
125 #endif
126 
128  /* Q_d (approx)= Q * delta_t */
129  /* In practice, square root of the information matrix is represented, so that:
130  * R_d (approx)= R / sqrt(delta_t)
131  * */
132  noiseModel::Gaussian::shared_ptr gaussian_model = std::dynamic_pointer_cast<noiseModel::Gaussian>(model);
133  SharedGaussian model_d(noiseModel::Gaussian::SqrtInformation(gaussian_model->R()/sqrt(delta_t)));
134  return model_d;
135  }
136 
137 }; // \class GaussMarkov1stOrderFactor
138 
140 template<class VALUE> struct traits<GaussMarkov1stOrderFactor<VALUE> > :
141  public Testable<GaussMarkov1stOrderFactor<VALUE> > {
142 };
143 
144 }
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
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.
gtsam::NoiseModelFactorN< VALUE, VALUE >::evaluateError
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0
gtsam::GaussMarkov1stOrderFactor::calcDiscreteNoiseModel
SharedGaussian calcDiscreteNoiseModel(const SharedGaussian &model, double delta_t)
Definition: GaussMarkov1stOrderFactor.h:127
gtsam::Factor
Definition: Factor.h:70
exp
const EIGEN_DEVICE_FUNC ExpReturnType exp() const
Definition: ArrayCwiseUnaryOps.h:97
gtsam::NoiseModelFactor::noiseModel_
SharedNoiseModel noiseModel_
Definition: NonlinearFactor.h:206
gtsam::noiseModel::Gaussian::shared_ptr
std::shared_ptr< Gaussian > shared_ptr
Definition: NoiseModel.h:191
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::GaussMarkov1stOrderFactor::This
GaussMarkov1stOrderFactor< VALUE > This
Definition: GaussMarkov1stOrderFactor.h:49
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
Eigen::internal::VALUE
@ VALUE
Definition: SpecialFunctionsImpl.h:729
gtsam::GaussMarkov1stOrderFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: GaussMarkov1stOrderFactor.h:77
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:432
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::GaussMarkov1stOrderFactor::shared_ptr
std::shared_ptr< GaussMarkov1stOrderFactor > shared_ptr
Definition: GaussMarkov1stOrderFactor.h:61
gtsam::GaussMarkov1stOrderFactor::tau_
Vector tau_
Definition: GaussMarkov1stOrderFactor.h:53
gtsam::noiseModel::Gaussian::SqrtInformation
static shared_ptr SqrtInformation(const Matrix &R, bool smart=true)
Definition: NoiseModel.cpp:84
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: NonlinearFactor.cpp:82
gtsam::SharedGaussian
noiseModel::Gaussian::shared_ptr SharedGaussian
Definition: NoiseModel.h:763
gtsam::GaussMarkov1stOrderFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
Definition: GaussMarkov1stOrderFactor.h:85
gtsam::NoiseModelFactorN< VALUE, VALUE >::key1
Key key1() const
Definition: NonlinearFactor.h:732
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
gtsam::GaussMarkov1stOrderFactor::GaussMarkov1stOrderFactor
GaussMarkov1stOrderFactor(const Key &key1, const Key &key2, double delta_t, Vector tau, const SharedGaussian &model)
Definition: GaussMarkov1stOrderFactor.h:67
gtsam::GaussMarkov1stOrderFactor::Base
NoiseModelFactorN< VALUE, VALUE > Base
Definition: GaussMarkov1stOrderFactor.h:50
NonlinearFactor.h
Non-linear factor base classes.
gtsam::NoiseModelFactorN< VALUE, VALUE >::key2
Key key2() const
Definition: NonlinearFactor.h:736
gtsam::GaussMarkov1stOrderFactor::evaluateError
Vector evaluateError(const VALUE &p1, const VALUE &p2, OptionalMatrixType H1, OptionalMatrixType H2) const override
Definition: GaussMarkov1stOrderFactor.h:93
Lie.h
Base class and basic functions for Lie types.
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
NoiseModel.h
gtsam::traits
Definition: Group.h:36
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:69
v2
Vector v2
Definition: testSerializationBase.cpp:39
gtsam::GaussMarkov1stOrderFactor::GaussMarkov1stOrderFactor
GaussMarkov1stOrderFactor()
Definition: GaussMarkov1stOrderFactor.h:64
gtsam::GaussMarkov1stOrderFactor::dt_
double dt_
Definition: GaussMarkov1stOrderFactor.h:52
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::GaussMarkov1stOrderFactor::~GaussMarkov1stOrderFactor
~GaussMarkov1stOrderFactor() override
Definition: GaussMarkov1stOrderFactor.h:72
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:56
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
Base
Definition: test_virtual_functions.cpp:156
gtsam::GaussMarkov1stOrderFactor
Definition: GaussMarkov1stOrderFactor.h:45
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
v1
Vector v1
Definition: testSerializationBase.cpp:38


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:34