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 testRFID.cpp
14 * @brief Unit tests for the RFID factor
15 * @author Stephen Williams (swilliams8@gatech.edu)
16 * @date Jan 16, 2012
17 */
18
19#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
20#include <gtsam/base/numericalDerivative.h>
21#include <gtsam/geometry/Point3.h>
22#include <gtsam/inference/Key.h>
23#include <gtsam/nonlinear/NonlinearFactorGraph.h>
24#include <gtsam/nonlinear/Values.h>
25#include <gtsam/slam/BetweenFactor.h>
26#include <CppUnitLite/TestHarness.h>
27
28using namespace gtsam;
29
30/* ************************************************************************* */
31TEST( LinearizedFactor, equals_jacobian )
32{
33 // Create a Between Factor from a Point3. This is actually a linear factor.
34 Key x1(1);
35 Key x2(2);
36 Values values;
37 values.insert(j: x1, val: Point3(-22.4, +8.5, +2.4));
38 values.insert(j: x2, val: Point3(-21.0, +5.0, +21.0));
39
40 Point3 measured(1.0, -2.5, 17.8);
41 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1);
42 BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
43
44
45 // Create two identical factors and make sure they're equal
46 JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(r: betweenFactor.linearize(x: values));
47 LinearizedJacobianFactor jacobian1(jf, values);
48 LinearizedJacobianFactor jacobian2(jf, values);
49
50 CHECK(assert_equal(jacobian1, jacobian2));
51}
52
53/* ************************************************************************* */
54TEST( LinearizedFactor, clone_jacobian )
55{
56 // Create a Between Factor from a Point3. This is actually a linear factor.
57 Key x1(1);
58 Key x2(2);
59 Values values;
60 values.insert(j: x1, val: Point3(-22.4, +8.5, +2.4));
61 values.insert(j: x2, val: Point3(-21.0, +5.0, +21.0));
62
63 Point3 measured(1.0, -2.5, 17.8);
64 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1);
65 BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
66
67 // Create one factor that is a clone of the other and make sure they're equal
68 JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(r: betweenFactor.linearize(x: values));
69 LinearizedJacobianFactor jacobian1(jf, values);
70 LinearizedJacobianFactor::shared_ptr jacobian2 = std::static_pointer_cast<LinearizedJacobianFactor>(r: jacobian1.clone());
71 CHECK(assert_equal(jacobian1, *jacobian2));
72
73 JacobianFactor::shared_ptr jf1 = std::static_pointer_cast<JacobianFactor>(r: jacobian1.linearize(c: values));
74 JacobianFactor::shared_ptr jf2 = std::static_pointer_cast<JacobianFactor>(r: jacobian2->linearize(c: values));
75 CHECK(assert_equal(*jf1, *jf2));
76
77 Matrix information1 = jf1->augmentedInformation();
78 Matrix information2 = jf2->augmentedInformation();
79 CHECK(assert_equal(information1, information2));
80}
81
82/* ************************************************************************* */
83TEST( LinearizedFactor, add_jacobian )
84{
85 // Create a Between Factor from a Point3. This is actually a linear factor.
86 Key x1(1);
87 Key x2(2);
88 Values values;
89 values.insert(j: x1, val: Point3(-22.4, +8.5, +2.4));
90 values.insert(j: x2, val: Point3(-21.0, +5.0, +21.0));
91
92 Point3 measured(1.0, -2.5, 17.8);
93 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1);
94 BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
95
96 // Create two factor graphs, one using 'push_back' and one using 'add' and make sure they're equal
97 JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(r: betweenFactor.linearize(x: values));
98 LinearizedJacobianFactor::shared_ptr jacobian(new LinearizedJacobianFactor(jf, values));
99 NonlinearFactorGraph graph1; graph1.push_back(factor: jacobian);
100 NonlinearFactorGraph graph2; graph2.push_back(factor: *jacobian);
101
102 // TODO: When creating a Jacobian from a cached factor, I experienced a problem in the 'add' version
103 // However, I am currently unable to reproduce the error in this unit test.
104 // I don't know if this affects the Hessian version as well.
105 CHECK(assert_equal(graph1, graph2));
106}
107
108///* ************************************************************************* */
109//TEST( LinearizedFactor, error_jacobian )
110//{
111// // Create a Between Factor from a Point3. This is actually a linear factor.
112// Key key1(1);
113// Key key2(2);
114// Ordering ordering;
115// ordering.push_back(key1);
116// ordering.push_back(key2);
117// Values values;
118// values.insert(key1, Point3(-22.4, +8.5, +2.4));
119// values.insert(key2, Point3(-21.0, +5.0, +21.0));
120//
121// Point3 measured(1.0, -2.5, 17.8);
122// SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1);
123// BetweenFactor<Point3> betweenFactor(key1, key2, measured, model);
124//
125//
126// // Create a linearized jacobian factors
127// JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
128// LinearizedJacobianFactor jacobian(jf, values);
129//
130//
131// for(double x1 = -10; x1 < 10; x1 += 2.0) {
132// for(double y1 = -10; y1 < 10; y1 += 2.0) {
133// for(double z1 = -10; z1 < 10; z1 += 2.0) {
134//
135// for(double x2 = -10; x2 < 10; x2 += 2.0) {
136// for(double y2 = -10; y2 < 10; y2 += 2.0) {
137// for(double z2 = -10; z2 < 10; z2 += 2.0) {
138//
139// Values linpoint;
140// linpoint.insert(key1, Point3(x1, y1, z1));
141// linpoint.insert(key2, Point3(x2, y2, z2));
142//
143// // Check that the error of the Linearized Jacobian and the original factor match
144// // This only works because a BetweenFactor on a Point3 is actually a linear system
145// double expected_error = betweenFactor.error(linpoint);
146// double actual_error = jacobian.error(linpoint);
147// EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 );
148//
149// // Check that the linearized factors are identical
150// GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint);
151// GaussianFactor::shared_ptr actual_factor = jacobian.linearize(linpoint);
152// CHECK(assert_equal(*expected_factor, *actual_factor));
153// }
154// }
155// }
156//
157// }
158// }
159// }
160//
161//}
162
163/* ************************************************************************* */
164TEST( LinearizedFactor, equals_hessian )
165{
166 // Create a Between Factor from a Point3. This is actually a linear factor.
167 Key x1(1);
168 Key x2(2);
169 Values values;
170 values.insert(j: x1, val: Point3(-22.4, +8.5, +2.4));
171 values.insert(j: x2, val: Point3(-21.0, +5.0, +21.0));
172
173 Point3 measured(1.0, -2.5, 17.8);
174 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1);
175 BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
176
177
178 // Create two identical factors and make sure they're equal
179 JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(r: betweenFactor.linearize(x: values));
180 HessianFactor::shared_ptr hf(new HessianFactor(*jf));
181 LinearizedHessianFactor hessian1(hf, values);
182 LinearizedHessianFactor hessian2(hf, values);
183
184 CHECK(assert_equal(hessian1, hessian2));
185}
186
187/* ************************************************************************* */
188TEST( LinearizedFactor, clone_hessian )
189{
190 // Create a Between Factor from a Point3. This is actually a linear factor.
191 Key x1(1);
192 Key x2(2);
193 Values values;
194 values.insert(j: x1, val: Point3(-22.4, +8.5, +2.4));
195 values.insert(j: x2, val: Point3(-21.0, +5.0, +21.0));
196
197 Point3 measured(1.0, -2.5, 17.8);
198 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1);
199 BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
200
201
202 // Create two identical factors and make sure they're equal
203 JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(r: betweenFactor.linearize(x: values));
204 HessianFactor::shared_ptr hf(new HessianFactor(*jf));
205 LinearizedHessianFactor hessian1(hf, values);
206 LinearizedHessianFactor::shared_ptr hessian2 = std::static_pointer_cast<LinearizedHessianFactor>(r: hessian1.clone());
207
208 CHECK(assert_equal(hessian1, *hessian2));
209}
210
211/* ************************************************************************* */
212TEST( LinearizedFactor, add_hessian )
213{
214 // Create a Between Factor from a Point3. This is actually a linear factor.
215 Key x1(1);
216 Key x2(2);
217 Values values;
218 values.insert(j: x1, val: Point3(-22.4, +8.5, +2.4));
219 values.insert(j: x2, val: Point3(-21.0, +5.0, +21.0));
220
221 Point3 measured(1.0, -2.5, 17.8);
222 SharedNoiseModel model = noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1);
223 BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
224
225
226 // Create two identical factors and make sure they're equal
227 JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(r: betweenFactor.linearize(x: values));
228 HessianFactor::shared_ptr hf(new HessianFactor(*jf));
229 LinearizedHessianFactor::shared_ptr hessian(new LinearizedHessianFactor(hf, values));
230 NonlinearFactorGraph graph1; graph1.push_back(factor: hessian);
231 NonlinearFactorGraph graph2; graph2.push_back(factor: *hessian);
232
233 CHECK(assert_equal(graph1, graph2));
234}
235
236///* ************************************************************************* */
237//TEST( LinearizedFactor, error_hessian )
238//{
239// // Create a Between Factor from a Point3. This is actually a linear factor.
240// Key key1(1);
241// Key key2(2);
242// Ordering ordering;
243// ordering.push_back(key1);
244// ordering.push_back(key2);
245// Values values;
246// values.insert(key1, Point3(-22.4, +8.5, +2.4));
247// values.insert(key2, Point3(-21.0, +5.0, +21.0));
248//
249// Point3 measured(1.0, -2.5, 17.8);
250// SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1);
251// BetweenFactor<Point3> betweenFactor(key1, key2, measured, model);
252//
253//
254// // Create a linearized hessian factor
255// JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
256// HessianFactor::shared_ptr hf(new HessianFactor(*jf));
257// LinearizedHessianFactor hessian(hf, values);
258//
259//
260// for(double x1 = -10; x1 < 10; x1 += 2.0) {
261// for(double y1 = -10; y1 < 10; y1 += 2.0) {
262// for(double z1 = -10; z1 < 10; z1 += 2.0) {
263//
264// for(double x2 = -10; x2 < 10; x2 += 2.0) {
265// for(double y2 = -10; y2 < 10; y2 += 2.0) {
266// for(double z2 = -10; z2 < 10; z2 += 2.0) {
267//
268// Values linpoint;
269// linpoint.insert(key1, Point3(x1, y1, z1));
270// linpoint.insert(key2, Point3(x2, y2, z2));
271//
272// // Check that the error of the Linearized Hessian and the original factor match
273// // This only works because a BetweenFactor on a Point3 is actually a linear system
274// double expected_error = betweenFactor.error(linpoint);
275// double actual_error = hessian.error(linpoint);
276// EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 );
277//
278// // Check that the linearized factors are identical
279// GaussianFactor::shared_ptr expected_factor = HessianFactor::shared_ptr(new HessianFactor(*betweenFactor.linearize(linpoint)));
280// GaussianFactor::shared_ptr actual_factor = hessian.linearize(linpoint);
281// CHECK(assert_equal(*expected_factor, *actual_factor));
282// }
283// }
284// }
285//
286// }
287// }
288// }
289//
290//}
291
292/* ************************************************************************* */
293int main()
294{
295 TestResult tr; return TestRegistry::runAllTests(result&: tr);
296}
297/* ************************************************************************* */
298
299