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 testSimulated2D.cpp
14 * @brief Unit tests for simulated 2D measurement functions
15 * @author Christian Potthast
16 * @author Carlos Nieto
17 **/
18
19#include <iostream>
20#include <CppUnitLite/TestHarness.h>
21
22#include <gtsam/base/Testable.h>
23#include <gtsam/base/numericalDerivative.h>
24#include <tests/simulated2D.h>
25
26using namespace std;
27using namespace gtsam;
28using namespace simulated2D;
29
30/* ************************************************************************* */
31TEST( simulated2D, Simulated2DValues )
32{
33 simulated2D::Values actual;
34 actual.insert(j: 1,val: Point2(1,1));
35 actual.insert(j: 2,val: Point2(2,2));
36 EXPECT(assert_equal(actual,actual,1e-9));
37}
38
39/* ************************************************************************* */
40TEST( simulated2D, Dprior )
41{
42 Point2 x(1,-9);
43 Matrix numerical = numericalDerivative11(h: simulated2D::prior,x);
44 Matrix computed;
45 simulated2D::prior(x,H: computed);
46 EXPECT(assert_equal(numerical,computed,1e-9));
47}
48
49/* ************************************************************************* */
50 TEST( simulated2D, DOdo )
51{
52 Point2 x1(1,-9),x2(-5,6);
53 Matrix H1,H2;
54 simulated2D::odo(x1,x2,H1,H2);
55 Matrix A1 = numericalDerivative21(h: simulated2D::odo,x1,x2);
56 EXPECT(assert_equal(A1,H1,1e-9));
57 Matrix A2 = numericalDerivative22(h: simulated2D::odo,x1,x2);
58 EXPECT(assert_equal(A2,H2,1e-9));
59}
60
61/* ************************************************************************* */
62int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
63/* ************************************************************************* */
64