timeInertialNavFactor_GlobalVelocity.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 
18 #include <iostream>
21 #include <gtsam/geometry/Pose3.h>
22 #include <gtsam/nonlinear/Values.h>
24 #include <gtsam/inference/Key.h>
25 
26 using namespace std;
27 using namespace gtsam;
28 
30  0.31686, 0.51505, 0.79645,
31  0.85173, -0.52399, 0,
32  0.41733, 0.67835, -0.60471);
33 
34 gtsam::Vector ECEF_omega_earth(Vector3(0.0, 0.0, 7.292115e-5));
36 
37 /* ************************************************************************* */
39  return Pose3::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).head(6));
40 }
41 
43  return factor.evaluateError(p1, v1, b1, p2, v2).tail(3);
44 }
45 
47 /* ************************************************************************* */
48 int main() {
49  gtsam::Key PoseKey1(11);
50  gtsam::Key PoseKey2(12);
51  gtsam::Key VelKey1(21);
52  gtsam::Key VelKey2(22);
53  gtsam::Key BiasKey1(31);
54 
55  double measurement_dt(0.1);
56  Vector world_g(Vector3(0.0, 0.0, 9.81));
57  Vector world_rho(Vector3(0.0, -1.5724e-05, 0.0)); // NED system
58  gtsam::Vector ECEF_omega_earth(Vector3(0.0, 0.0, 7.292115e-5));
60 
61  SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
62 
63  // Second test: zero angular motion, some acceleration - generated in matlab
64  Vector measurement_acc(Vector3(6.501390843381716, -6.763926150509185, -2.300389940090343));
65  Vector measurement_gyro(Vector3(0.1, 0.2, 0.3));
66 
67  InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
68 
69  Rot3 R1(0.487316618, 0.125253866, 0.86419557,
70  0.580273724, 0.693095498, -0.427669306,
71  -0.652537293, 0.709880342, 0.265075427);
72  Point3 t1(2.0,1.0,3.0);
73  Pose3 Pose1(R1, t1);
74  Vector3 Vel1 = Vector(Vector3(0.5,-0.5,0.4));
75  Rot3 R2(0.473618898, 0.119523052, 0.872582019,
76  0.609241153, 0.67099888, -0.422594037,
77  -0.636011287, 0.731761397, 0.244979388);
78  Point3 t2 = t1 + Point3(Vel1*measurement_dt);
79  Pose3 Pose2(R2, t2);
80  Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g);
81  Vector3 Vel2 = Vel1 + dv;
83 
84  Values values;
85  values.insert(PoseKey1, Pose1);
86  values.insert(PoseKey2, Pose2);
87  values.insert(VelKey1, Vel1);
88  values.insert(VelKey2, Vel2);
89  values.insert(BiasKey1, Bias1);
90 
92  ordering.push_back(PoseKey1);
93  ordering.push_back(VelKey1);
94  ordering.push_back(BiasKey1);
95  ordering.push_back(PoseKey2);
96  ordering.push_back(VelKey2);
97 
99  gttic_(LinearizeTiming);
100  for(size_t i = 0; i < 100000; ++i) {
101  GaussianFactor::shared_ptr g = f.linearize(values);
102  graph.push_back(g);
103  }
104  gttoc_(LinearizeTiming);
106  tictoc_print_();
107 }
108 
109 /* ************************************************************************* */
gtsam::Pose3 predictionErrorPose(const Pose3 &p1, const Vector3 &v1, const imuBias::ConstantBias &b1, const Pose3 &p2, const Vector3 &v2, const InertialNavFactor_GlobalVelocity< Pose3, Vector3, imuBias::ConstantBias > &factor)
Vector v2
#define gttic_(label)
Definition: timing.h:245
Eigen::Vector3d Vector3
Definition: Vector.h:43
Vector v1
A non-templated config holding any types of Manifold-group elements.
Vector3f p1
Inertial navigation factor (velocity in the global frame)
noiseModel::Diagonal::shared_ptr model
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
leaf::MyValues values
Pose2_ Expmap(const Vector3_ &xi)
Definition: BFloat16.h:88
Some functions to compute numerical derivatives.
NonlinearFactorGraph graph
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
static enum @1107 ordering
void g(const string &key, int i)
Definition: testBTree.cpp:41
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() *ECEF_omega_earth)
Eigen::VectorXd Vector
Definition: Vector.h:38
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
void tictoc_print_()
Definition: timing.h:268
gtsam::Vector ECEF_omega_earth(Vector3(0.0, 0.0, 7.292115e-5))
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Vector evaluateError(const POSE &Pose1, const VELOCITY &Vel1, const IMUBIAS &Bias1, const POSE &Pose2, const VELOCITY &Vel2, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4, OptionalMatrixType H5) const override
Linear Factor Graph where all factors are Gaussians.
static const Vector3 world_g(0.0, 0.0, 9.81)
std::shared_ptr< GaussianFactor > linearize(const Values &x) const override
traits
Definition: chartTesting.h:28
gtsam::Vector3 predictionErrorVel(const Pose3 &p1, const Vector3 &v1, const imuBias::ConstantBias &b1, const Pose3 &p2, const Vector3 &v2, const InertialNavFactor_GlobalVelocity< Pose3, Vector3, imuBias::ConstantBias > &factor)
static const Vector3 world_rho(0.0, -1.5724e-05, 0.0)
Matrix3 matrix() const
Definition: Rot3M.cpp:218
gtsam::Rot3 world_R_ECEF(0.31686, 0.51505, 0.79645, 0.85173, -0.52399, 0, 0.41733, 0.67835, -0.60471)
void insert(Key j, const Value &val)
Definition: Values.cpp:155
static Point3 p2
void tictoc_finishedIteration_()
Definition: timing.h:264
Vector3 Point3
Definition: Point3.h:38
#define gttoc_(label)
Definition: timing.h:250
3D Pose
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
noiseModel::Gaussian::shared_ptr SharedGaussian
Definition: NoiseModel.h:742
Vector2 b1(2, -1)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:01