testScenarioRunner.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 // #define ENABLE_TIMING // uncomment for timing results
19 
20 #include <gtsam/navigation/ScenarioRunner.h>
21 #include <gtsam/base/timing.h>
22 #include <CppUnitLite/TestHarness.h>
23 #include <cmath>
24 
25 using namespace std;
26 using namespace gtsam;
27 
28 static const double kDegree = M_PI / 180.0;
29 static const double kDt = 1e-2;
30 
31 // realistic white noise strengths are 0.5 deg/sqrt(hr) and 0.1 (m/s)/sqrt(h)
32 static const double kGyroSigma = 0.5 * kDegree / 60;
33 static const double kAccelSigma = 0.1 / 60.0;
34 
35 static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3);
36 static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias);
37 
38 // Create default parameters with Z-up and above noise parameters
39 static std::shared_ptr<PreintegrationParams> defaultParams() {
40  auto p = PreintegrationParams::MakeSharedU(10);
41  p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
42  p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
43  p->integrationCovariance = 0.0000001 * I_3x3;
44  return p;
45 }
46 
47 #define EXPECT_NEAR(a, b, c) EXPECT(assert_equal(Vector(a), Vector(b), c));
48 
49 /* ************************************************************************* */
50 TEST(ScenarioRunner, Spin) {
51  gttic(Spin);
52  // angular velocity 6 degree/sec
53  const double w = 6 * kDegree;
54  const Vector3 W(0, 0, w), V(0, 0, 0);
55  const ConstantTwistScenario scenario(W, V);
56 
57  auto p = defaultParams();
58  ScenarioRunner runner(scenario, p, kDt);
59  const double T = 2 * kDt; // seconds
60 
61  auto pim = runner.integrate(T);
62  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
63 
64 #if 0
65  // Check sampled noise is kosher
66  Matrix6 expected;
67  expected << p->accelerometerCovariance / kDt, Z_3x3, //
68  Z_3x3, p->gyroscopeCovariance / kDt;
69  Matrix6 actual = runner.estimateNoiseCovariance(1000);
70  EXPECT(assert_equal(expected, actual, 1e-2));
71 #endif
72 
73  // Check calculated covariance against Monte Carlo estimate
74  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
75  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
76 }
77 
78 /* ************************************************************************* */
79 namespace forward {
80 const double v = 2; // m/s
81 ConstantTwistScenario scenario(Z_3x1, Vector3(v, 0, 0));
82 }
83 /* ************************************************************************* */
84 TEST(ScenarioRunner, Forward) {
85  gttic(Forward);
86  using namespace forward;
87  ScenarioRunner runner(scenario, defaultParams(), kDt);
88  const double T = 0.1; // seconds
89 
90  auto pim = runner.integrate(T);
91  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
92 
93  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
94  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
95  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
96 }
97 
98 /* ************************************************************************* */
99 TEST(ScenarioRunner, ForwardWithBias) {
100  gttic(ForwardWithBias);
101  using namespace forward;
102  ScenarioRunner runner(scenario, defaultParams(), kDt);
103  const double T = 0.1; // seconds
104 
105  auto pim = runner.integrate(T, kNonZeroBias);
106  Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias);
107  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
108 }
109 
110 /* ************************************************************************* */
111 TEST(ScenarioRunner, Circle) {
112  gttic(Circle);
113  // Forward velocity 2m/s, angular velocity 6 degree/sec
114  const double v = 2, w = 6 * kDegree;
115  ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
116 
117  ScenarioRunner runner(scenario, defaultParams(), kDt);
118  const double T = 0.1; // seconds
119 
120  auto pim = runner.integrate(T);
121  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
122 
123  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
124  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
125  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
126 }
127 
128 /* ************************************************************************* */
129 TEST(ScenarioRunner, Loop) {
130  gttic(Loop);
131  // Forward velocity 2m/s
132  // Pitch up with angular velocity 6 degree/sec (negative in FLU)
133  const double v = 2, w = 6 * kDegree;
134  ConstantTwistScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0));
135 
136  ScenarioRunner runner(scenario, defaultParams(), kDt);
137  const double T = 0.1; // seconds
138 
139  auto pim = runner.integrate(T);
140  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
141 
142  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
143  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
144  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
145 }
146 
147 /* ************************************************************************* */
148 namespace initial {
149 const Rot3 nRb;
150 const Point3 P0(0,0,0);
151 const Vector3 V0(0, 0, 0);
152 }
153 
154 /* ************************************************************************* */
155 namespace accelerating {
156 using namespace initial;
157 const double a = 0.2; // m/s^2
158 const Vector3 A(0, a, 0);
159 const AcceleratingScenario scenario(nRb, P0, V0, A);
160 
161 const double T = 3; // seconds
162 }
163 
164 /* ************************************************************************* */
165 TEST(ScenarioRunner, Accelerating) {
166  gttic(Accelerating);
167  using namespace accelerating;
168  ScenarioRunner runner(scenario, defaultParams(), T / 10);
169 
170  auto pim = runner.integrate(T);
171  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
172 
173  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
174  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
175  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
176 }
177 
178 /* ************************************************************************* */
179 TEST(ScenarioRunner, AcceleratingWithBias) {
180  gttic(AcceleratingWithBias);
181  using namespace accelerating;
182  ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias);
183 
184  auto pim = runner.integrate(T, kNonZeroBias);
185  Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias);
186  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
187 }
188 
189 /* ************************************************************************* */
190 TEST(ScenarioRunner, AcceleratingAndRotating) {
191  gttic(AcceleratingAndRotating);
192  using namespace initial;
193  const double a = 0.2; // m/s^2
194  const Vector3 A(0, a, 0), W(0, 0.1, 0);
195  const AcceleratingScenario scenario(nRb, P0, V0, A, W);
196 
197  const double T = 10; // seconds
198  ScenarioRunner runner(scenario, defaultParams(), T / 10);
199 
200  auto pim = runner.integrate(T);
201  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
202 
203  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
204  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
205  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
206 }
207 
208 /* ************************************************************************* */
209 namespace initial2 {
210 // No rotation, but non-zero position and velocities
211 const Rot3 nRb;
212 const Point3 P0(10, 20, 0);
213 const Vector3 V0(50, 0, 0);
214 }
215 
216 /* ************************************************************************* */
217 namespace accelerating2 {
218 using namespace initial2;
219 const double a = 0.2; // m/s^2
220 const Vector3 A(0, a, 0);
221 const AcceleratingScenario scenario(nRb, P0, V0, A);
222 
223 const double T = 3; // seconds
224 }
225 
226 /* ************************************************************************* */
227 TEST(ScenarioRunner, Accelerating2) {
228  gttic(Accelerating2);
229  using namespace accelerating2;
230  ScenarioRunner runner(scenario, defaultParams(), T / 10);
231 
232  auto pim = runner.integrate(T);
233  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
234 
235  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
236  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
237  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
238 }
239 
240 /* ************************************************************************* */
241 TEST(ScenarioRunner, AcceleratingWithBias2) {
242  gttic(AcceleratingWithBias2);
243  using namespace accelerating2;
244  ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias);
245 
246  auto pim = runner.integrate(T, kNonZeroBias);
247  Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias);
248  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
249 }
250 
251 /* ************************************************************************* */
252 TEST(ScenarioRunner, AcceleratingAndRotating2) {
253  gttic(AcceleratingAndRotating2);
254  using namespace initial2;
255  const double a = 0.2; // m/s^2
256  const Vector3 A(0, a, 0), W(0, 0.1, 0);
257  const AcceleratingScenario scenario(nRb, P0, V0, A, W);
258 
259  const double T = 10; // seconds
260  ScenarioRunner runner(scenario, defaultParams(), T / 10);
261 
262  auto pim = runner.integrate(T);
263  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
264 
265  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
266  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
267  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
268 }
269 
270 /* ************************************************************************* */
271 namespace initial3 {
272 // Rotation only
273 // Set up body pointing towards y axis. The body itself has Z axis pointing down
274 const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
275 const Point3 P0(0,0,0);
276 const Vector3 V0(0, 0, 0);
277 }
278 
279 /* ************************************************************************* */
280 namespace accelerating3 {
281 using namespace initial3;
282 const double a = 0.2; // m/s^2
283 const Vector3 A(0, a, 0);
284 const AcceleratingScenario scenario(nRb, P0, V0, A);
285 
286 const double T = 3; // seconds
287 }
288 
289 /* ************************************************************************* */
290 TEST(ScenarioRunner, Accelerating3) {
291  gttic(Accelerating3);
292  using namespace accelerating3;
293  ScenarioRunner runner(scenario, defaultParams(), T / 10);
294 
295  auto pim = runner.integrate(T);
296  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
297 
298  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
299  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
300  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
301 }
302 
303 /* ************************************************************************* */
304 TEST(ScenarioRunner, AcceleratingWithBias3) {
305  gttic(AcceleratingWithBias3);
306  using namespace accelerating3;
307  ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias);
308 
309  auto pim = runner.integrate(T, kNonZeroBias);
310  Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias);
311  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
312 }
313 
314 /* ************************************************************************* */
315 TEST(ScenarioRunner, AcceleratingAndRotating3) {
316  gttic(AcceleratingAndRotating3);
317  using namespace initial3;
318  const double a = 0.2; // m/s^2
319  const Vector3 A(0, a, 0), W(0, 0.1, 0);
320  const AcceleratingScenario scenario(nRb, P0, V0, A, W);
321 
322  const double T = 10; // seconds
323  ScenarioRunner runner(scenario, defaultParams(), T / 10);
324 
325  auto pim = runner.integrate(T);
326  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
327 
328  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
329  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
330  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
331 }
332 
333 /* ************************************************************************* */
334 namespace initial4 {
335 // Both rotation and translation
336 // Set up body pointing towards y axis, and start at 10,20,0 with velocity
337 // going in X. The body itself has Z axis pointing down
338 const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
339 const Point3 P0(10, 20, 0);
340 const Vector3 V0(50, 0, 0);
341 }
342 
343 /* ************************************************************************* */
344 namespace accelerating4 {
345 using namespace initial4;
346 const double a = 0.2; // m/s^2
347 const Vector3 A(0, a, 0);
348 const AcceleratingScenario scenario(nRb, P0, V0, A);
349 
350 const double T = 3; // seconds
351 }
352 
353 /* ************************************************************************* */
354 TEST(ScenarioRunner, Accelerating4) {
355  gttic(Accelerating4);
356  using namespace accelerating4;
357  ScenarioRunner runner(scenario, defaultParams(), T / 10);
358 
359  auto pim = runner.integrate(T);
360  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
361 
362  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
363  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
364  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
365 }
366 
367 /* ************************************************************************* */
368 TEST(ScenarioRunner, AcceleratingWithBias4) {
369  gttic(AcceleratingWithBias4);
370  using namespace accelerating4;
371  ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias);
372 
373  auto pim = runner.integrate(T, kNonZeroBias);
374  Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias);
375  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
376 }
377 
378 /* ************************************************************************* */
379 TEST(ScenarioRunner, AcceleratingAndRotating4) {
380  gttic(AcceleratingAndRotating4);
381  using namespace initial4;
382  const double a = 0.2; // m/s^2
383  const Vector3 A(0, a, 0), W(0, 0.1, 0);
384  const AcceleratingScenario scenario(nRb, P0, V0, A, W);
385 
386  const double T = 10; // seconds
387  ScenarioRunner runner(scenario, defaultParams(), T / 10);
388 
389  auto pim = runner.integrate(T);
390  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
391 
392  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
393  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
394  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
395 }
396 
397 /* ************************************************************************* */
398 int main() {
399  TestResult tr;
400  auto result = TestRegistry::runAllTests(tr);
401 #ifdef ENABLE_TIMING
402  tictoc_print_();
403 #endif
404  return result;
405 }
406 /* ************************************************************************* */
gtsam::ScenarioRunner::estimateNoiseCovariance
Matrix6 estimateNoiseCovariance(size_t N=1000) const
Estimate covariance of sampled noise for sanity-check.
Definition: ScenarioRunner.cpp:85
main
int main()
Definition: testScenarioRunner.cpp:398
kDegree
static const double kDegree
Definition: testScenarioRunner.cpp:28
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::ScenarioRunner::estimateCovariance
Matrix9 estimateCovariance(double T, size_t N=1000, const Bias &estimatedBias=Bias()) const
Compute a Monte Carlo estimate of the predict covariance using N samples.
Definition: ScenarioRunner.cpp:55
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::ScenarioRunner::integrate
PreintegratedImuMeasurements integrate(double T, const Bias &estimatedBias=Bias(), bool corrupted=false) const
Integrate measurements for T seconds into a PIM.
Definition: ScenarioRunner.cpp:30
kRotBias
static const Vector3 kRotBias(0.1, 0, 0.3)
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
M_PI
#define M_PI
Definition: main.h:106
std
Definition: BFloat16.h:88
gtsam::symbol_shorthand::W
Key W(std::uint64_t j)
Definition: inference/Symbol.h:170
defaultParams
static std::shared_ptr< PreintegrationParams > defaultParams()
Definition: testScenarioRunner.cpp:39
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
forward::v
const double v
Definition: testScenarioRunner.cpp:80
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
accelerating4::scenario
const AcceleratingScenario scenario(nRb, P0, V0, A)
kAccelSigma
static const double kAccelSigma
Definition: testScenarioRunner.cpp:33
gtsam::ScenarioRunner::predict
NavState predict(const PreintegratedImuMeasurements &pim, const Bias &estimatedBias=Bias()) const
Predict predict given a PIM.
Definition: ScenarioRunner.cpp:49
EXPECT_NEAR
#define EXPECT_NEAR(a, b, c)
Definition: testScenarioRunner.cpp:47
gttic
#define gttic(label)
Definition: timing.h:295
gtsam::ConstantTwistScenario::pose
Pose3 pose(double t) const override
pose at time t
Definition: Scenario.h:67
result
Values result
Definition: OdometryOptimize.cpp:8
V
MatrixXcd V
Definition: EigenSolver_EigenSolver_MatrixType.cpp:15
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::tictoc_print_
void tictoc_print_()
Definition: timing.h:268
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
initial4::P0
const Point3 P0(10, 20, 0)
TEST
TEST(ScenarioRunner, Spin)
Definition: testScenarioRunner.cpp:50
initial4::nRb
const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1))
kAccBias
static const Vector3 kAccBias(0.2, 0, 0)
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
gtsam::AcceleratingScenario::pose
Pose3 pose(double t) const override
pose at time t
Definition: Scenario.h:92
gtsam
traits
Definition: chartTesting.h:28
ScenarioRunner.h
Simple class to test navigation scenarios.
p
float * p
Definition: Tutorial_Map_using.cpp:9
kNonZeroBias
static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias)
gtsam::NavState::pose
const Pose3 pose() const
Definition: NavState.h:86
initial4::V0
const Vector3 V0(50, 0, 0)
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
kDt
static const double kDt
Definition: testScenarioRunner.cpp:29
gtsam::AcceleratingScenario
Accelerating from an arbitrary initial state, with optional rotation.
Definition: Scenario.h:83
kGyroSigma
static const double kGyroSigma
Definition: testScenarioRunner.cpp:32
timing.h
Timing utilities.

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:39:18