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 Matrix7 NavState::matrix() const {
81  Matrix3 R = this->R();
82  Matrix7 T;
83  T << R, Z_3x3, t(), Z_3x3, R, v(), Vector6::Zero().transpose(), 1.0;
84  return T;
85 }
86 
87 //------------------------------------------------------------------------------
88 std::ostream& operator<<(std::ostream& os, const NavState& state) {
89  os << "R: " << state.attitude() << "\n";
90  os << "p: " << state.position().transpose() << "\n";
91  os << "v: " << state.velocity().transpose();
92  return os;
93 }
94 
95 //------------------------------------------------------------------------------
96 void NavState::print(const std::string& s) const {
97  std::cout << (s.empty() ? s : s + " ") << *this << std::endl;
98 }
99 
100 //------------------------------------------------------------------------------
101 bool NavState::equals(const NavState& other, double tol) const {
102  return R_.equals(other.R_, tol) && traits<Point3>::Equals(t_, other.t_, tol)
103  && equal_with_abs_tol(v_, other.v_, tol);
104 }
105 
106 //------------------------------------------------------------------------------
107 NavState NavState::retract(const Vector9& xi, //
109  Rot3 nRb = R_;
110  Point3 n_t = t_, n_v = v_;
111  Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb;
112  const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0);
113  const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0);
114  const Point3 t = n_t + nRb.rotate(dP(xi), H1 ? &D_t_nRb : 0);
115  const Point3 v = n_v + nRb.rotate(dV(xi), H1 ? &D_v_nRb : 0);
116  if (H1) {
117  *H1 << D_R_nRb, Z_3x3, Z_3x3, //
118  // Note(frank): the derivative of n_t with respect to xi is nRb
119  // We pre-multiply with nRc' to account for NavState::Create
120  // Then we make use of the identity nRc' * nRb = bRc'
121  nRc.transpose() * D_t_nRb, bRc.transpose(), Z_3x3,
122  // Similar reasoning for v:
123  nRc.transpose() * D_v_nRb, Z_3x3, bRc.transpose();
124  }
125  if (H2) {
126  *H2 << D_bRc_xi, Z_3x3, Z_3x3, //
127  Z_3x3, bRc.transpose(), Z_3x3, //
128  Z_3x3, Z_3x3, bRc.transpose();
129  }
130  return NavState(nRc, t, v);
131 }
132 
133 //------------------------------------------------------------------------------
136  Matrix3 D_dR_R, D_dt_R, D_dv_R;
137  const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0);
138  const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0);
139  const Vector dV = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0);
140 
141  Vector9 xi;
142  Matrix3 D_xi_R;
143  xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV;
144  if (H1) {
145  *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
146  D_dt_R, -I_3x3, Z_3x3, //
147  D_dv_R, Z_3x3, -I_3x3;
148  }
149  if (H2) {
150  *H2 << D_xi_R, Z_3x3, Z_3x3, //
151  Z_3x3, dR.matrix(), Z_3x3, //
152  Z_3x3, Z_3x3, dR.matrix();
153  }
154  return xi;
155 }
156 
157 //------------------------------------------------------------------------------
158 // sugar for derivative blocks
159 #define D_R_R(H) (H)->block<3,3>(0,0)
160 #define D_R_t(H) (H)->block<3,3>(0,3)
161 #define D_R_v(H) (H)->block<3,3>(0,6)
162 #define D_t_R(H) (H)->block<3,3>(3,0)
163 #define D_t_t(H) (H)->block<3,3>(3,3)
164 #define D_t_v(H) (H)->block<3,3>(3,6)
165 #define D_v_R(H) (H)->block<3,3>(6,0)
166 #define D_v_t(H) (H)->block<3,3>(6,3)
167 #define D_v_v(H) (H)->block<3,3>(6,6)
168 
169 //------------------------------------------------------------------------------
170 NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
172  OptionalJacobian<9, 3> G2) const {
173 
174  Vector9 xi;
175  Matrix39 D_xiP_state;
176  Vector3 b_v = bodyVelocity(F ? &D_xiP_state : 0);
177  double dt22 = 0.5 * dt * dt;
178 
179  // Integrate on tangent space. TODO(frank): coriolis?
180  dR(xi) << dt * b_omega;
181  dP(xi) << dt * b_v + dt22 * b_acceleration;
182  dV(xi) << dt * b_acceleration;
183 
184  // Bring back to manifold
185  Matrix9 D_newState_xi;
186  NavState newState = retract(xi, F, G1 || G2 ? &D_newState_xi : 0);
187 
188  // Derivative wrt state is computed by retract directly
189  // However, as dP(xi) also depends on state, we need to add that contribution
190  if (F) {
191  F->middleRows<3>(3) += dt * D_t_t(F) * D_xiP_state;
192  }
193  // derivative wrt acceleration
194  if (G1) {
195  // D_newState_dPxi = D_newState_xi.middleCols<3>(3)
196  // D_dPxi_acc = dt22 * I_3x3
197  // D_newState_dVxi = D_newState_xi.rightCols<3>()
198  // D_dVxi_acc = dt * I_3x3
199  // *G2 = D_newState_acc = D_newState_dPxi * D_dPxi_acc + D_newState_dVxi * D_dVxi_acc
200  *G1 = D_newState_xi.middleCols<3>(3) * dt22
201  + D_newState_xi.rightCols<3>() * dt;
202  }
203  // derivative wrt omega
204  if (G2) {
205  // D_newState_dRxi = D_newState_xi.leftCols<3>()
206  // D_dRxi_omega = dt * I_3x3
207  // *G1 = D_newState_omega = D_newState_dRxi * D_dRxi_omega
208  *G2 = D_newState_xi.leftCols<3>() * dt;
209  }
210  return newState;
211 }
212 
213 //------------------------------------------------------------------------------
214 Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
215  OptionalJacobian<9, 9> H) const {
216  auto [nRb, n_t, n_v] = (*this);
217 
218  const double dt2 = dt * dt;
219  const Vector3 omega_cross_vel = omega.cross(n_v);
220 
221  // Get perturbations in nav frame
222  Vector9 n_xi, xi;
223  Matrix3 D_dR_R, D_dP_R, D_dV_R, D_body_nav;
224  dR(n_xi) << ((-dt) * omega);
225  dP(n_xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper
226  dV(n_xi) << ((-2.0 * dt) * omega_cross_vel);
227  if (secondOrder) {
228  const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t));
229  dP(n_xi) -= (0.5 * dt2) * omega_cross2_t;
230  dV(n_xi) -= dt * omega_cross2_t;
231  }
232 
233  // Transform n_xi into the body frame
234  xi << nRb.unrotate(dR(n_xi), H ? &D_dR_R : 0, H ? &D_body_nav : 0),
235  nRb.unrotate(dP(n_xi), H ? &D_dP_R : 0),
236  nRb.unrotate(dV(n_xi), H ? &D_dV_R : 0);
237 
238  if (H) {
239  H->setZero();
240  const Matrix3 Omega = skewSymmetric(omega);
241  const Matrix3 D_cross_state = Omega * R();
242  H->setZero();
243  D_R_R(H) << D_dR_R;
244  D_t_v(H) << D_body_nav * (-dt2) * D_cross_state;
245  D_t_R(H) << D_dP_R;
246  D_v_v(H) << D_body_nav * (-2.0 * dt) * D_cross_state;
247  D_v_R(H) << D_dV_R;
248  if (secondOrder) {
249  const Matrix3 D_cross2_state = Omega * D_cross_state;
250  D_t_t(H) -= D_body_nav * (0.5 * dt2) * D_cross2_state;
251  D_v_t(H) -= D_body_nav * dt * D_cross2_state;
252  }
253  }
254  return xi;
255 }
256 
257 //------------------------------------------------------------------------------
258 Vector9 NavState::correctPIM(const Vector9& pim, double dt,
259  const Vector3& n_gravity, const std::optional<Vector3>& omegaCoriolis,
260  bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1,
261  OptionalJacobian<9, 9> H2) const {
262  const Rot3& nRb = R_;
263  const Velocity3& n_v = v_; // derivative is Ri !
264  const double dt22 = 0.5 * dt * dt;
265 
266  Vector9 xi;
267  Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv, D_dV_Ri;
268  dR(xi) = dR(pim);
269  dP(xi) = dP(pim)
270  + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri1 : 0, H2 ? &D_dP_nv : 0)
271  + dt22 * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0);
272  dV(xi) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0);
273 
274  if (omegaCoriolis) {
275  xi += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1);
276  }
277 
278  if (H1 || H2) {
279  Matrix3 Ri = nRb.matrix();
280 
281  if (H1) {
282  if (!omegaCoriolis)
283  H1->setZero(); // if coriolis H1 is already initialized
284  D_t_R(H1) += dt * D_dP_Ri1 + dt22 * D_dP_Ri2;
285  D_t_v(H1) += dt * D_dP_nv * Ri;
286  D_v_R(H1) += dt * D_dV_Ri;
287  }
288  if (H2) {
289  H2->setIdentity();
290  }
291  }
292 
293  return xi;
294 }
295 //------------------------------------------------------------------------------
296 
297 }
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:138
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:170
D_R_R
#define D_R_R(H)
Definition: NavState.cpp:159
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
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
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
D_v_R
#define D_v_R(H)
Definition: NavState.cpp:165
gtsam::NavState::t_
Point3 t_
position n_t, in nav frame
Definition: NavState.h:40
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:55
gtsam::NavState::dP
static Eigen::Block< Vector9, 3, 1 > dP(Vector9 &v)
Definition: NavState.h:141
dt
const double dt
Definition: testVelocityConstraint.cpp:15
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:34
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
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:163
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::NavState::v
const Vector3 & v() const
Return velocity as Vector3. Computation-free.
Definition: NavState.h:107
os
ofstream os("timeSchurFactors.csv")
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::skewSymmetric
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:400
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:135
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:258
biased_x_rotation::omega
const double omega
Definition: testPreintegratedRotation.cpp:32
gtsam::NavState::attitude
const Rot3 & attitude(OptionalJacobian< 3, 9 > H={}) const
Definition: NavState.cpp:48
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:39
gtsam::NavState::v_
Velocity3 v_
velocity n_v in nav frame
Definition: NavState.h:41
gtsam::NavState::t
Vector3 t() const
Return position as Vector3.
Definition: NavState.h:103
gtsam::NavState::R
Matrix3 R() const
Return rotation matrix. Induces computation in quaternion mode.
Definition: NavState.h:95
Eigen::Triplet< double >
gtsam::symbol_shorthand::F
Key F(std::uint64_t j)
Definition: inference/Symbol.h:153
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:144
D_v_v
#define D_v_v(H)
Definition: NavState.cpp:167
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
D_v_t
#define D_v_t(H)
Definition: NavState.cpp:166
D_t_v
#define D_t_v(H)
Definition: NavState.cpp:164
gtsam
traits
Definition: SFMdata.h:40
gtsam::NavState::matrix
Matrix7 matrix() const
Definition: NavState.cpp:80
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:101
gtsam::NavState::position
const Point3 & position(OptionalJacobian< 3, 9 > H={}) const
Definition: NavState.cpp:55
gtsam::Rot3::Expmap
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
Definition: Rot3.h:374
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::NavState::retract
NavState retract(const Vector9 &v, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
retract with optional derivatives
Definition: NavState.cpp:107
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:96
Eigen::Matrix< double, 9, 9 >
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.examples.ImuFactorISAM2Example.n_gravity
def n_gravity
Definition: ImuFactorISAM2Example.py:31
gtsam::NavState::localCoordinates
Vector9 localCoordinates(const NavState &g, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
localCoordinates with optional derivatives
Definition: NavState.cpp:134
D_t_R
#define D_t_R(H)
Definition: NavState.cpp:162
gtsam::NavState::pose
const Pose3 pose() const
Definition: NavState.h:86
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:214
align_3::t
Point2 t(10, 10)
gtsam::Rot3::equals
bool equals(const Rot3 &p, double tol=1e-9) const
Definition: Rot3.cpp:99
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 Fri Nov 1 2024 03:33:49