gtsam  4.1.0
gtsam
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 NoiseModelFactor2<VALUE, VALUE> {
46 
47 private:
48 
51 
52  double dt_;
53  Vector tau_;
54 
55 public:
56 
57  // shorthand for a smart pointer to a factor
58  typedef typename boost::shared_ptr<GaussMarkov1stOrderFactor> shared_ptr;
59 
62 
64  GaussMarkov1stOrderFactor(const Key& key1, const Key& key2, double delta_t, Vector tau,
65  const SharedGaussian& model) :
66  Base(calcDiscreteNoiseModel(model, delta_t), key1, key2), dt_(delta_t), tau_(tau) {
67  }
68 
69  virtual ~GaussMarkov1stOrderFactor() {}
70 
74  void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
75  std::cout << s << "GaussMarkov1stOrderFactor("
76  << keyFormatter(this->key1()) << ","
77  << keyFormatter(this->key2()) << ")\n";
78  this->noiseModel_->print(" noise model");
79  }
80 
82  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
83  const This *e = dynamic_cast<const This*> (&expected);
84  return e != nullptr && Base::equals(*e, tol);
85  }
86 
90  Vector evaluateError(const VALUE& p1, const VALUE& p2,
91  boost::optional<Matrix&> H1 = boost::none,
92  boost::optional<Matrix&> H2 = boost::none) const override {
93 
94  Vector v1( traits<VALUE>::Logmap(p1) );
95  Vector v2( traits<VALUE>::Logmap(p2) );
96 
97  Vector alpha(tau_.size());
98  Vector alpha_v1(tau_.size());
99  for(int i=0; i<tau_.size(); i++){
100  alpha(i) = exp(- 1/tau_(i)*dt_ );
101  alpha_v1(i) = alpha(i) * v1(i);
102  }
103 
104  Vector hx(v2 - alpha_v1);
105 
106  if(H1) *H1 = -1 * alpha.asDiagonal();
107  if(H2) *H2 = Matrix::Identity(v2.size(),v2.size());
108 
109  return hx;
110  }
111 
112 private:
113 
116  template<class ARCHIVE>
117  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
118  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
119  ar & BOOST_SERIALIZATION_NVP(dt_);
120  ar & BOOST_SERIALIZATION_NVP(tau_);
121  }
122 
123  SharedGaussian calcDiscreteNoiseModel(const SharedGaussian& model, double delta_t){
124  /* Q_d (approx)= Q * delta_t */
125  /* In practice, square root of the information matrix is represented, so that:
126  * R_d (approx)= R / sqrt(delta_t)
127  * */
128  noiseModel::Gaussian::shared_ptr gaussian_model = boost::dynamic_pointer_cast<noiseModel::Gaussian>(model);
129  SharedGaussian model_d(noiseModel::Gaussian::SqrtInformation(gaussian_model->R()/sqrt(delta_t)));
130  return model_d;
131  }
132 
133 }; // \class GaussMarkov1stOrderFactor
134 
136 template<class VALUE> struct traits<GaussMarkov1stOrderFactor<VALUE> > :
137  public Testable<GaussMarkov1stOrderFactor<VALUE> > {
138 };
139 
140 }
gtsam::GaussMarkov1stOrderFactor::evaluateError
Vector evaluateError(const VALUE &p1, const VALUE &p2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
implement functions needed to derive from Factor
Definition: GaussMarkov1stOrderFactor.h:90
Testable.h
Concept check for values that can be used in unit tests.
gtsam::GaussMarkov1stOrderFactor::GaussMarkov1stOrderFactor
GaussMarkov1stOrderFactor()
default constructor - only use for serialization
Definition: GaussMarkov1stOrderFactor.h:61
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::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::NoiseModelFactor2< VALUE, VALUE >::key1
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:387
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::GaussMarkov1stOrderFactor::GaussMarkov1stOrderFactor
GaussMarkov1stOrderFactor(const Key &key1, const Key &key2, double delta_t, Vector tau, const SharedGaussian &model)
Constructor.
Definition: GaussMarkov1stOrderFactor.h:64
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
gtsam::GaussMarkov1stOrderFactor
Definition: GaussMarkov1stOrderFactor.h:45
NonlinearFactor.h
Non-linear factor base classes.
gtsam::GaussMarkov1stOrderFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
equals
Definition: GaussMarkov1stOrderFactor.h:82
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::GaussMarkov1stOrderFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
implement functions needed for Testable
Definition: GaussMarkov1stOrderFactor.h:74
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
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
gtsam::noiseModel::Gaussian::SqrtInformation
static shared_ptr SqrtInformation(const Matrix &R, bool smart=true)
A Gaussian noise model created by specifying a square root information matrix.
Definition: NoiseModel.cpp:85
NoiseModel.h
gtsam::GaussMarkov1stOrderFactor::access
friend class boost::serialization::access
Serialization function.
Definition: GaussMarkov1stOrderFactor.h:115
Lie.h
Base class and basic functions for Lie types.