NavState.cpp
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 
20 
21 #include <string>
22 
23 namespace gtsam {
24 
25 //------------------------------------------------------------------------------
26 NavState NavState::Create(const Rot3& R, const Point3& t, const Velocity3& v,
29  if (H1)
30  *H1 << I_3x3, Z_3x3, Z_3x3;
31  if (H2)
32  *H2 << Z_3x3, R.transpose(), Z_3x3;
33  if (H3)
34  *H3 << Z_3x3, Z_3x3, R.transpose();
35  return NavState(R, t, v);
36 }
37 //------------------------------------------------------------------------------
40  if (H1)
41  *H1 << I_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3;
42  if (H2)
43  *H2 << Z_3x3, Z_3x3, pose.rotation().transpose();
44  return NavState(pose, vel);
45 }
46 
47 //------------------------------------------------------------------------------
49  if (H)
50  *H << I_3x3, Z_3x3, Z_3x3;
51  return R_;
52 }
53 
54 //------------------------------------------------------------------------------
56  if (H)
57  *H << Z_3x3, R(), Z_3x3;
58  return t_;
59 }
60 
61 //------------------------------------------------------------------------------
63  if (H)
64  *H << Z_3x3, Z_3x3, R();
65  return v_;
66 }
67 
68 //------------------------------------------------------------------------------
70  const Rot3& nRb = R_;
71  const Vector3& n_v = v_;
72  Matrix3 D_bv_nRb;
73  Vector3 b_v = nRb.unrotate(n_v, H ? &D_bv_nRb : 0);
74  if (H)
75  *H << D_bv_nRb, Z_3x3, I_3x3;
76  return b_v;
77 }
78 
79 //------------------------------------------------------------------------------
80 Matrix5 NavState::matrix() const {
81  Matrix3 R = this->R();
82 
83  Matrix5 T = Matrix5::Identity();
84  T.block<3, 3>(0, 0) = R;
85  T.block<3, 1>(0, 3) = t_;
86  T.block<3, 1>(0, 4) = v_;
87  return T;
88 }
89 
90 //------------------------------------------------------------------------------
91 std::ostream& operator<<(std::ostream& os, const NavState& state) {
92  os << "R: " << state.attitude() << "\n";
93  os << "p: " << state.position().transpose() << "\n";
94  os << "v: " << state.velocity().transpose();
95  return os;
96 }
97 
98 //------------------------------------------------------------------------------
99 void NavState::print(const std::string& s) const {
100  std::cout << (s.empty() ? s : s + " ") << *this << std::endl;
101 }
102 
103 //------------------------------------------------------------------------------
104 bool NavState::equals(const NavState& other, double tol) const {
105  return R_.equals(other.R_, tol) && traits<Point3>::Equals(t_, other.t_, tol)
106  && equal_with_abs_tol(v_, other.v_, tol);
107 }
108 
109 //------------------------------------------------------------------------------
111  Rot3 Rt = R_.inverse();
112  return NavState(Rt, Rt * (-t_), Rt * -(v_));
113 }
114 
115 //------------------------------------------------------------------------------
117  // Get angular velocity w and components rho (for t) and nu (for v) from xi
118  Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>();
119 
120  // Instantiate functor for Dexp-related operations:
121  const bool nearZero = (w.dot(w) <= 1e-5);
122  const so3::DexpFunctor local(w, nearZero);
123 
124  // Compute rotation using Expmap
125 #ifdef GTSAM_USE_QUATERNIONS
127 #else
128  const Rot3 R(local.expmap());
129 #endif
130 
131  // Compute translation and velocity. See Pose3::Expmap
132  Matrix3 H_t_w, H_v_w;
133  const Vector3 t = local.applyLeftJacobian(rho, Hxi ? &H_t_w : nullptr);
134  const Vector3 v = local.applyLeftJacobian(nu, Hxi ? &H_v_w : nullptr);
135 
136  if (Hxi) {
137  const Matrix3 Jr = local.rightJacobian();
138  // We are creating a NavState, so we still need to chain H_t_w and H_v_w
139  // with R^T, the Jacobian of Navstate::Create with respect to both t and v.
140  const Matrix3 M = R.matrix();
141  *Hxi << Jr, Z_3x3, Z_3x3, // Jr here *is* the Jacobian of expmap
142  M.transpose() * H_t_w, Jr, Z_3x3, //
143  M.transpose() * H_v_w, Z_3x3, Jr;
144  // In the last two rows, Jr = R^T * J_l, see Barfoot eq. (8.83).
145  // J_l is the Jacobian of applyLeftJacobian in the second argument.
146  }
147 
148  return NavState(R, t, v);
149 }
150 
151 //------------------------------------------------------------------------------
153  if (Hstate) *Hstate = LogmapDerivative(state);
154 
155  const Vector3 phi = Rot3::Logmap(state.rotation());
156  const Vector3& p = state.position();
157  const Vector3& v = state.velocity();
158  const double t = phi.norm();
159  if (t < 1e-8) {
160  Vector9 log;
161  log << phi, p, v;
162  return log;
163 
164  } else {
165  const Matrix3 W = skewSymmetric(phi / t);
166 
167  const double Tan = tan(0.5 * t);
168  const Vector3 Wp = W * p;
169  const Vector3 Wv = W * v;
170  const Vector3 rho = p - (0.5 * t) * Wp + (1 - t / (2. * Tan)) * (W * Wp);
171  const Vector3 nu = v - (0.5 * t) * Wv + (1 - t / (2. * Tan)) * (W * Wv);
172  Vector9 log;
173  // Order is ω, p, v
174  log << phi, rho, nu;
175  return log;
176  }
177 }
178 
179 //------------------------------------------------------------------------------
181  const Matrix3 R = R_.matrix();
182  Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R;
183  Matrix3 B = skewSymmetric(v_.x(), v_.y(), v_.z()) * R;
184  // Eqn 2 in Barrau20icra
185  Matrix9 adj;
186  adj << R, Z_3x3, Z_3x3, A, R, Z_3x3, B, Z_3x3, R;
187  return adj;
188 }
189 
190 //------------------------------------------------------------------------------
191 Vector9 NavState::Adjoint(const Vector9& xi_b, OptionalJacobian<9, 9> H_state,
192  OptionalJacobian<9, 9> H_xib) const {
193  const Matrix9 Ad = AdjointMap();
194 
195  // Jacobians
196  if (H_state) *H_state = -Ad * adjointMap(xi_b);
197  if (H_xib) *H_xib = Ad;
198 
199  return Ad * xi_b;
200 }
201 
202 //------------------------------------------------------------------------------
203 Matrix9 NavState::adjointMap(const Vector9& xi) {
204  Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
205  Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
206  Matrix3 a_hat = skewSymmetric(xi(6), xi(7), xi(8));
207  Matrix9 adj;
208  adj << w_hat, Z_3x3, Z_3x3, v_hat, w_hat, Z_3x3, a_hat, Z_3x3, w_hat;
209  return adj;
210 }
211 
212 //------------------------------------------------------------------------------
213 Vector9 NavState::adjoint(const Vector9& xi, const Vector9& y,
216  if (Hxi) {
217  Hxi->setZero();
218  for (int i = 0; i < 9; ++i) {
219  Vector9 dxi;
220  dxi.setZero();
221  dxi(i) = 1.0;
222  Matrix9 Gi = adjointMap(dxi);
223  Hxi->col(i) = Gi * y;
224  }
225  }
226 
227  const Matrix9& ad_xi = adjointMap(xi);
228  if (H_y) *H_y = ad_xi;
229 
230  return ad_xi * y;
231 }
232 
233 //------------------------------------------------------------------------------
235  Matrix9 J;
236  Expmap(xi, J);
237  return J;
238 }
239 
240 //------------------------------------------------------------------------------
242  const Vector9 xi = Logmap(state);
243  const Vector3 w = xi.head<3>();
244  Vector3 rho = xi.segment<3>(3);
245  Vector3 nu = xi.tail<3>();
246 
247  // Instantiate functor for Dexp-related operations:
248  const bool nearZero = (w.dot(w) <= 1e-5);
249  const so3::DexpFunctor local(w, nearZero);
250 
251  // Call applyLeftJacobian to get its Jacobians
252  Matrix3 H_t_w, H_v_w;
253  local.applyLeftJacobian(rho, H_t_w);
254  local.applyLeftJacobian(nu, H_v_w);
255 
256  // Multiply with R^T to account for NavState::Create Jacobian.
257  const Matrix3 R = local.expmap();
258  const Matrix3 Qt = R.transpose() * H_t_w;
259  const Matrix3 Qv = R.transpose() * H_v_w;
260 
261  // Now compute the blocks of the LogmapDerivative Jacobian
262  const Matrix3 Jw = Rot3::LogmapDerivative(w);
263  const Matrix3 Qt2 = -Jw * Qt * Jw;
264  const Matrix3 Qv2 = -Jw * Qv * Jw;
265 
266  Matrix9 J;
267  J << Jw, Z_3x3, Z_3x3,
268  Qt2, Jw, Z_3x3,
269  Qv2, Z_3x3, Jw;
270  return J;
271 }
272 
273 //------------------------------------------------------------------------------
275  ChartJacobian Hxi) {
276  return Expmap(xi, Hxi);
277 }
278 
279 //------------------------------------------------------------------------------
281  ChartJacobian Hstate) {
282  return Logmap(state, Hstate);
283 }
284 
285 //------------------------------------------------------------------------------
286 NavState NavState::retract(const Vector9& xi, //
288  Rot3 nRb = R_;
289  Point3 n_t = t_, n_v = v_;
290  Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb;
291  const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0);
292  const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0);
293  const Point3 t = n_t + nRb.rotate(dP(xi), H1 ? &D_t_nRb : 0);
294  const Point3 v = n_v + nRb.rotate(dV(xi), H1 ? &D_v_nRb : 0);
295  if (H1) {
296  *H1 << D_R_nRb, Z_3x3, Z_3x3, //
297  // Note(frank): the derivative of n_t with respect to xi is nRb
298  // We pre-multiply with nRc' to account for NavState::Create
299  // Then we make use of the identity nRc' * nRb = bRc'
300  nRc.transpose() * D_t_nRb, bRc.transpose(), Z_3x3,
301  // Similar reasoning for v:
302  nRc.transpose() * D_v_nRb, Z_3x3, bRc.transpose();
303  }
304  if (H2) {
305  *H2 << D_bRc_xi, Z_3x3, Z_3x3, //
306  Z_3x3, bRc.transpose(), Z_3x3, //
307  Z_3x3, Z_3x3, bRc.transpose();
308  }
309  return NavState(nRc, t, v);
310 }
311 
312 //------------------------------------------------------------------------------
315  Matrix3 D_dR_R, D_dt_R, D_dv_R;
316  const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0);
317  const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0);
318  const Vector dV = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0);
319 
320  Vector9 xi;
321  Matrix3 D_xi_R;
322  xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV;
323  if (H1) {
324  *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
325  D_dt_R, -I_3x3, Z_3x3, //
326  D_dv_R, Z_3x3, -I_3x3;
327  }
328  if (H2) {
329  *H2 << D_xi_R, Z_3x3, Z_3x3, //
330  Z_3x3, dR.matrix(), Z_3x3, //
331  Z_3x3, Z_3x3, dR.matrix();
332  }
333 
334  return xi;
335 }
336 
337 //------------------------------------------------------------------------------
338 // sugar for derivative blocks
339 #define D_R_R(H) (H)->block<3,3>(0,0)
340 #define D_R_t(H) (H)->block<3,3>(0,3)
341 #define D_R_v(H) (H)->block<3,3>(0,6)
342 #define D_t_R(H) (H)->block<3,3>(3,0)
343 #define D_t_t(H) (H)->block<3,3>(3,3)
344 #define D_t_v(H) (H)->block<3,3>(3,6)
345 #define D_v_R(H) (H)->block<3,3>(6,0)
346 #define D_v_t(H) (H)->block<3,3>(6,3)
347 #define D_v_v(H) (H)->block<3,3>(6,6)
348 
349 //------------------------------------------------------------------------------
350 NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
352  OptionalJacobian<9, 3> G2) const {
353 
354  Vector9 xi;
355  Matrix39 D_xiP_state;
356  Vector3 b_v = bodyVelocity(F ? &D_xiP_state : 0);
357  double dt22 = 0.5 * dt * dt;
358 
359  // Integrate on tangent space. TODO(frank): coriolis?
360  dR(xi) << dt * b_omega;
361  dP(xi) << dt * b_v + dt22 * b_acceleration;
362  dV(xi) << dt * b_acceleration;
363 
364  // Bring back to manifold
365  Matrix9 D_newState_xi;
366  NavState newState = retract(xi, F, G1 || G2 ? &D_newState_xi : 0);
367 
368  // Derivative wrt state is computed by retract directly
369  // However, as dP(xi) also depends on state, we need to add that contribution
370  if (F) {
371  F->middleRows<3>(3) += dt * D_t_t(F) * D_xiP_state;
372  }
373  // derivative wrt acceleration
374  if (G1) {
375  // D_newState_dPxi = D_newState_xi.middleCols<3>(3)
376  // D_dPxi_acc = dt22 * I_3x3
377  // D_newState_dVxi = D_newState_xi.rightCols<3>()
378  // D_dVxi_acc = dt * I_3x3
379  // *G2 = D_newState_acc = D_newState_dPxi * D_dPxi_acc + D_newState_dVxi * D_dVxi_acc
380  *G1 = D_newState_xi.middleCols<3>(3) * dt22
381  + D_newState_xi.rightCols<3>() * dt;
382  }
383  // derivative wrt omega
384  if (G2) {
385  // D_newState_dRxi = D_newState_xi.leftCols<3>()
386  // D_dRxi_omega = dt * I_3x3
387  // *G1 = D_newState_omega = D_newState_dRxi * D_dRxi_omega
388  *G2 = D_newState_xi.leftCols<3>() * dt;
389  }
390  return newState;
391 }
392 
393 //------------------------------------------------------------------------------
394 Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
395  OptionalJacobian<9, 9> H) const {
396  Rot3 nRb = R_;
397  Point3 n_t = t_, n_v = v_;
398 
399  const double dt2 = dt * dt;
400  const Vector3 omega_cross_vel = omega.cross(n_v);
401 
402  // Get perturbations in nav frame
403  Vector9 n_xi, xi;
404  Matrix3 D_dR_R, D_dP_R, D_dV_R, D_body_nav;
405  dR(n_xi) << ((-dt) * omega);
406  dP(n_xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper
407  dV(n_xi) << ((-2.0 * dt) * omega_cross_vel);
408  if (secondOrder) {
409  const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t));
410  dP(n_xi) -= (0.5 * dt2) * omega_cross2_t;
411  dV(n_xi) -= dt * omega_cross2_t;
412  }
413 
414  // Transform n_xi into the body frame
415  xi << nRb.unrotate(dR(n_xi), H ? &D_dR_R : 0, H ? &D_body_nav : 0),
416  nRb.unrotate(dP(n_xi), H ? &D_dP_R : 0),
417  nRb.unrotate(dV(n_xi), H ? &D_dV_R : 0);
418 
419  if (H) {
420  H->setZero();
421  const Matrix3 Omega = skewSymmetric(omega);
422  const Matrix3 D_cross_state = Omega * R();
423  H->setZero();
424  D_R_R(H) << D_dR_R;
425  D_t_v(H) << D_body_nav * (-dt2) * D_cross_state;
426  D_t_R(H) << D_dP_R;
427  D_v_v(H) << D_body_nav * (-2.0 * dt) * D_cross_state;
428  D_v_R(H) << D_dV_R;
429  if (secondOrder) {
430  const Matrix3 D_cross2_state = Omega * D_cross_state;
431  D_t_t(H) -= D_body_nav * (0.5 * dt2) * D_cross2_state;
432  D_v_t(H) -= D_body_nav * dt * D_cross2_state;
433  }
434  }
435  return xi;
436 }
437 
438 //------------------------------------------------------------------------------
439 Vector9 NavState::correctPIM(const Vector9& pim, double dt,
440  const Vector3& n_gravity, const std::optional<Vector3>& omegaCoriolis,
441  bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1,
442  OptionalJacobian<9, 9> H2) const {
443  const Rot3& nRb = R_;
444  const Velocity3& n_v = v_; // derivative is Ri !
445  const double dt22 = 0.5 * dt * dt;
446 
447  Vector9 xi;
448  Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv, D_dV_Ri;
449  dR(xi) = dR(pim);
450  dP(xi) = dP(pim)
451  + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri1 : 0, H2 ? &D_dP_nv : 0)
452  + dt22 * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0);
453  dV(xi) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0);
454 
455  if (omegaCoriolis) {
456  xi += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1);
457  }
458 
459  if (H1 || H2) {
460  Matrix3 Ri = nRb.matrix();
461 
462  if (H1) {
463  if (!omegaCoriolis)
464  H1->setZero(); // if coriolis H1 is already initialized
465  D_t_R(H1) += dt * D_dP_Ri1 + dt22 * D_dP_Ri2;
466  D_t_v(H1) += dt * D_dP_nv * Ri;
467  D_v_R(H1) += dt * D_dV_Ri;
468  }
469  if (H2) {
470  H2->setIdentity();
471  }
472  }
473 
474  return xi;
475 }
476 //------------------------------------------------------------------------------
477 
478 }
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:52
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
gtsam::NavState::dR
static Eigen::Block< Vector9, 3, 1 > dR(Vector9 &v)
Definition: NavState.h:163
D_R_R
#define D_R_R(H)
Definition: NavState.cpp:339
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
D_v_R
#define D_v_R(H)
Definition: NavState.cpp:345
gtsam::NavState::t_
Point3 t_
position n_t, in nav frame
Definition: NavState.h:44
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
gtsam::Velocity3
Vector3 Velocity3
Velocity is currently typedef'd to Vector3.
Definition: NavState.h:28
gtsam::NavState::NavState
NavState()
Default constructor.
Definition: NavState.h:56
gtsam::NavState::dP
static Eigen::Block< Vector9, 3, 1 > dP(Vector9 &v)
Definition: NavState.h:166
gtsam::NavState::adjoint
static Vector9 adjoint(const Vector9 &xi, const Vector9 &y, OptionalJacobian< 9, 9 > Hxi={}, OptionalJacobian< 9, 9 > H_y={})
Definition: NavState.cpp:213
B
Definition: test_numpy_dtypes.cpp:299
dt
const double dt
Definition: testVelocityConstraint.cpp:15
gtsam::NavState::AdjointMap
Matrix9 AdjointMap() const
Definition: NavState.cpp:180
vel
static const Velocity3 vel(0.4, 0.5, 0.6)
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
gtsam::NavState
Definition: NavState.h:38
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
log
const EIGEN_DEVICE_FUNC LogReturnType log() const
Definition: ArrayCwiseUnaryOps.h:128
gtsam::NavState::bodyVelocity
Velocity3 bodyVelocity(OptionalJacobian< 3, 9 > H={}) const
Definition: NavState.cpp:69
initial::nRb
const Rot3 nRb
Definition: testScenarioRunner.cpp:149
D_t_t
#define D_t_t(H)
Definition: NavState.cpp:343
gtsam::NavState::update
NavState update(const Vector3 &b_acceleration, const Vector3 &b_omega, const double dt, OptionalJacobian< 9, 9 > F={}, OptionalJacobian< 9, 3 > G1={}, OptionalJacobian< 9, 3 > G2={}) const
Definition: NavState.cpp:350
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
gtsam::NavState::v
const Vector3 & v() const
Return velocity as Vector3. Computation-free.
Definition: NavState.h:111
os
ofstream os("timeSchurFactors.csv")
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::skewSymmetric
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:394
gtsam::NavState::adjointMap
static Matrix9 adjointMap(const Vector9 &xi)
Definition: NavState.cpp:203
gtsam::Rot3::LogmapDerivative
static Matrix3 LogmapDerivative(const Vector3 &x)
Derivative of Logmap.
Definition: Rot3.cpp:242
gtsam::Rot3::unrotate
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
rotate point from world to rotated frame
Definition: Rot3.cpp:136
gtsam::NavState::ChartAtOrigin::Local
static Vector9 Local(const NavState &state, ChartJacobian Hstate={})
Definition: NavState.cpp:280
gtsam::NavState::correctPIM
Vector9 correctPIM(const Vector9 &pim, double dt, const Vector3 &n_gravity, const std::optional< Vector3 > &omegaCoriolis, bool use2ndOrderCoriolis=false, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
Definition: NavState.cpp:439
biased_x_rotation::omega
const double omega
Definition: testPreintegratedRotation.cpp:32
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
J
JacobiRotation< float > J
Definition: Jacobi_makeJacobi.cpp:3
A
Definition: test_numpy_dtypes.cpp:298
gtsam::NavState::attitude
const Rot3 & attitude(OptionalJacobian< 3, 9 > H={}) const
Definition: NavState.cpp:48
gtsam::NavState::Adjoint
Vector9 Adjoint(const Vector9 &xi_b, OptionalJacobian< 9, 9 > H_this={}, OptionalJacobian< 9, 9 > H_xib={}) const
Definition: NavState.cpp:191
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
NavState.h
Navigation state composing of attitude, position, and velocity.
gtsam::NavState::R_
Rot3 R_
Rotation nRb, rotates points/velocities in body to points/velocities in nav.
Definition: NavState.h:43
gtsam::NavState::v_
Velocity3 v_
velocity n_v in nav frame
Definition: NavState.h:45
gtsam::NavState::t
Vector3 t() const
Return position as Vector3.
Definition: NavState.h:107
gtsam::NavState::Expmap
static NavState Expmap(const Vector9 &xi, OptionalJacobian< 9, 9 > Hxi={})
Definition: NavState.cpp:116
gtsam::NavState::R
Matrix3 R() const
Return rotation matrix. Induces computation in quaternion mode.
Definition: NavState.h:99
Eigen::Triplet< double >
tan
const EIGEN_DEVICE_FUNC TanReturnType tan() const
Definition: ArrayCwiseUnaryOps.h:269
gtsam::symbol_shorthand::F
Key F(std::uint64_t j)
Definition: inference/Symbol.h:153
gtsam::NavState::ExpmapDerivative
static Matrix9 ExpmapDerivative(const Vector9 &xi)
Derivative of Expmap.
Definition: NavState.cpp:234
gtsam::NavState::Create
static NavState Create(const Rot3 &R, const Point3 &t, const Velocity3 &v, OptionalJacobian< 9, 3 > H1={}, OptionalJacobian< 9, 3 > H2={}, OptionalJacobian< 9, 3 > H3={})
Named constructor with derivatives.
Definition: NavState.cpp:26
gtsam::Rot3::inverse
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:312
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
gtsam::NavState::dV
static Eigen::Block< Vector9, 3, 1 > dV(Vector9 &v)
Definition: NavState.h:169
y
Scalar * y
Definition: level1_cplx_impl.h:124
gtsam::NavState::matrix
Matrix5 matrix() const
Definition: NavState.cpp:80
gtsam::so3::DexpFunctor
Definition: SO3.h:162
D_v_v
#define D_v_v(H)
Definition: NavState.cpp:347
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:322
D_v_t
#define D_v_t(H)
Definition: NavState.cpp:346
D_t_v
#define D_t_v(H)
Definition: NavState.cpp:344
gtsam
traits
Definition: SFMdata.h:40
gtsam::traits
Definition: Group.h:36
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::NavState::equals
bool equals(const NavState &other, double tol=1e-8) const
equals
Definition: NavState.cpp:104
gtsam::symbol_shorthand::W
Key W(std::uint64_t j)
Definition: inference/Symbol.h:170
gtsam::NavState::position
const Point3 & position(OptionalJacobian< 3, 9 > H={}) const
Definition: NavState.cpp:55
gtsam::NavState::Logmap
static Vector9 Logmap(const NavState &pose, OptionalJacobian< 9, 9 > Hpose={})
Definition: NavState.cpp:152
gtsam::Rot3::Expmap
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
Definition: Rot3.h:378
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::Rot3::transpose
Matrix3 transpose() const
Definition: Rot3M.cpp:143
gtsam::NavState::velocity
const Velocity3 & velocity(OptionalJacobian< 3, 9 > H={}) const
Definition: NavState.cpp:62
gtsam::Rot3::matrix
Matrix3 matrix() const
Definition: Rot3M.cpp:218
gtsam::NavState::inverse
NavState inverse() const
inverse transformation with derivatives
Definition: NavState.cpp:110
gtsam::NavState::retract
NavState retract(const Vector9 &v, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
retract with optional derivatives
Definition: NavState.cpp:286
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::Rot3::Logmap
static Vector3 Logmap(const Rot3 &R, OptionalJacobian< 3, 3 > H={})
Definition: Rot3M.cpp:157
gtsam::NavState::print
void print(const std::string &s="") const
print
Definition: NavState.cpp:99
Eigen::Matrix< double, 9, 9 >
gtsam.examples.ImuFactorISAM2Example.n_gravity
def n_gravity
Definition: ImuFactorISAM2Example.py:31
gtsam::NavState::ChartAtOrigin::Retract
static NavState Retract(const Vector9 &xi, ChartJacobian Hxi={})
Definition: NavState.cpp:274
gtsam::NavState::localCoordinates
Vector9 localCoordinates(const NavState &g, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
localCoordinates with optional derivatives
Definition: NavState.cpp:313
D_t_R
#define D_t_R(H)
Definition: NavState.cpp:342
gtsam::NavState::pose
const Pose3 pose() const
Definition: NavState.h:90
gtsam::NavState::coriolis
Vector9 coriolis(double dt, const Vector3 &omega, bool secondOrder=false, OptionalJacobian< 9, 9 > H={}) const
Compute tangent space contribution due to Coriolis forces.
Definition: NavState.cpp:394
align_3::t
Point2 t(10, 10)
gtsam::Rot3::equals
bool equals(const Rot3 &p, double tol=1e-9) const
Definition: Rot3.cpp:100
gtsam::NavState::LogmapDerivative
static Matrix9 LogmapDerivative(const NavState &xi)
Derivative of Logmap.
Definition: NavState.cpp:241
gtsam::NavState::FromPoseVelocity
static NavState FromPoseVelocity(const Pose3 &pose, const Vector3 &vel, OptionalJacobian< 9, 6 > H1={}, OptionalJacobian< 9, 3 > H2={})
Named constructor with derivatives.
Definition: NavState.cpp:38
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
R
Rot2 R(Rot2::fromAngle(0.1))
make_changelog.state
state
Definition: make_changelog.py:29
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


gtsam
Author(s):
autogenerated on Sun Feb 16 2025 04:02:13