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