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
12/**
13 * @file testEquivInertialNavFactor_GlobalVel.h.cpp
14 * @brief Unit test for the InertialNavFactor_GlobalVelocity
15 * @author Vadim Indelman, Stephen Williams
16 */
17
18#include <gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h>
19#include <gtsam/navigation/ImuBias.h>
20#include <gtsam/geometry/Pose3.h>
21#include <gtsam/nonlinear/Values.h>
22#include <gtsam/inference/Key.h>
23#include <gtsam/base/numericalDerivative.h>
24
25#include <CppUnitLite/TestHarness.h>
26#include <iostream>
27
28using namespace std::placeholders;
29using namespace std;
30using namespace gtsam;
31
32/* ************************************************************************* */
33TEST( EquivInertialNavFactor_GlobalVel, Constructor)
34{
35 Key poseKey1(11);
36 Key poseKey2(12);
37 Key velKey1(21);
38 Key velKey2(22);
39 Key biasKey1(31);
40
41 // IMU accumulation variables
42 Vector delta_pos_in_t0 = Vector3(0.0, 0.0, 0.0);
43 Vector delta_vel_in_t0 = Vector3(0.0, 0.0, 0.0);
44 Vector delta_angles = Vector3(0.0, 0.0, 0.0);
45 double delta_t = 0.0;
46 Matrix EquivCov_Overall = Matrix::Zero(rows: 15,cols: 15);
47 Matrix Jacobian_wrt_t0_Overall = Matrix::Identity(rows: 15,cols: 15);
48 imuBias::ConstantBias bias1 = imuBias::ConstantBias();
49
50 // Earth Terms (gravity, etc)
51 Vector3 g(0.0, 0.0, -9.80);
52 Vector3 rho(0.0, 0.0, 0.0);
53 Vector3 omega_earth(0.0, 0.0, 0.0);
54
55 // IMU Noise Model
56 SharedGaussian imu_model = noiseModel::Gaussian::Covariance(covariance: EquivCov_Overall.block(startRow: 0,startCol: 0,blockRows: 9,blockCols: 9));
57
58 // Constructor
59 EquivInertialNavFactor_GlobalVel<Pose3, Vector3, imuBias::ConstantBias> factor(
60 poseKey1, velKey1, biasKey1, poseKey2, velKey2,
61 delta_pos_in_t0, delta_vel_in_t0, delta_angles, delta_t,
62 g, rho, omega_earth, imu_model, Jacobian_wrt_t0_Overall, bias1);
63
64}
65
66/* ************************************************************************* */
67 int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
68/* ************************************************************************* */
69