testEquivInertialNavFactor_GlobalVel.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 #include <gtsam/geometry/Pose3.h>
21 #include <gtsam/nonlinear/Values.h>
22 #include <gtsam/inference/Key.h>
25 #include <iostream>
26 
27 using namespace std;
28 using namespace gtsam;
29 
30 /* ************************************************************************* */
32 {
33  Key poseKey1(11);
34  Key poseKey2(12);
35  Key velKey1(21);
36  Key velKey2(22);
37  Key biasKey1(31);
38 
39  // IMU accumulation variables
40  Vector delta_pos_in_t0 = Vector3(0.0, 0.0, 0.0);
41  Vector delta_vel_in_t0 = Vector3(0.0, 0.0, 0.0);
42  Vector delta_angles = Vector3(0.0, 0.0, 0.0);
43  double delta_t = 0.0;
44  Matrix EquivCov_Overall = Matrix::Zero(15,15);
45  Matrix Jacobian_wrt_t0_Overall = Matrix::Identity(15,15);
47 
48  // Earth Terms (gravity, etc)
49  Vector3 g(0.0, 0.0, -9.80);
50  Vector3 rho(0.0, 0.0, 0.0);
51  Vector3 omega_earth(0.0, 0.0, 0.0);
52 
53  // IMU Noise Model
54  SharedGaussian imu_model = noiseModel::Gaussian::Covariance(EquivCov_Overall.block(0,0,9,9));
55 
56  // Constructor
58  poseKey1, velKey1, biasKey1, poseKey2, velKey2,
59  delta_pos_in_t0, delta_vel_in_t0, delta_angles, delta_t,
60  g, rho, omega_earth, imu_model, Jacobian_wrt_t0_Overall, bias1);
61 
62 }
63 
64 /* ************************************************************************* */
65  int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
66 /* ************************************************************************* */
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
A non-templated config holding any types of Manifold-group elements.
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
Definition: Half.h:150
Some functions to compute numerical derivatives.
void g(const string &key, int i)
Definition: testBTree.cpp:43
Eigen::VectorXd Vector
Definition: Vector.h:38
Equivalent inertial navigation factor (velocity in the global frame).
traits
Definition: chartTesting.h:28
Bias bias1(biasAcc1, biasGyro1)
3D Pose
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
static Key poseKey1(x1)
noiseModel::Gaussian::shared_ptr SharedGaussian
Definition: NoiseModel.h:735
TEST(EquivInertialNavFactor_GlobalVel, Constructor)


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:46:28