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  // Compute rotation using Expmap
121  Matrix3 Jr;
122  Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
123 
124  // Compute translations and optionally their Jacobians Q in w
125  // The Jacobians with respect to rho and nu are equal to Jr
126  Matrix3 Qt, Qv;
127  Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr);
128  Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr);
129 
130  if (Hxi) {
131  *Hxi << Jr, Z_3x3, Z_3x3, //
132  Qt, Jr, Z_3x3, //
133  Qv, Z_3x3, Jr;
134  }
135 
136  return NavState(R, t, v);
137 }
138 
139 //------------------------------------------------------------------------------
141  if (Hstate) *Hstate = LogmapDerivative(state);
142 
143  const Vector3 phi = Rot3::Logmap(state.rotation());
144  const Vector3& p = state.position();
145  const Vector3& v = state.velocity();
146  const double t = phi.norm();
147  if (t < 1e-8) {
148  Vector9 log;
149  log << phi, p, v;
150  return log;
151 
152  } else {
153  const Matrix3 W = skewSymmetric(phi / t);
154 
155  const double Tan = tan(0.5 * t);
156  const Vector3 Wp = W * p;
157  const Vector3 Wv = W * v;
158  const Vector3 rho = p - (0.5 * t) * Wp + (1 - t / (2. * Tan)) * (W * Wp);
159  const Vector3 nu = v - (0.5 * t) * Wv + (1 - t / (2. * Tan)) * (W * Wv);
160  Vector9 log;
161  // Order is ω, p, v
162  log << phi, rho, nu;
163  return log;
164  }
165 }
166 
167 //------------------------------------------------------------------------------
169  const Matrix3 R = R_.matrix();
170  Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R;
171  Matrix3 B = skewSymmetric(v_.x(), v_.y(), v_.z()) * R;
172  // Eqn 2 in Barrau20icra
173  Matrix9 adj;
174  adj << R, Z_3x3, Z_3x3, A, R, Z_3x3, B, Z_3x3, R;
175  return adj;
176 }
177 
178 //------------------------------------------------------------------------------
179 Vector9 NavState::Adjoint(const Vector9& xi_b, OptionalJacobian<9, 9> H_state,
180  OptionalJacobian<9, 9> H_xib) const {
181  const Matrix9 Ad = AdjointMap();
182 
183  // Jacobians
184  if (H_state) *H_state = -Ad * adjointMap(xi_b);
185  if (H_xib) *H_xib = Ad;
186 
187  return Ad * xi_b;
188 }
189 
190 //------------------------------------------------------------------------------
191 Matrix9 NavState::adjointMap(const Vector9& xi) {
192  Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
193  Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
194  Matrix3 a_hat = skewSymmetric(xi(6), xi(7), xi(8));
195  Matrix9 adj;
196  adj << w_hat, Z_3x3, Z_3x3, v_hat, w_hat, Z_3x3, a_hat, Z_3x3, w_hat;
197  return adj;
198 }
199 
200 //------------------------------------------------------------------------------
201 Vector9 NavState::adjoint(const Vector9& xi, const Vector9& y,
204  if (Hxi) {
205  Hxi->setZero();
206  for (int i = 0; i < 9; ++i) {
207  Vector9 dxi;
208  dxi.setZero();
209  dxi(i) = 1.0;
210  Matrix9 Gi = adjointMap(dxi);
211  Hxi->col(i) = Gi * y;
212  }
213  }
214 
215  const Matrix9& ad_xi = adjointMap(xi);
216  if (H_y) *H_y = ad_xi;
217 
218  return ad_xi * y;
219 }
220 
221 //------------------------------------------------------------------------------
223  Matrix9 J;
224  Expmap(xi, J);
225  return J;
226 }
227 
228 //------------------------------------------------------------------------------
230  const Vector9 xi = Logmap(state);
231  const Vector3 w = xi.head<3>();
232  Vector3 rho = xi.segment<3>(3);
233  Vector3 nu = xi.tail<3>();
234 
235  Matrix3 Qt, Qv;
236  const Rot3 R = Rot3::Expmap(w);
237  Pose3::ExpmapTranslation(w, rho, Qt);
238  Pose3::ExpmapTranslation(w, nu, Qv);
239  const Matrix3 Jw = Rot3::LogmapDerivative(w);
240  const Matrix3 Qt2 = -Jw * Qt * Jw;
241  const Matrix3 Qv2 = -Jw * Qv * Jw;
242 
243  Matrix9 J;
244  J << Jw, Z_3x3, Z_3x3,
245  Qt2, Jw, Z_3x3,
246  Qv2, Z_3x3, Jw;
247  return J;
248 }
249 
250 
251 //------------------------------------------------------------------------------
253  ChartJacobian Hxi) {
254  return Expmap(xi, Hxi);
255 }
256 
257 //------------------------------------------------------------------------------
259  ChartJacobian Hstate) {
260  return Logmap(state, Hstate);
261 }
262 
263 //------------------------------------------------------------------------------
264 NavState NavState::retract(const Vector9& xi, //
266  Rot3 nRb = R_;
267  Point3 n_t = t_, n_v = v_;
268  Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb;
269  const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0);
270  const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0);
271  const Point3 t = n_t + nRb.rotate(dP(xi), H1 ? &D_t_nRb : 0);
272  const Point3 v = n_v + nRb.rotate(dV(xi), H1 ? &D_v_nRb : 0);
273  if (H1) {
274  *H1 << D_R_nRb, Z_3x3, Z_3x3, //
275  // Note(frank): the derivative of n_t with respect to xi is nRb
276  // We pre-multiply with nRc' to account for NavState::Create
277  // Then we make use of the identity nRc' * nRb = bRc'
278  nRc.transpose() * D_t_nRb, bRc.transpose(), Z_3x3,
279  // Similar reasoning for v:
280  nRc.transpose() * D_v_nRb, Z_3x3, bRc.transpose();
281  }
282  if (H2) {
283  *H2 << D_bRc_xi, Z_3x3, Z_3x3, //
284  Z_3x3, bRc.transpose(), Z_3x3, //
285  Z_3x3, Z_3x3, bRc.transpose();
286  }
287  return NavState(nRc, t, v);
288 }
289 
290 //------------------------------------------------------------------------------
293  Matrix3 D_dR_R, D_dt_R, D_dv_R;
294  const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0);
295  const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0);
296  const Vector dV = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0);
297 
298  Vector9 xi;
299  Matrix3 D_xi_R;
300  xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV;
301  if (H1) {
302  *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
303  D_dt_R, -I_3x3, Z_3x3, //
304  D_dv_R, Z_3x3, -I_3x3;
305  }
306  if (H2) {
307  *H2 << D_xi_R, Z_3x3, Z_3x3, //
308  Z_3x3, dR.matrix(), Z_3x3, //
309  Z_3x3, Z_3x3, dR.matrix();
310  }
311 
312  return xi;
313 }
314 
315 //------------------------------------------------------------------------------
316 // sugar for derivative blocks
317 #define D_R_R(H) (H)->block<3,3>(0,0)
318 #define D_R_t(H) (H)->block<3,3>(0,3)
319 #define D_R_v(H) (H)->block<3,3>(0,6)
320 #define D_t_R(H) (H)->block<3,3>(3,0)
321 #define D_t_t(H) (H)->block<3,3>(3,3)
322 #define D_t_v(H) (H)->block<3,3>(3,6)
323 #define D_v_R(H) (H)->block<3,3>(6,0)
324 #define D_v_t(H) (H)->block<3,3>(6,3)
325 #define D_v_v(H) (H)->block<3,3>(6,6)
326 
327 //------------------------------------------------------------------------------
328 NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
330  OptionalJacobian<9, 3> G2) const {
331 
332  Vector9 xi;
333  Matrix39 D_xiP_state;
334  Vector3 b_v = bodyVelocity(F ? &D_xiP_state : 0);
335  double dt22 = 0.5 * dt * dt;
336 
337  // Integrate on tangent space. TODO(frank): coriolis?
338  dR(xi) << dt * b_omega;
339  dP(xi) << dt * b_v + dt22 * b_acceleration;
340  dV(xi) << dt * b_acceleration;
341 
342  // Bring back to manifold
343  Matrix9 D_newState_xi;
344  NavState newState = retract(xi, F, G1 || G2 ? &D_newState_xi : 0);
345 
346  // Derivative wrt state is computed by retract directly
347  // However, as dP(xi) also depends on state, we need to add that contribution
348  if (F) {
349  F->middleRows<3>(3) += dt * D_t_t(F) * D_xiP_state;
350  }
351  // derivative wrt acceleration
352  if (G1) {
353  // D_newState_dPxi = D_newState_xi.middleCols<3>(3)
354  // D_dPxi_acc = dt22 * I_3x3
355  // D_newState_dVxi = D_newState_xi.rightCols<3>()
356  // D_dVxi_acc = dt * I_3x3
357  // *G2 = D_newState_acc = D_newState_dPxi * D_dPxi_acc + D_newState_dVxi * D_dVxi_acc
358  *G1 = D_newState_xi.middleCols<3>(3) * dt22
359  + D_newState_xi.rightCols<3>() * dt;
360  }
361  // derivative wrt omega
362  if (G2) {
363  // D_newState_dRxi = D_newState_xi.leftCols<3>()
364  // D_dRxi_omega = dt * I_3x3
365  // *G1 = D_newState_omega = D_newState_dRxi * D_dRxi_omega
366  *G2 = D_newState_xi.leftCols<3>() * dt;
367  }
368  return newState;
369 }
370 
371 //------------------------------------------------------------------------------
372 Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
373  OptionalJacobian<9, 9> H) const {
374  Rot3 nRb = R_;
375  Point3 n_t = t_, n_v = v_;
376 
377  const double dt2 = dt * dt;
378  const Vector3 omega_cross_vel = omega.cross(n_v);
379 
380  // Get perturbations in nav frame
381  Vector9 n_xi, xi;
382  Matrix3 D_dR_R, D_dP_R, D_dV_R, D_body_nav;
383  dR(n_xi) << ((-dt) * omega);
384  dP(n_xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper
385  dV(n_xi) << ((-2.0 * dt) * omega_cross_vel);
386  if (secondOrder) {
387  const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t));
388  dP(n_xi) -= (0.5 * dt2) * omega_cross2_t;
389  dV(n_xi) -= dt * omega_cross2_t;
390  }
391 
392  // Transform n_xi into the body frame
393  xi << nRb.unrotate(dR(n_xi), H ? &D_dR_R : 0, H ? &D_body_nav : 0),
394  nRb.unrotate(dP(n_xi), H ? &D_dP_R : 0),
395  nRb.unrotate(dV(n_xi), H ? &D_dV_R : 0);
396 
397  if (H) {
398  H->setZero();
399  const Matrix3 Omega = skewSymmetric(omega);
400  const Matrix3 D_cross_state = Omega * R();
401  H->setZero();
402  D_R_R(H) << D_dR_R;
403  D_t_v(H) << D_body_nav * (-dt2) * D_cross_state;
404  D_t_R(H) << D_dP_R;
405  D_v_v(H) << D_body_nav * (-2.0 * dt) * D_cross_state;
406  D_v_R(H) << D_dV_R;
407  if (secondOrder) {
408  const Matrix3 D_cross2_state = Omega * D_cross_state;
409  D_t_t(H) -= D_body_nav * (0.5 * dt2) * D_cross2_state;
410  D_v_t(H) -= D_body_nav * dt * D_cross2_state;
411  }
412  }
413  return xi;
414 }
415 
416 //------------------------------------------------------------------------------
417 Vector9 NavState::correctPIM(const Vector9& pim, double dt,
418  const Vector3& n_gravity, const std::optional<Vector3>& omegaCoriolis,
419  bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1,
420  OptionalJacobian<9, 9> H2) const {
421  const Rot3& nRb = R_;
422  const Velocity3& n_v = v_; // derivative is Ri !
423  const double dt22 = 0.5 * dt * dt;
424 
425  Vector9 xi;
426  Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv, D_dV_Ri;
427  dR(xi) = dR(pim);
428  dP(xi) = dP(pim)
429  + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri1 : 0, H2 ? &D_dP_nv : 0)
430  + dt22 * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0);
431  dV(xi) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0);
432 
433  if (omegaCoriolis) {
434  xi += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1);
435  }
436 
437  if (H1 || H2) {
438  Matrix3 Ri = nRb.matrix();
439 
440  if (H1) {
441  if (!omegaCoriolis)
442  H1->setZero(); // if coriolis H1 is already initialized
443  D_t_R(H1) += dt * D_dP_Ri1 + dt22 * D_dP_Ri2;
444  D_t_v(H1) += dt * D_dP_nv * Ri;
445  D_v_R(H1) += dt * D_dV_Ri;
446  }
447  if (H2) {
448  H2->setIdentity();
449  }
450  }
451 
452  return xi;
453 }
454 //------------------------------------------------------------------------------
455 
456 }
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:317
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:323
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:201
B
Definition: test_numpy_dtypes.cpp:299
dt
const double dt
Definition: testVelocityConstraint.cpp:15
gtsam::NavState::AdjointMap
Matrix9 AdjointMap() const
Definition: NavState.cpp:168
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:321
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:328
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:400
gtsam::NavState::adjointMap
static Matrix9 adjointMap(const Vector9 &xi)
Definition: NavState.cpp:191
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:258
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:417
biased_x_rotation::omega
const double omega
Definition: testPreintegratedRotation.cpp:32
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:179
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:222
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::Pose3::ExpmapTranslation
static Vector3 ExpmapTranslation(const Vector3 &w, const Vector3 &v, OptionalJacobian< 3, 3 > Q={}, OptionalJacobian< 3, 3 > J={}, double nearZeroThreshold=1e-5)
Definition: Pose3.cpp:261
D_v_v
#define D_v_v(H)
Definition: NavState.cpp:325
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:316
D_v_t
#define D_v_t(H)
Definition: NavState.cpp:324
D_t_v
#define D_t_v(H)
Definition: NavState.cpp:322
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:140
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:264
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:252
gtsam::NavState::localCoordinates
Vector9 localCoordinates(const NavState &g, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
localCoordinates with optional derivatives
Definition: NavState.cpp:291
D_t_R
#define D_t_R(H)
Definition: NavState.cpp:320
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:372
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:229
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


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:12:22