testIMUSystem.cpp
Go to the documentation of this file.
1 
6 #include <iostream>
7 
9 
12 #include <gtsam/sam/RangeFactor.h>
17 
22 
23 using namespace std;
24 using namespace gtsam;
25 
26 const double tol=1e-5;
27 
28 static const Key x0 = 0, x1 = 1, x2 = 2, x3 = 3, x4 = 4;
29 static const Vector g = Vector::Unit(3,2)*(-9.81);
30 
31 /* ************************************************************************* */
32 TEST(testIMUSystem, instantiations) {
33  // just checking for compilation
34  PoseRTV x1_v;
35 
40 
41  Vector accel = Vector::Ones(3), gyro = Vector::Ones(3);
42 
43  IMUFactor<PoseRTV> imu(accel, gyro, 0.01, x1, x2, model6);
44  FullIMUFactor<PoseRTV> full_imu(accel, gyro, 0.01, x1, x2, model9);
45  NonlinearEquality<gtsam::PoseRTV> poseHardPrior(x1, x1_v);
46  BetweenFactor<gtsam::PoseRTV> odom(x1, x2, x1_v, model9);
48  VelocityConstraint constraint(x1, x2, 0.1, 10000);
49  PriorFactor<gtsam::PoseRTV> posePrior(x1, x1_v, model9);
50  DHeightPrior heightPrior(x1, 0.1, model1);
51  VelocityPrior velPrior(x1, Vector::Ones(3), model3);
52 }
53 
54 /* ************************************************************************* */
55 TEST( testIMUSystem, optimize_chain ) {
56  // create a simple chain of poses to generate IMU measurements
57  const double dt = 1.0;
58  PoseRTV pose1,
59  pose2(Point3(1.0, 1.0, 0.0), Rot3::Ypr(0.1, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)),
60  pose3(Point3(2.0, 2.0, 0.0), Rot3::Ypr(0.2, 0.0, 0.0), Velocity3(0.0, 0.0, 0.0)),
61  pose4(Point3(3.0, 3.0, 0.0), Rot3::Ypr(0.3, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0));
62 
63  // create measurements
64  SharedDiagonal model = noiseModel::Unit::Create(6);
65  Vector6 imu12 = pose1.imuPrediction(pose2, dt);
66  Vector6 imu23 = pose2.imuPrediction(pose3, dt);
67  Vector6 imu34 = pose3.imuPrediction(pose4, dt);
68 
69  // assemble simple graph with IMU measurements and velocity constraints
72  graph += IMUFactor<PoseRTV>(imu12, dt, x1, x2, model);
73  graph += IMUFactor<PoseRTV>(imu23, dt, x2, x3, model);
74  graph += IMUFactor<PoseRTV>(imu34, dt, x3, x4, model);
75  graph += VelocityConstraint(x1, x2, dt);
76  graph += VelocityConstraint(x2, x3, dt);
77  graph += VelocityConstraint(x3, x4, dt);
78 
79  // ground truth values
80  Values true_values;
81  true_values.insert(x1, pose1);
82  true_values.insert(x2, pose2);
83  true_values.insert(x3, pose3);
84  true_values.insert(x4, pose4);
85 
86  // verify zero error
87  EXPECT_DOUBLES_EQUAL(0, graph.error(true_values), 1e-5);
88 
89  // initialize with zero values and optimize
90  Values values;
91  values.insert(x1, PoseRTV());
92  values.insert(x2, PoseRTV());
93  values.insert(x3, PoseRTV());
94  values.insert(x4, PoseRTV());
95 
96  Values actual = LevenbergMarquardtOptimizer(graph, values).optimize();
97  EXPECT(assert_equal(true_values, actual, tol));
98 }
99 
100 /* ************************************************************************* */
101 TEST( testIMUSystem, optimize_chain_fullfactor ) {
102  // create a simple chain of poses to generate IMU measurements
103  const double dt = 1.0;
104  PoseRTV pose1,
105  pose2(Point3(1.0, 0.0, 0.0), Rot3::Ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)),
106  pose3(Point3(2.0, 0.0, 0.0), Rot3::Ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)),
107  pose4(Point3(3.0, 0.0, 0.0), Rot3::Ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0));
108 
109  // create measurements
110  SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0);
111  Vector6 imu12 = pose1.imuPrediction(pose2, dt);
112  Vector6 imu23 = pose2.imuPrediction(pose3, dt);
113  Vector6 imu34 = pose3.imuPrediction(pose4, dt);
114 
115  // assemble simple graph with IMU measurements and velocity constraints
118  graph += FullIMUFactor<PoseRTV>(imu12, dt, x1, x2, model);
119  graph += FullIMUFactor<PoseRTV>(imu23, dt, x2, x3, model);
120  graph += FullIMUFactor<PoseRTV>(imu34, dt, x3, x4, model);
121 
122  // ground truth values
123  Values true_values;
124  true_values.insert(x1, pose1);
125  true_values.insert(x2, pose2);
126  true_values.insert(x3, pose3);
127  true_values.insert(x4, pose4);
128 
129  // verify zero error
130  EXPECT_DOUBLES_EQUAL(0, graph.error(true_values), 1e-5);
131 
132  // initialize with zero values and optimize
133  Values values;
134  values.insert(x1, PoseRTV());
135  values.insert(x2, PoseRTV());
136  values.insert(x3, PoseRTV());
137  values.insert(x4, PoseRTV());
138 
139  cout << "Initial Error: " << graph.error(values) << endl; // Initial error is 0.5 - need better prediction model
140 
141  Values actual = LevenbergMarquardtOptimizer(graph, values).optimize();
142 // EXPECT(assert_equal(true_values, actual, tol)); // FAIL
143 }
144 
145 /* ************************************************************************* */
146 TEST( testIMUSystem, linear_trajectory) {
147  // create a linear trajectory of poses
148  // and verify simple solution
149  const double dt = 1.0;
150 
151  PoseRTV start;
152  Vector accel = Vector::Unit(3,0)*0.5; // forward force
153  Vector gyro = Vector::Unit(3,0)*0.1; // constant rotation
154  SharedDiagonal model = noiseModel::Unit::Create(9);
155 
156  Values true_traj, init_traj;
158 
159  graph += NonlinearEquality<gtsam::PoseRTV>(x0, start);
160  true_traj.insert(x0, start);
161  init_traj.insert(x0, start);
162 
163  size_t nrPoses = 10;
164  PoseRTV cur_pose = start;
165  for (size_t i=1; i<nrPoses; ++i) {
166  Key xA = i-1, xB = i;
167  cur_pose = cur_pose.generalDynamics(accel, gyro, dt);
168  graph += FullIMUFactor<PoseRTV>(accel - g, gyro, dt, xA, xB, model);
169  true_traj.insert(xB, cur_pose);
170  init_traj.insert(xB, PoseRTV());
171  }
172 // EXPECT_DOUBLES_EQUAL(0, graph.error(true_traj), 1e-5); // FAIL
173 }
174 
175 /* ************************************************************************* */
176 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
177 /* ************************************************************************* */
double error(const Values &c) const override
virtual const Values & optimize()
static const Key x2
static int runAllTests(TestResult &result)
Factor Graph consisting of non-linear factors.
noiseModel::Diagonal::shared_ptr model
void insert(Key j, const Value &val)
Definition: Values.cpp:140
Priors to be used with dynamic systems (Specifically PoseRTV)
Vector3 Velocity3
Velocity is currently typedef&#39;d to Vector3.
Definition: NavState.h:28
leaf::MyValues values
static const Key x3
Vector6 imuPrediction(const PoseRTV &x2, double dt) const
Definition: PoseRTV.cpp:133
Definition: Half.h:150
Factor to express an IMU measurement between dynamic poses.
NonlinearFactorGraph graph
Constraint enforcing the relationship between pose and velocity.
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:162
noiseModel::Isotropic::shared_ptr model1
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:608
const double dt
static const Key x1
A simple prior factor that allows for setting a prior only on a part of linear parameters.
Eigen::VectorXd Vector
Definition: Vector.h:38
TEST(testIMUSystem, instantiations)
static const Vector g
#define EXPECT(condition)
Definition: Test.h:151
static Pose3 pose3(rt3, pt3)
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
const SharedNoiseModel model3
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:736
traits
Definition: chartTesting.h:28
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
Factor to express an IMU measurement between dynamic poses.
static const Key x0
const double tol
static const Key x4
int main()
static const Pose3 pose2
Vector3 Point3
Definition: Point3.h:35
PoseRTV generalDynamics(const Vector &accel, const Vector &gyro, double dt) const
General Dynamics update - supply control inputs in body frame.
Definition: PoseRTV.cpp:118
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:47:23