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 testSimulated2DOriented.cpp
14 * @date Jun 10, 2010
15 * @author nikai
16 * @brief unit tests for simulated2DOriented
17 */
18
19#include <tests/simulated2D.h>
20#include <tests/simulated2DOriented.h>
21#include <gtsam/inference/Symbol.h>
22#include <gtsam/base/Testable.h>
23#include <gtsam/base/numericalDerivative.h>
24
25using namespace std;
26using namespace gtsam;
27
28#include <iostream>
29#include <CppUnitLite/TestHarness.h>
30
31// Convenience for named keys
32using symbol_shorthand::X;
33using symbol_shorthand::L;
34
35/* ************************************************************************* */
36TEST( simulated2DOriented, Dprior )
37{
38 Pose2 x(1,-9, 0.1);
39 Matrix numerical = numericalDerivative11(h: simulated2DOriented::prior,x);
40 Matrix computed;
41 simulated2DOriented::prior(x,H: computed);
42 CHECK(assert_equal(numerical,computed,1e-9));
43}
44
45/* ************************************************************************* */
46 TEST( simulated2DOriented, DOdo )
47{
48 Pose2 x1(1,-9,0.1),x2(-5,6,0.2);
49 Matrix H1,H2;
50 simulated2DOriented::odo(x1,x2,H1,H2);
51 Matrix A1 = numericalDerivative21(h: simulated2DOriented::odo,x1,x2);
52 CHECK(assert_equal(A1,H1,1e-9));
53 Matrix A2 = numericalDerivative22(h: simulated2DOriented::odo,x1,x2);
54 CHECK(assert_equal(A2,H2,1e-9));
55}
56
57/* ************************************************************************* */
58TEST( simulated2DOriented, constructor )
59{
60 Pose2 measurement(0.2, 0.3, 0.1);
61 SharedDiagonal model = noiseModel::Diagonal::Sigmas(sigmas: Vector3(1., 1., 1.));
62 simulated2DOriented::Odometry factor(measurement, model, X(j: 1), X(j: 2));
63
64 simulated2DOriented::Values config;
65 config.insert(j: X(j: 1), val: Pose2(1., 0., 0.2));
66 config.insert(j: X(j: 2), val: Pose2(2., 0., 0.1));
67 std::shared_ptr<GaussianFactor> lf = factor.linearize(x: config);
68}
69
70/* ************************************************************************* */
71int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
72/* ************************************************************************* */
73