gtsam  4.1.0
gtsam
SimpleHelicopter.h
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 
11 #include <gtsam/geometry/Pose3.h>
13 #include <cmath>
14 
15 namespace gtsam {
16 
27 class Reconstruction : public NoiseModelFactor3<Pose3, Pose3, Vector6> {
28 
29  double h_; // time step
31 public:
32  Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) :
33  Base(noiseModel::Constrained::All(6, std::abs(mu)), gKey1, gKey,
34  xiKey), h_(h) {
35  }
36  virtual ~Reconstruction() {}
37 
39  gtsam::NonlinearFactor::shared_ptr clone() const override {
40  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
41  gtsam::NonlinearFactor::shared_ptr(new Reconstruction(*this))); }
42 
44  Vector evaluateError(const Pose3& gk1, const Pose3& gk, const Vector6& xik,
45  boost::optional<Matrix&> H1 = boost::none,
46  boost::optional<Matrix&> H2 = boost::none,
47  boost::optional<Matrix&> H3 = boost::none) const override {
48 
49  Matrix6 D_exphxi_xi;
50  Pose3 exphxi = Pose3::Expmap(h_ * xik, H3 ? &D_exphxi_xi : 0);
51 
52  Matrix6 D_gkxi_gk, D_gkxi_exphxi;
53  Pose3 gkxi = gk.compose(exphxi, D_gkxi_gk, H3 ? &D_gkxi_exphxi : 0);
54 
55  Matrix6 D_hx_gk1, D_hx_gkxi;
56  Pose3 hx = gkxi.between(gk1, (H2 || H3) ? &D_hx_gkxi : 0, H1 ? &D_hx_gk1 : 0);
57 
58  Matrix6 D_log_hx;
59  Vector error = Pose3::Logmap(hx, D_log_hx);
60 
61  if (H1) *H1 = D_log_hx * D_hx_gk1;
62  if (H2 || H3) {
63  Matrix6 D_log_gkxi = D_log_hx * D_hx_gkxi;
64  if (H2) *H2 = D_log_gkxi * D_gkxi_gk;
65  if (H3) *H3 = D_log_gkxi * D_gkxi_exphxi * D_exphxi_xi * h_;
66  }
67 
68  return error;
69  }
70 
71 };
72 
76 class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3<Vector6, Vector6, Pose3> {
77 
78  double h_;
79  Matrix Inertia_;
80  Vector Fu_;
81  double m_;
84 
85  // TODO: Fk_ and f_ext should be generalized as functions (factor nodes) on control signals and poses/velocities.
86  // This might be needed in control or system identification problems.
87  // We treat them as constant here, since the control inputs are to specify.
88 
90 
91 public:
92  DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey,
93  double h, const Matrix& Inertia, const Vector& Fu, double m,
94  double mu = 1000.0) :
95  Base(noiseModel::Constrained::All(6, std::abs(mu)), xiKey1, xiKey_1, gKey),
96  h_(h), Inertia_(Inertia), Fu_(Fu), m_(m) {
97  }
99 
101  gtsam::NonlinearFactor::shared_ptr clone() const override {
102  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
103  gtsam::NonlinearFactor::shared_ptr(new DiscreteEulerPoincareHelicopter(*this))); }
104 
110  Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk,
111  boost::optional<Matrix&> H1 = boost::none,
112  boost::optional<Matrix&> H2 = boost::none,
113  boost::optional<Matrix&> H3 = boost::none) const override {
114 
115  Vector muk = Inertia_*xik;
116  Vector muk_1 = Inertia_*xik_1;
117 
118  // Apply the inverse right-trivialized tangent (derivative) map of the exponential map,
119  // using the trapezoidal Lie-Newmark (TLN) scheme, to a vector.
120  // TLN is just a first order approximation of the dExpInv_exp above, detailed in [Kobilarov09siggraph]
121  // C_TLN formula: I6 - 1/2 ad[xi].
122  Matrix D_adjThxik_muk, D_adjThxik1_muk1;
123  Vector pk = muk - 0.5*Pose3::adjointTranspose(h_*xik, muk, D_adjThxik_muk);
124  Vector pk_1 = muk_1 - 0.5*Pose3::adjointTranspose(-h_*xik_1, muk_1, D_adjThxik1_muk1);
125 
126  Matrix D_gravityBody_gk;
127  Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk, boost::none);
128  Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()).finished();
129 
130  Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext;
131 
132  if (H1) {
133  Matrix D_pik_xi = Inertia_-0.5*(h_*D_adjThxik_muk + Pose3::adjointMap(h_*xik).transpose()*Inertia_);
134  *H1 = D_pik_xi;
135  }
136 
137  if (H2) {
138  Matrix D_pik1_xik1 = Inertia_-0.5*(-h_*D_adjThxik1_muk1 + Pose3::adjointMap(-h_*xik_1).transpose()*Inertia_);
139  *H2 = -D_pik1_xik1;
140  }
141 
142  if (H3) {
143  *H3 = Z_6x6;
144  insertSub(*H3, -h_*D_gravityBody_gk, 3, 0);
145  }
146 
147  return hx;
148  }
149 
150 #if 0
151  Vector computeError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk) const {
152  Vector pk = Pose3::dExpInv_exp(h_*xik).transpose()*Inertia_*xik;
153  Vector pk_1 = Pose3::dExpInv_exp(-h_*xik_1).transpose()*Inertia_*xik_1;
154 
155  Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_));
156  Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z());
157 
158  Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext;
159 
160  return hx;
161  }
162 
163  Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk,
164  boost::optional<Matrix&> H1 = boost::none,
165  boost::optional<Matrix&> H2 = boost::none,
166  boost::optional<Matrix&> H3 = boost::none) const {
167  if (H1) {
168  (*H1) = numericalDerivative31(
169  boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
170  boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
171  ),
172  xik, xik_1, gk, 1e-5
173  );
174  }
175  if (H2) {
176  (*H2) = numericalDerivative32(
177  boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
178  boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
179  ),
180  xik, xik_1, gk, 1e-5
181  );
182  }
183  if (H3) {
184  (*H3) = numericalDerivative33(
185  boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
186  boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
187  ),
188  xik, xik_1, gk, 1e-5
189  );
190  }
191 
192  return computeError(xik, xik_1, gk);
193  }
194 #endif
195 
196 };
197 
198 
199 } /* namespace gtsam */
gtsam::insertSub
void insertSub(Eigen::MatrixBase< Derived1 > &fullMatrix, const Eigen::MatrixBase< Derived2 > &subMatrix, size_t i, size_t j)
insert a submatrix IN PLACE at a specified location in a larger matrix NOTE: there is no size checkin...
Definition: Matrix.h:198
gtsam::numericalDerivative33
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative33(boost::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Compute numerical derivative in argument 3 of ternary function.
Definition: numericalDerivative.h:285
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim)
Fully constrained variations.
Definition: NoiseModel.h:467
gtsam::NoiseModelFactor3
A convenient base class for creating your own NoiseModelFactor with 3 variables.
Definition: NonlinearFactor.h:430
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::Pose3::Expmap
static Pose3 Expmap(const Vector6 &xi, OptionalJacobian< 6, 6 > Hxi=boost::none)
Exponential map at identity - create a rotation from canonical coordinates .
Definition: Pose3.cpp:120
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::DiscreteEulerPoincareHelicopter::clone
return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override
Creates a shared_ptr clone of the factor - needs to be specialized to allow for subclasses.
Definition: SimpleHelicopter.h:101
gtsam::Pose3::Logmap
static Vector6 Logmap(const Pose3 &pose, OptionalJacobian< 6, 6 > Hpose=boost::none)
Log map at identity - return the canonical coordinates of this rotation.
Definition: Pose3.cpp:139
gtsam::DiscreteEulerPoincareHelicopter
Implement the Discrete Euler-Poincare' equation:
Definition: SimpleHelicopter.h:76
Pose3.h
3D Pose
gtsam::numericalDerivative32
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative32(boost::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Compute numerical derivative in argument 2 of ternary function.
Definition: numericalDerivative.h:256
gtsam::Pose3::adjointTranspose
static Vector6 adjointTranspose(const Vector6 &xi, const Vector6 &y, OptionalJacobian< 6, 6 > Hxi=boost::none)
The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
Definition: Pose3.cpp:93
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Reconstruction
Implement the Reconstruction equation: , where : timestep (parameter) : poses at the current and the ...
Definition: SimpleHelicopter.h:27
gtsam::Rot3::unrotate
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from world to rotated frame
Definition: Rot3.cpp:136
gtsam::Reconstruction::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: SimpleHelicopter.h:39
NonlinearFactor.h
Non-linear factor base classes.
gtsam::Pose3::adjointMap
static Matrix6 adjointMap(const Vector6 &xi)
FIXME Not tested - marked as incorrect.
Definition: Pose3.cpp:67
gtsam::NoiseModelFactor3< Pose3, Pose3, Vector6 >::evaluateError
virtual Vector evaluateError(const X1 &, const X2 &, const X3 &, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const=0
Override this method to finish implementing a trinary factor.
gtsam::Pose3
Definition: Pose3.h:37
gtsam::DiscreteEulerPoincareHelicopter::evaluateError
Vector evaluateError(const Vector6 &xik, const Vector6 &xik_1, const Pose3 &gk, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override
DEP, with optional derivatives pk - pk_1 - h_*Fu_ - h_*f_ext = 0 where pk = CT_TLN(h*xi_k)*Inertia*xi...
Definition: SimpleHelicopter.h:110
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::numericalDerivative31
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative31(boost::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Compute numerical derivative in argument 1 of ternary function.
Definition: numericalDerivative.h:227
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition: Pose3.cpp:266
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition: NonlinearFactor.cpp:119