1/*
2 * @file SimpleHelicopter.h
3 * @brief Implement SimpleHelicopter discrete dynamics model and variational integrator,
4 * following [Kobilarov09siggraph]
5 * @author Duy-Nguyen Ta
6 */
7
8#pragma once
9
10#include <gtsam/nonlinear/NonlinearFactor.h>
11#include <gtsam/geometry/Pose3.h>
12#include <gtsam/base/numericalDerivative.h>
13#include <cmath>
14
15namespace gtsam {
16
17/**
18 * Implement the Reconstruction equation: \f$ g_{k+1} = g_k \exp (h\xi_k) \f$, where
19 * \f$ h \f$: timestep (parameter)
20 * \f$ g_{k+1}, g_{k} \f$: poses at the current and the next timestep
21 * \f$ \xi_k \f$: the body-fixed velocity (Lie algebra)
22 * It is somewhat similar to BetweenFactor, but treats the body-fixed velocity
23 * \f$ \xi_k \f$ as a variable. So it is a three-way factor.
24 * Note: this factor is necessary if one needs to smooth the entire graph. It's not needed
25 * in sequential update method.
26 */
27class Reconstruction : public NoiseModelFactorN<Pose3, Pose3, Vector6> {
28
29 double h_; // time step
30 typedef NoiseModelFactorN<Pose3, Pose3, Vector6> Base;
31public:
32
33 // Provide access to the Matrix& version of evaluateError:
34 using Base::evaluateError;
35
36 Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) :
37 Base(noiseModel::Constrained::All(dim: 6, mu: std::abs(x: mu)), gKey1, gKey,
38 xiKey), h_(h) {
39 }
40 ~Reconstruction() override {}
41
42 /// @return a deep copy of this factor
43 gtsam::NonlinearFactor::shared_ptr clone() const override {
44 return std::static_pointer_cast<gtsam::NonlinearFactor>(
45 r: gtsam::NonlinearFactor::shared_ptr(new Reconstruction(*this))); }
46
47 /** \f$ log((g_k\exp(h\xi_k))^{-1}g_{k+1}) = 0 \f$, with optional derivatives */
48 Vector evaluateError(const Pose3& gk1, const Pose3& gk, const Vector6& xik,
49 OptionalMatrixType H1, OptionalMatrixType H2,
50 OptionalMatrixType H3) const override {
51
52 Matrix6 D_exphxi_xi;
53 Pose3 exphxi = Pose3::Expmap(xi: h_ * xik, Hxi: H3 ? &D_exphxi_xi : 0);
54
55 Matrix6 D_gkxi_gk, D_gkxi_exphxi;
56 Pose3 gkxi = gk.compose(g: exphxi, H1: D_gkxi_gk, H2: H3 ? &D_gkxi_exphxi : 0);
57
58 Matrix6 D_hx_gk1, D_hx_gkxi;
59 Pose3 hx = gkxi.between(g: gk1, H1: (H2 || H3) ? &D_hx_gkxi : 0, H2: H1 ? &D_hx_gk1 : 0);
60
61 Matrix6 D_log_hx;
62 Vector error = Pose3::Logmap(pose: hx, Hpose: D_log_hx);
63
64 if (H1) *H1 = D_log_hx * D_hx_gk1;
65 if (H2 || H3) {
66 Matrix6 D_log_gkxi = D_log_hx * D_hx_gkxi;
67 if (H2) *H2 = D_log_gkxi * D_gkxi_gk;
68 if (H3) *H3 = D_log_gkxi * D_gkxi_exphxi * D_exphxi_xi * h_;
69 }
70
71 return error;
72 }
73
74};
75
76/**
77 * Implement the Discrete Euler-Poincare' equation:
78 */
79class DiscreteEulerPoincareHelicopter : public NoiseModelFactorN<Vector6, Vector6, Pose3> {
80
81 double h_; /// time step
82 Matrix Inertia_; /// Inertia tensors Inertia = [ J 0; 0 M ]
83 Vector Fu_; /// F is the 6xc Control matrix, where c is the number of control variables uk, which directly change the vehicle pose (e.g., gas/brake/speed)
84 /// F(.) is actually a function of the shape variables, which do not change the pose, but affect the vehicle's shape, e.g. steering wheel.
85 /// Fu_ encodes everything we need to know about the vehicle's dynamics.
86 double m_; /// mass. For gravity external force f_ext, which has a fixed formula in this case.
87
88 // TODO: Fk_ and f_ext should be generalized as functions (factor nodes) on control signals and poses/velocities.
89 // This might be needed in control or system identification problems.
90 // We treat them as constant here, since the control inputs are to specify.
91
92 typedef NoiseModelFactorN<Vector6, Vector6, Pose3> Base;
93
94public:
95
96 // Provide access to the Matrix& version of evaluateError:
97 using Base::evaluateError;
98
99 DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey,
100 double h, const Matrix& Inertia, const Vector& Fu, double m,
101 double mu = 1000.0) :
102 Base(noiseModel::Constrained::All(dim: 6, mu: std::abs(x: mu)), xiKey1, xiKey_1, gKey),
103 h_(h), Inertia_(Inertia), Fu_(Fu), m_(m) {
104 }
105 ~DiscreteEulerPoincareHelicopter() override {}
106
107 /// @return a deep copy of this factor
108 gtsam::NonlinearFactor::shared_ptr clone() const override {
109 return std::static_pointer_cast<gtsam::NonlinearFactor>(
110 r: gtsam::NonlinearFactor::shared_ptr(new DiscreteEulerPoincareHelicopter(*this))); }
111
112 /** DEP, with optional derivatives
113 * pk - pk_1 - h_*Fu_ - h_*f_ext = 0
114 * where pk = CT_TLN(h*xi_k)*Inertia*xi_k
115 * pk_1 = CT_TLN(-h*xi_k_1)*Inertia*xi_k_1
116 * */
117 Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk,
118 OptionalMatrixType H1, OptionalMatrixType H2,
119 OptionalMatrixType H3) const override {
120
121 Vector muk = Inertia_*xik;
122 Vector muk_1 = Inertia_*xik_1;
123
124 // Apply the inverse right-trivialized tangent (derivative) map of the exponential map,
125 // using the trapezoidal Lie-Newmark (TLN) scheme, to a vector.
126 // TLN is just a first order approximation of the dExpInv_exp above, detailed in [Kobilarov09siggraph]
127 // C_TLN formula: I6 - 1/2 ad[xi].
128 Matrix D_adjThxik_muk, D_adjThxik1_muk1;
129 Vector pk = muk - 0.5*Pose3::adjointTranspose(xi: h_*xik, y: muk, Hxi: D_adjThxik_muk);
130 Vector pk_1 = muk_1 - 0.5*Pose3::adjointTranspose(xi: -h_*xik_1, y: muk_1, Hxi: D_adjThxik1_muk1);
131
132 Matrix D_gravityBody_gk;
133 Point3 gravityBody = gk.rotation().unrotate(p: Point3(0.0, 0.0, -9.81*m_), H1: D_gravityBody_gk, H2: {});
134 Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()).finished();
135
136 Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext;
137
138 if (H1) {
139 Matrix D_pik_xi = Inertia_-0.5*(h_*D_adjThxik_muk + Pose3::adjointMap(xi: h_*xik).transpose()*Inertia_);
140 *H1 = D_pik_xi;
141 }
142
143 if (H2) {
144 Matrix D_pik1_xik1 = Inertia_-0.5*(-h_*D_adjThxik1_muk1 + Pose3::adjointMap(xi: -h_*xik_1).transpose()*Inertia_);
145 *H2 = -D_pik1_xik1;
146 }
147
148 if (H3) {
149 *H3 = Z_6x6;
150 insertSub(fullMatrix&: *H3, subMatrix: -h_*D_gravityBody_gk, i: 3, j: 0);
151 }
152
153 return hx;
154 }
155
156#if 0
157 Vector computeError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk) const {
158 Vector pk = Pose3::dExpInv_exp(h_*xik).transpose()*Inertia_*xik;
159 Vector pk_1 = Pose3::dExpInv_exp(-h_*xik_1).transpose()*Inertia_*xik_1;
160
161 Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_));
162 Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z());
163
164 Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext;
165
166 return hx;
167 }
168
169 Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk,
170 OptionalMatrixType H1, OptionalMatrixType H2,
171 OptionalMatrixType H3) const {
172 if (H1) {
173 (*H1) = numericalDerivative31(
174 std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
175 std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
176 ),
177 xik, xik_1, gk, 1e-5
178 );
179 }
180 if (H2) {
181 (*H2) = numericalDerivative32(
182 std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
183 std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
184 ),
185 xik, xik_1, gk, 1e-5
186 );
187 }
188 if (H3) {
189 (*H3) = numericalDerivative33(
190 std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
191 std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
192 ),
193 xik, xik_1, gk, 1e-5
194 );
195 }
196
197 return computeError(xik, xik_1, gk);
198 }
199#endif
200
201};
202
203} // namespace gtsam
204