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 /* ************************************************************************* */
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
predictionErrorVel
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)
Definition: timeInertialNavFactor_GlobalVelocity.cpp:42
simple_graph::b1
Vector2 b1(2, -1)
predictionErrorPose
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)
Definition: timeInertialNavFactor_GlobalVelocity.cpp:38
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
main
int main()
Definition: timeInertialNavFactor_GlobalVelocity.cpp:48
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
world_g
static const Vector3 world_g(0.0, 0.0, 9.81)
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
so3::R1
SO3 R1
Definition: testShonanFactor.cpp:41
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
ECEF_omega_earth
gtsam::Vector ECEF_omega_earth(Vector3(0.0, 0.0, 7.292115e-5))
Key.h
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
gtsam::InertialNavFactor_GlobalVelocity
Definition: InertialNavFactor_GlobalVelocity.h:77
numericalDerivative.h
Some functions to compute numerical derivatives.
gttoc_
#define gttoc_(label)
Definition: timing.h:250
gtsam::SO::matrix
const MatrixNN & matrix() const
Return matrix.
Definition: SOn.h:157
world_rho
static const Vector3 world_rho(0.0, -1.5724e-05, 0.0)
gttic_
#define gttic_(label)
Definition: timing.h:245
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::tictoc_finishedIteration_
void tictoc_finishedIteration_()
Definition: timing.h:264
gtsam::Pose3
Definition: Pose3.h:37
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::tictoc_print_
void tictoc_print_()
Definition: timing.h:268
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
gtsam::SharedGaussian
noiseModel::Gaussian::shared_ptr SharedGaussian
Definition: NoiseModel.h:763
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
ordering
static enum @1096 ordering
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
ImuBias.h
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:32
so3::R2
SO3 R2
Definition: testShonanFactor.cpp:43
gtsam
traits
Definition: SFMdata.h:40
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
v2
Vector v2
Definition: testSerializationBase.cpp:39
gtsam::Rot3::matrix
Matrix3 matrix() const
Definition: Rot3M.cpp:218
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
world_R_ECEF
gtsam::Rot3 world_R_ECEF(0.31686, 0.51505, 0.79645, 0.85173, -0.52399, 0, 0.41733, 0.67835, -0.60471)
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
world_omega_earth
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() *ECEF_omega_earth)
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::Ordering
Definition: inference/Ordering.h:33
gtsam::InertialNavFactor_GlobalVelocity::evaluateError
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
Definition: InertialNavFactor_GlobalVelocity.h:228
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
Values.h
A non-templated config holding any types of Manifold-group elements.
InertialNavFactor_GlobalVelocity.h
Inertial navigation factor (velocity in the global frame)
Pose3.h
3D Pose
v1
Vector v1
Definition: testSerializationBase.cpp:38
gtsam::Pose2
Definition: Pose2.h:39


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:42:08