gtsam  4.1.0
gtsam
InertialNavFactor_GlobalVelocity.h
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 
19 #pragma once
20 
23 #include <gtsam/geometry/Rot3.h>
24 #include <gtsam/base/Matrix.h>
25 
26 // Using numerical derivative to calculate d(Pose3::Expmap)/dw
28 
29 #include <boost/optional.hpp>
30 
31 #include <ostream>
32 
33 namespace gtsam {
34 
35 /*
36  * NOTES:
37  * =====
38  * - The global frame (NED or ENU) is defined by the user by specifying the gravity vector in this frame.
39  * - The IMU frame is implicitly defined by the user via the rotation matrix between global and imu frames.
40  * - Camera and IMU frames are identical
41  * - The user should specify a continuous equivalent noise covariance, which can be calculated using
42  * the static function CalcEquivalentNoiseCov based on the IMU gyro and acc measurement noise covariance
43  * matrices and the process\modeling covariance matrix. The IneritalNavFactor converts this into a
44  * discrete form using the supplied delta_t between sub-sequential measurements.
45  * - Earth-rate correction:
46  * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global
47  * frame (Local-Level system: ENU or NED, see above).
48  * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system.
49  * + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant.
50  * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon.
51  *
52  * - Frame Notation:
53  * Quantities are written as {Frame of Representation/Destination Frame}_{Quantity Type}_{Quatity Description/Origination Frame}
54  * So, the rotational velocity of the sensor written in the body frame is: body_omega_sensor
55  * And the transformation from the body frame to the world frame would be: world_P_body
56  * This allows visual chaining. For example, converting the sensed angular velocity of the IMU
57  * (angular velocity of the sensor in the sensor frame) into the world frame can be performed as:
58  * world_R_body * body_R_sensor * sensor_omega_sensor = world_omega_sensor
59  *
60  *
61  * - Common Quantity Types
62  * P : pose/3d transformation
63  * R : rotation
64  * omega : angular velocity
65  * t : translation
66  * v : velocity
67  * a : acceleration
68  *
69  * - Common Frames
70  * sensor : the coordinate system attached to the sensor origin
71  * body : the coordinate system attached to body/inertial frame.
72  * Unless an optional frame transformation is provided, the
73  * sensor frame and the body frame will be identical
74  * world : the global/world coordinate frame. This is assumed to be
75  * a tangent plane to the earth's surface somewhere near the
76  * vehicle
77  */
78 template<class POSE, class VELOCITY, class IMUBIAS>
79 class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> {
80 
81 private:
82 
85 
86  Vector measurement_acc_;
87  Vector measurement_gyro_;
88  double dt_;
89 
90  Vector world_g_;
91  Vector world_rho_;
92  Vector world_omega_earth_;
93 
94  boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
95 
96 public:
97 
98  // shorthand for a smart pointer to a factor
99  typedef typename boost::shared_ptr<InertialNavFactor_GlobalVelocity> shared_ptr;
100 
103 
105  InertialNavFactor_GlobalVelocity(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2,
106  const Vector& measurement_acc, const Vector& measurement_gyro, const double measurement_dt, const Vector world_g, const Vector world_rho,
107  const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_continuous, boost::optional<POSE> body_P_sensor = boost::none) :
108  Base(calc_descrete_noise_model(model_continuous, measurement_dt ),
109  Pose1, Vel1, IMUBias1, Pose2, Vel2), measurement_acc_(measurement_acc), measurement_gyro_(measurement_gyro),
110  dt_(measurement_dt), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), body_P_sensor_(body_P_sensor) { }
111 
113 
117  void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
118  std::cout << s << "("
119  << keyFormatter(this->key1()) << ","
120  << keyFormatter(this->key2()) << ","
121  << keyFormatter(this->key3()) << ","
122  << keyFormatter(this->key4()) << ","
123  << keyFormatter(this->key5()) << "\n";
124  std::cout << "acc measurement: " << this->measurement_acc_.transpose() << std::endl;
125  std::cout << "gyro measurement: " << this->measurement_gyro_.transpose() << std::endl;
126  std::cout << "dt: " << this->dt_ << std::endl;
127  std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl;
128  std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl;
129  std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl;
130  if(this->body_P_sensor_)
131  this->body_P_sensor_->print(" sensor pose in body frame: ");
132  this->noiseModel_->print(" noise model");
133  }
134 
136  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
137  const This *e = dynamic_cast<const This*> (&expected);
138  return e != nullptr && Base::equals(*e, tol)
139  && (measurement_acc_ - e->measurement_acc_).norm() < tol
140  && (measurement_gyro_ - e->measurement_gyro_).norm() < tol
141  && (dt_ - e->dt_) < tol
142  && (world_g_ - e->world_g_).norm() < tol
143  && (world_rho_ - e->world_rho_).norm() < tol
144  && (world_omega_earth_ - e->world_omega_earth_).norm() < tol
145  && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
146  }
147 
148  POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
149  // Calculate the corrected measurements using the Bias object
150  Vector GyroCorrected(Bias1.correctGyroscope(measurement_gyro_));
151 
152  const POSE& world_P1_body = Pose1;
153  const VELOCITY& world_V1_body = Vel1;
154 
155  // Calculate the acceleration and angular velocity of the body in the body frame (including earth-related rotations)
156  Vector body_omega_body;
157  if(body_P_sensor_) {
158  body_omega_body = body_P_sensor_->rotation().matrix() * GyroCorrected;
159  } else {
160  body_omega_body = GyroCorrected;
161  }
162 
163  // Convert earth-related terms into the body frame
164  Matrix body_R_world(world_P1_body.rotation().inverse().matrix());
165  Vector body_rho = body_R_world * world_rho_;
166  Vector body_omega_earth = body_R_world * world_omega_earth_;
167 
168  // Correct for earth-related terms
169  body_omega_body -= body_rho + body_omega_earth;
170 
171  // The velocity is in the global frame, so composing Pose1 with v*dt is incorrect
172  return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_omega_body*dt_), Pose1.translation() + typename POSE::Translation(world_V1_body*dt_));
173  }
174 
175  VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
176  // Calculate the corrected measurements using the Bias object
177  Vector AccCorrected(Bias1.correctAccelerometer(measurement_acc_));
178 
179  const POSE& world_P1_body = Pose1;
180  const VELOCITY& world_V1_body = Vel1;
181 
182  // Calculate the acceleration and angular velocity of the body in the body frame (including earth-related rotations)
183  Vector body_a_body, body_omega_body;
184  if(body_P_sensor_) {
185  Matrix body_R_sensor = body_P_sensor_->rotation().matrix();
186 
187  Vector GyroCorrected(Bias1.correctGyroscope(measurement_gyro_));
188  body_omega_body = body_R_sensor * GyroCorrected;
189  Matrix body_omega_body__cross = skewSymmetric(body_omega_body);
190  body_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor_->translation();
191  } else {
192  body_a_body = AccCorrected;
193  }
194 
195  // Correct for earth-related terms
196  Vector world_a_body = world_P1_body.rotation().matrix() * body_a_body + world_g_ - 2*skewSymmetric(world_rho_ + world_omega_earth_)*world_V1_body;
197 
198  // Calculate delta in the body frame
199  VELOCITY VelDelta(world_a_body*dt_);
200 
201  // Predict
202  return Vel1 + VelDelta;
203  }
204 
205  void predict(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, POSE& Pose2, VELOCITY& Vel2) const {
206  Pose2 = predictPose(Pose1, Vel1, Bias1);
207  Vel2 = predictVelocity(Pose1, Vel1, Bias1);
208  }
209 
210  POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const {
211  // Predict
212  POSE Pose2Pred = predictPose(Pose1, Vel1, Bias1);
213 
214  // Calculate error
215  return Pose2.between(Pose2Pred);
216  }
217 
218  VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const {
219  // Predict
220  VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1);
221 
222  // Calculate error
223  return Vel2Pred - Vel2;
224  }
225 
227  Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2,
228  boost::optional<Matrix&> H1 = boost::none,
229  boost::optional<Matrix&> H2 = boost::none,
230  boost::optional<Matrix&> H3 = boost::none,
231  boost::optional<Matrix&> H4 = boost::none,
232  boost::optional<Matrix&> H5 = boost::none) const override {
233 
234  // TODO: Write analytical derivative calculations
235  // Jacobian w.r.t. Pose1
236  if (H1){
237  Matrix H1_Pose = gtsam::numericalDerivative11<POSE, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1);
238  Matrix H1_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1);
239  *H1 = stack(2, &H1_Pose, &H1_Vel);
240  }
241 
242  // Jacobian w.r.t. Vel1
243  if (H2){
244  if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
245  Matrix H2_Pose = gtsam::numericalDerivative11<POSE, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
246  Matrix H2_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
247  *H2 = stack(2, &H2_Pose, &H2_Vel);
248  }
249 
250  // Jacobian w.r.t. IMUBias1
251  if (H3){
252  Matrix H3_Pose = gtsam::numericalDerivative11<POSE, IMUBIAS>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1);
253  Matrix H3_Vel = gtsam::numericalDerivative11<VELOCITY, IMUBIAS>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1);
254  *H3 = stack(2, &H3_Pose, &H3_Vel);
255  }
256 
257  // Jacobian w.r.t. Pose2
258  if (H4){
259  Matrix H4_Pose = gtsam::numericalDerivative11<POSE, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2);
260  Matrix H4_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2);
261  *H4 = stack(2, &H4_Pose, &H4_Vel);
262  }
263 
264  // Jacobian w.r.t. Vel2
265  if (H5){
266  if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
267  Matrix H5_Pose = gtsam::numericalDerivative11<POSE, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
268  Matrix H5_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
269  *H5 = stack(2, &H5_Pose, &H5_Vel);
270  }
271 
272  Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2)));
273  Vector ErrVelVector(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2));
274 
275  return concatVectors(2, &ErrPoseVector, &ErrVelVector);
276  }
277 
278  static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
279  const noiseModel::Gaussian::shared_ptr& gaussian_process){
280 
281  Matrix cov_acc = ( gaussian_acc->R().transpose() * gaussian_acc->R() ).inverse();
282  Matrix cov_gyro = ( gaussian_gyro->R().transpose() * gaussian_gyro->R() ).inverse();
283  Matrix cov_process = ( gaussian_process->R().transpose() * gaussian_process->R() ).inverse();
284 
285  cov_process.block(0,0, 3,3) += cov_gyro;
286  cov_process.block(6,6, 3,3) += cov_acc;
287 
288  return noiseModel::Gaussian::Covariance(cov_process);
289  }
290 
291  static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial,
292  Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) {
293 
294  Matrix ENU_to_NED = (Matrix(3, 3) <<
295  0.0, 1.0, 0.0,
296  1.0, 0.0, 0.0,
297  0.0, 0.0, -1.0).finished();
298 
299  Matrix NED_to_ENU = (Matrix(3, 3) <<
300  0.0, 1.0, 0.0,
301  1.0, 0.0, 0.0,
302  0.0, 0.0, -1.0).finished();
303 
304  // Convert incoming parameters to ENU
305  Vector Pos_ENU = NED_to_ENU * Pos_NED;
306  Vector Vel_ENU = NED_to_ENU * Vel_NED;
307  Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial;
308 
309  // Call ENU version
310  Vector g_ENU;
311  Vector rho_ENU;
312  Vector omega_earth_ENU;
313  Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU);
314 
315  // Convert output to NED
316  g_NED = ENU_to_NED * g_ENU;
317  rho_NED = ENU_to_NED * rho_ENU;
318  omega_earth_NED = ENU_to_NED * omega_earth_ENU;
319  }
320 
321  static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial,
322  Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){
323  double R0 = 6.378388e6;
324  double e = 1/297;
325  double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) );
326 
327  // Calculate current lat, lon
328  Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial);
329  double delta_lat(delta_Pos_ENU(1)/Re);
330  double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0))));
331  double lat_new(LatLonHeight_IC(0) + delta_lat);
332  double lon_new(LatLonHeight_IC(1) + delta_lon);
333 
334  // Rotation of lon about z axis
335  Rot3 C1(cos(lon_new), sin(lon_new), 0.0,
336  -sin(lon_new), cos(lon_new), 0.0,
337  0.0, 0.0, 1.0);
338 
339  // Rotation of lat about y axis
340  Rot3 C2(cos(lat_new), 0.0, sin(lat_new),
341  0.0, 1.0, 0.0,
342  -sin(lat_new), 0.0, cos(lat_new));
343 
344  Rot3 UEN_to_ENU(0, 1, 0,
345  0, 0, 1,
346  1, 0, 0);
347 
348  Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
349 
350  Vector omega_earth_ECEF(Vector3(0.0, 0.0, 7.292115e-5));
351  omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
352 
353  // Calculating g
354  double height(LatLonHeight_IC(2));
355  double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84
356  double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid
357  double e2( pow(ECCENTRICITY,2) );
358  double den( 1-e2*pow(sin(lat_new),2) );
359  double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) );
360  double Rp( EQUA_RADIUS/( sqrt(den) ) );
361  double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature
362  double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) );
363  double g_calc( g0/( pow(1 + height/Ro, 2) ) );
364  g_ENU = (Vector(3) << 0.0, 0.0, -g_calc).finished();
365 
366 
367  // Calculate rho
368  double Ve( Vel_ENU(0) );
369  double Vn( Vel_ENU(1) );
370  double rho_E = -Vn/(Rm + height);
371  double rho_N = Ve/(Rp + height);
372  double rho_U = Ve*tan(lat_new)/(Rp + height);
373  rho_ENU = (Vector(3) << rho_E, rho_N, rho_U).finished();
374  }
375 
376  static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){
377  /* Q_d (approx)= Q * delta_t */
378  /* In practice, square root of the information matrix is represented, so that:
379  * R_d (approx)= R / sqrt(delta_t)
380  * */
381  return noiseModel::Gaussian::SqrtInformation(model->R()/std::sqrt(delta_t));
382  }
383 
384 private:
385 
388  template<class ARCHIVE>
389  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
390  ar & boost::serialization::make_nvp("NonlinearFactor2",
391  boost::serialization::base_object<Base>(*this));
392  }
393 
394 }; // \class InertialNavFactor_GlobalVelocity
395 
397 template<class POSE, class VELOCITY, class IMUBIAS>
398 struct traits<InertialNavFactor_GlobalVelocity<POSE, VELOCITY, IMUBIAS> > :
399  public Testable<InertialNavFactor_GlobalVelocity<POSE, VELOCITY, IMUBIAS> > {
400 };
401 
402 }
gtsam::InertialNavFactor_GlobalVelocity
Definition: InertialNavFactor_GlobalVelocity.h:79
gtsam::InertialNavFactor_GlobalVelocity::InertialNavFactor_GlobalVelocity
InertialNavFactor_GlobalVelocity()
default constructor - only use for serialization
Definition: InertialNavFactor_GlobalVelocity.h:102
gtsam::InertialNavFactor_GlobalVelocity::access
friend class boost::serialization::access
Serialization function.
Definition: InertialNavFactor_GlobalVelocity.h:387
gtsam::NoiseModelFactor5< POSE, VELOCITY, IMUBIAS, POSE, VELOCITY >::key1
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:626
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::NoiseModelFactor5
A convenient base class for creating your own NoiseModelFactor with 5 variables.
Definition: NonlinearFactor.h:588
gtsam::Pose2
Definition: Pose2.h:36
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: NonlinearFactor.cpp:71
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::noiseModel::Gaussian::Covariance
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
A Gaussian noise model created by specifying a covariance matrix.
Definition: NoiseModel.cpp:116
gtsam::concatVectors
Vector concatVectors(const std::list< Vector > &vs)
concatenate Vectors
Definition: Vector.cpp:302
gtsam::KeyFormatter
boost::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::skewSymmetric
Matrix3 skewSymmetric(double wx, double wy, double wz)
skew symmetric matrix returns this: 0 -wz wy wz 0 -wx -wy wx 0
Definition: Matrix.h:404
gtsam::noiseModel::Gaussian::SqrtInformation
static shared_ptr SqrtInformation(const Matrix &R, bool smart=true)
A Gaussian noise model created by specifying a square root information matrix.
Definition: NoiseModel.cpp:85
gtsam::InertialNavFactor_GlobalVelocity::print
void print(const std::string &s="InertialNavFactor_GlobalVelocity", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
implement functions needed for Testable
Definition: InertialNavFactor_GlobalVelocity.h:117
NoiseModel.h
gtsam::InertialNavFactor_GlobalVelocity::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
equals
Definition: InertialNavFactor_GlobalVelocity.h:136
gtsam::InertialNavFactor_GlobalVelocity::InertialNavFactor_GlobalVelocity
InertialNavFactor_GlobalVelocity(const Key &Pose1, const Key &Vel1, const Key &IMUBias1, const Key &Pose2, const Key &Vel2, const Vector &measurement_acc, const Vector &measurement_gyro, const double measurement_dt, const Vector world_g, const Vector world_rho, const Vector &world_omega_earth, const noiseModel::Gaussian::shared_ptr &model_continuous, boost::optional< POSE > body_P_sensor=boost::none)
Constructor.
Definition: InertialNavFactor_GlobalVelocity.h:105
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:42
Rot3.h
3D rotation represented as a rotation matrix or quaternion
gtsam::stack
Matrix stack(size_t nrMatrices,...)
create a matrix by stacking other matrices Given a set of matrices: A1, A2, A3...
Definition: Matrix.cpp:396
gtsam::InertialNavFactor_GlobalVelocity::evaluateError
Vector evaluateError(const POSE &Pose1, const VELOCITY &Vel1, const IMUBIAS &Bias1, const POSE &Pose2, const VELOCITY &Vel2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none, boost::optional< Matrix & > H4=boost::none, boost::optional< Matrix & > H5=boost::none) const override
implement functions needed to derive from Factor
Definition: InertialNavFactor_GlobalVelocity.h:227