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
12/**
13 * @file GaussMarkov1stOrderFactor.h
14 * @author Vadim Indelman, Stephen Williams, Luca Carlone
15 * @date Jan 17, 2012
16 **/
17#pragma once
18
19#include <gtsam/nonlinear/NonlinearFactor.h>
20#include <gtsam/linear/GaussianFactor.h>
21#include <gtsam/linear/NoiseModel.h>
22#include <gtsam/base/Testable.h>
23#include <gtsam/base/Lie.h>
24
25#include <ostream>
26
27namespace 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 */
44template<class VALUE>
45class GaussMarkov1stOrderFactor: public NoiseModelFactorN<VALUE, VALUE> {
46
47private:
48
49 typedef GaussMarkov1stOrderFactor<VALUE> This;
50 typedef NoiseModelFactorN<VALUE, VALUE> Base;
51
52 double dt_;
53 Vector tau_;
54
55public:
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
63 /** default constructor - only use for serialization */
64 GaussMarkov1stOrderFactor() {}
65
66 /** Constructor */
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
72 ~GaussMarkov1stOrderFactor() override {}
73
74 /** implement functions needed for Testable */
75
76 /** print */
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
84 /** equals */
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
90 /** implement functions needed to derive from Factor */
91
92 /** vector of errors */
93 Vector evaluateError(const VALUE& p1, const VALUE& p2,
94 OptionalMatrixType H1, OptionalMatrixType H2) const override {
95
96 Vector v1( traits<VALUE>::Logmap(p1) );
97 Vector v2( traits<VALUE>::Logmap(p2) );
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(x: - 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(rows: v2.size(),cols: v2.size());
110
111 return hx;
112 }
113
114private:
115
116#if GTSAM_ENABLE_BOOST_SERIALIZATION
117 /** Serialization function */
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
127 SharedGaussian calcDiscreteNoiseModel(const SharedGaussian& model, double delta_t){
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>(r: model);
133 SharedGaussian model_d(noiseModel::Gaussian::SqrtInformation(R: gaussian_model->R()/sqrt(x: delta_t)));
134 return model_d;
135 }
136
137}; // \class GaussMarkov1stOrderFactor
138
139/// traits
140template<class VALUE> struct traits<GaussMarkov1stOrderFactor<VALUE> > :
141 public Testable<GaussMarkov1stOrderFactor<VALUE> > {
142};
143
144} /// namespace gtsam
145