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 testGaussMarkov1stOrderFactor.cpp
14 * @brief Unit tests for the GaussMarkov1stOrder factor
15 * @author Vadim Indelman
16 * @date Jan 17, 2012
17 */
18
19#include <CppUnitLite/TestHarness.h>
20#include <gtsam/base/Vector.h>
21#include <gtsam/base/numericalDerivative.h>
22#include <gtsam/inference/Key.h>
23#include <gtsam/nonlinear/Values.h>
24#include <gtsam_unstable/slam/GaussMarkov1stOrderFactor.h>
25
26using namespace std::placeholders;
27using namespace std;
28using namespace gtsam;
29
30//! Factors
31typedef GaussMarkov1stOrderFactor<Vector3> GaussMarkovFactor;
32
33/* ************************************************************************* */
34Vector predictionError(const Vector& v1, const Vector& v2,
35 const GaussMarkovFactor factor) {
36 return factor.evaluateError(x: v1, x: v2);
37}
38
39/* ************************************************************************* */
40TEST( GaussMarkovFactor, equals )
41{
42 // Create two identical factors and make sure they're equal
43 Key x1(1);
44 Key x2(2);
45 double delta_t = 0.10;
46 Vector tau = Vector3(100.0, 150.0, 10.0);
47 SharedGaussian model = noiseModel::Isotropic::Sigma(dim: 3, sigma: 1.0);
48
49 GaussMarkovFactor factor1(x1, x2, delta_t, tau, model);
50 GaussMarkovFactor factor2(x1, x2, delta_t, tau, model);
51
52 CHECK(assert_equal(factor1, factor2));
53}
54
55/* ************************************************************************* */
56TEST( GaussMarkovFactor, error )
57{
58 Values linPoint;
59 Key x1(1);
60 Key x2(2);
61 double delta_t = 0.10;
62 Vector3 tau(100.0, 150.0, 10.0);
63 SharedGaussian model = noiseModel::Isotropic::Sigma(dim: 3, sigma: 1.0);
64
65 Vector3 v1(10.0, 12.0, 13.0);
66 Vector3 v2(10.0, 15.0, 14.0);
67
68 // Create two nodes
69 linPoint.insert(j: x1, val: v1);
70 linPoint.insert(j: x2, val: v2);
71
72 GaussMarkovFactor factor(x1, x2, delta_t, tau, model);
73 Vector3 error1 = factor.evaluateError(x: v1, x: v2);
74
75 // Manually calculate the error
76 Vector3 alpha(tau.size());
77 Vector3 alpha_v1(tau.size());
78 for(int i=0; i<tau.size(); i++){
79 alpha(i) = exp(x: - 1/tau(i)*delta_t );
80 alpha_v1(i) = alpha(i) * v1(i);
81 }
82 Vector3 error2 = v2 - alpha_v1;
83
84 CHECK(assert_equal(error1, error2, 1e-8));
85}
86
87/* ************************************************************************* */
88TEST (GaussMarkovFactor, jacobian ) {
89
90 Values linPoint;
91 Key x1(1);
92 Key x2(2);
93 double delta_t = 0.10;
94 Vector3 tau(100.0, 150.0, 10.0);
95 SharedGaussian model = noiseModel::Isotropic::Sigma(dim: 3, sigma: 1.0);
96
97 GaussMarkovFactor factor(x1, x2, delta_t, tau, model);
98
99 // Update the linearization point
100 Vector3 v1_upd(0.5, -0.7, 0.3);
101 Vector3 v2_upd(-0.7, 0.4, 0.9);
102
103 // Calculate the Jacobian matrix using the factor
104 Matrix computed_H1, computed_H2;
105 factor.evaluateError(x: v1_upd, x: v2_upd, H&: computed_H1, H&: computed_H2);
106
107 // Calculate the Jacobian matrices H1 and H2 using the numerical derivative function
108 Matrix numerical_H1, numerical_H2;
109 numerical_H1 = numericalDerivative21<Vector3, Vector3, Vector3>(
110 h: std::bind(f: &predictionError, args: std::placeholders::_1, args: std::placeholders::_2,
111 args&: factor),
112 x1: v1_upd, x2: v2_upd);
113 numerical_H2 = numericalDerivative22<Vector3, Vector3, Vector3>(
114 h: std::bind(f: &predictionError, args: std::placeholders::_1, args: std::placeholders::_2,
115 args&: factor),
116 x1: v1_upd, x2: v2_upd);
117
118 // Verify they are equal for this choice of state
119 CHECK(assert_equal(numerical_H1, computed_H1, 1e-9));
120 CHECK(assert_equal(numerical_H2, computed_H2, 1e-9));
121}
122
123/* ************************************************************************* */
124int main()
125{
126 TestResult tr; return TestRegistry::runAllTests(result&: tr);
127}
128/* ************************************************************************* */
129
130