testJacobianFactor.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 
19 #include <gtsam/base/TestableAssertions.h>
20 #include <CppUnitLite/TestHarness.h>
21 
22 #include <gtsam/inference/VariableSlots.h>
23 #include <gtsam/linear/JacobianFactor.h>
24 #include <gtsam/linear/GaussianFactorGraph.h>
25 #include <gtsam/linear/GaussianConditional.h>
26 #include <gtsam/linear/VectorValues.h>
27 
28 using namespace std;
29 using namespace gtsam;
30 
31 using Dims = std::vector<Eigen::Index>; // For constructing block matrices
32 
33 namespace {
34  namespace simple {
35  // Terms we'll use
36  using Terms = vector<pair<Key, Matrix> >;
37  const Terms terms{{5, I_3x3}, {10, 2 * I_3x3}, {15, 3 * I_3x3}};
38 
39  // RHS and sigmas
40  const Vector b = Vector3(1., 2., 3.);
41  const SharedDiagonal noise =
42  noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.5));
43  }
44 }
45 
46 /* ************************************************************************* */
47 TEST(JacobianFactor, constructors_and_accessors)
48 {
49  using namespace simple;
50 
51  // Test for using different numbers of terms
52  {
53  // b vector only constructor
54  JacobianFactor expected(Terms{}, b);
55  JacobianFactor actual(b);
56  EXPECT(assert_equal(expected, actual));
57  EXPECT(assert_equal(b, expected.getb()));
58  EXPECT(assert_equal(b, actual.getb()));
59  EXPECT(!expected.get_model());
60  EXPECT(!actual.get_model());
61  }
62  {
63  // One term constructor
64  JacobianFactor expected(Terms{terms[0]}, b, noise);
65  JacobianFactor actual(terms[0].first, terms[0].second, b, noise);
66  EXPECT(assert_equal(expected, actual));
67  LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back());
68  // Key iterator
69  EXPECT(assert_equal(terms[0].second, actual.getA(actual.end() - 1)));
70  // Key
71  EXPECT(assert_equal(terms[0].second, actual.getA(terms[0].first)));
72  EXPECT(assert_equal(b, expected.getb()));
73  EXPECT(assert_equal(b, actual.getb()));
74  EXPECT(noise == expected.get_model());
75  EXPECT(noise == actual.get_model());
76  }
77  {
78  // Two term constructor
79  JacobianFactor expected(Terms{terms[0], terms[1]}, b, noise);
80  JacobianFactor actual(terms[0].first, terms[0].second,
81  terms[1].first, terms[1].second, b, noise);
82  EXPECT(assert_equal(expected, actual));
83  LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back());
84  // Key iterator
85  EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1)));
86  // Key
87  EXPECT(assert_equal(terms[1].second, actual.getA(terms[1].first)));
88  EXPECT(assert_equal(b, expected.getb()));
89  EXPECT(assert_equal(b, actual.getb()));
90  EXPECT(noise == expected.get_model());
91  EXPECT(noise == actual.get_model());
92  }
93  {
94  // Three term constructor
95  JacobianFactor expected(Terms{terms[0], terms[1], terms[2]}, b, noise);
96  JacobianFactor actual(terms[0].first, terms[0].second,
97  terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
98  EXPECT(assert_equal(expected, actual));
99  LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
100  // Key iterator
101  EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1)));
102  // Key
103  EXPECT(assert_equal(terms[2].second, actual.getA(terms[2].first)));
104  EXPECT(assert_equal(b, expected.getb()));
105  EXPECT(assert_equal(b, actual.getb()));
106  EXPECT(noise == expected.get_model());
107  EXPECT(noise == actual.get_model());
108  }
109  {
110  // Test three-term constructor with std::map
111  JacobianFactor expected(Terms{terms[0], terms[1], terms[2]}, b, noise);
112  map<Key,Matrix> mapTerms;
113  // note order of insertion plays no role: order will be determined by keys
114  mapTerms.insert(terms[2]);
115  mapTerms.insert(terms[1]);
116  mapTerms.insert(terms[0]);
117  JacobianFactor actual(mapTerms, b, noise);
118  EXPECT(assert_equal(expected, actual));
119  LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
120  EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1)));
121  EXPECT(assert_equal(b, expected.getb()));
122  EXPECT(assert_equal(b, actual.getb()));
123  EXPECT(noise == expected.get_model());
124  EXPECT(noise == actual.get_model());
125  }
126  {
127  // VerticalBlockMatrix constructor
128  JacobianFactor expected(Terms{terms[0], terms[1], terms[2]}, b, noise);
129  VerticalBlockMatrix blockMatrix(Dims{3, 3, 3, 1}, 3);
130  blockMatrix(0) = terms[0].second;
131  blockMatrix(1) = terms[1].second;
132  blockMatrix(2) = terms[2].second;
133  blockMatrix(3) = b;
134  // get a vector of keys from the terms
135  vector<Key> keys;
136  for (const auto& term : terms)
137  keys.push_back(term.first);
138  JacobianFactor actual(keys, blockMatrix, noise);
139  EXPECT(assert_equal(expected, actual));
140  LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
141  EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1)));
142  EXPECT(assert_equal(b, expected.getb()));
143  EXPECT(assert_equal(b, actual.getb()));
144  EXPECT(noise == expected.get_model());
145  EXPECT(noise == actual.get_model());
146  }
147 }
148 
149 /* ************************************************************************* */
150 TEST(JabobianFactor, Hessian_conversion) {
151  HessianFactor hessian(0, (Matrix(4, 4) <<
152  1.57, 2.695, -1.1, -2.35,
153  2.695, 11.3125, -0.65, -10.225,
154  -1.1, -0.65, 1, 0.5,
155  -2.35, -10.225, 0.5, 9.25).finished(),
156  (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(),
157  73.1725);
158 
159  JacobianFactor expected(0, (Matrix(2, 4) <<
160  1.2530, 2.1508, -0.8779, -1.8755,
161  0, 2.5858, 0.4789, -2.3943).finished(),
162  Vector2(-6.2929, -5.7941));
163 
164  EXPECT(assert_equal(expected, JacobianFactor(hessian), 1e-3));
165 }
166 
167 /* ************************************************************************* */
168 TEST(JabobianFactor, Hessian_conversion2) {
169  JacobianFactor jf(0, (Matrix(3, 3) <<
170  1, 2, 3,
171  0, 2, 3,
172  0, 0, 3).finished(),
173  Vector3(1, 2, 2));
174  HessianFactor hessian(jf);
175  EXPECT(assert_equal(jf, JacobianFactor(hessian), 1e-9));
176 }
177 
178 /* ************************************************************************* */
179 TEST(JabobianFactor, Hessian_conversion3) {
180  JacobianFactor jf(0, (Matrix(2, 4) <<
181  1, 2, 3, 0,
182  0, 3, 2, 1).finished(),
183  Vector2(1, 2));
184  HessianFactor hessian(jf);
185  EXPECT(assert_equal(jf, JacobianFactor(hessian), 1e-9));
186 }
187 
188 /* ************************************************************************* */
189 namespace simple_graph {
190 
191 Key keyX(10), keyY(8), keyZ(12);
192 
193 double sigma1 = 0.1;
194 Matrix A11 = I_2x2;
195 Vector2 b1(2, -1);
196 auto factor1 = std::make_shared<JacobianFactor>(
197  keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1));
198 
199 double sigma2 = 0.5;
200 Matrix A21 = -2 * I_2x2;
201 Matrix A22 = 3 * I_2x2;
202 Vector2 b2(4, -5);
203 auto factor2 = std::make_shared<JacobianFactor>(
204  keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2));
205 
206 double sigma3 = 1.0;
207 Matrix A32 = -4 * I_2x2;
208 Matrix A33 = 5 * I_2x2;
209 Vector2 b3(3, -6);
210 auto factor3 = std::make_shared<JacobianFactor>(
211  keyY, A32, keyZ, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3));
212 
213 const GaussianFactorGraph factors { factor1, factor2, factor3 };
214 const Ordering ordering { keyX, keyY, keyZ };
215 }
216 
217 /* ************************************************************************* */
218 TEST( JacobianFactor, construct_from_graph)
219 {
220  using namespace simple_graph;
221 
222  Matrix A1(6,2); A1 << A11, A21, Z_2x2;
223  Matrix A2(6,2); A2 << Z_2x2, A22, A32;
224  Matrix A3(6,2); A3 << Matrix::Zero(4,2), A33;
225  Vector b(6); b << b1, b2, b3;
226  Vector sigmas(6); sigmas << sigma1, sigma1, sigma2, sigma2, sigma3, sigma3;
227  JacobianFactor expected(keyX, A1, keyY, A2, keyZ, A3, b, noiseModel::Diagonal::Sigmas(sigmas));
228 
229  // The ordering here specifies the order in which the variables will appear in the combined factor
230  JacobianFactor actual(factors, ordering);
231 
232  EXPECT(assert_equal(expected, actual));
233 }
234 
235 /* ************************************************************************* */
236 TEST(JacobianFactor, error)
237 {
238  JacobianFactor factor(simple::terms, simple::b, simple::noise);
239 
240  VectorValues values;
241  values.insert(5, Vector::Constant(3, 1.0));
242  values.insert(10, Vector::Constant(3, 0.5));
243  values.insert(15, Vector::Constant(3, 1.0/3.0));
244 
245  Vector expected_unwhitened(3); expected_unwhitened << 2.0, 1.0, 0.0;
246  Vector actual_unwhitened = factor.unweighted_error(values);
247  EXPECT(assert_equal(expected_unwhitened, actual_unwhitened));
248 
249  Vector expected_whitened(3); expected_whitened << 4.0, 2.0, 0.0;
250  Vector actual_whitened = factor.error_vector(values);
251  EXPECT(assert_equal(expected_whitened, actual_whitened));
252 
253  double expected_error = 0.5 * expected_whitened.squaredNorm();
254  double actual_error = factor.error(values);
255  DOUBLES_EQUAL(expected_error, actual_error, 1e-10);
256 }
257 
258 /* ************************************************************************* */
259 TEST(JacobianFactor, matrices_NULL)
260 {
261  // Make sure everything works with nullptr noise model
262  JacobianFactor factor(simple::terms, simple::b);
263 
264  Matrix jacobianExpected(3, 9);
265  jacobianExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second;
266  Vector rhsExpected = simple::b;
267  Matrix augmentedJacobianExpected(3, 10);
268  augmentedJacobianExpected << jacobianExpected, rhsExpected;
269 
270  Matrix augmentedHessianExpected =
271  augmentedJacobianExpected.transpose() * augmentedJacobianExpected;
272 
273  // Hessian
274  EXPECT(assert_equal(Matrix(augmentedHessianExpected.topLeftCorner(9,9)), factor.information()));
275  EXPECT(assert_equal(augmentedHessianExpected, factor.augmentedInformation()));
276 
277  // Whitened Jacobian
278  EXPECT(assert_equal(jacobianExpected, factor.jacobian().first));
279  EXPECT(assert_equal(rhsExpected, factor.jacobian().second));
280  EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobian()));
281 
282  // Unwhitened Jacobian
283  EXPECT(assert_equal(jacobianExpected, factor.jacobianUnweighted().first));
284  EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second));
285  EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted()));
286 
287  // hessianDiagonal
288  VectorValues expectDiagonal;
289  expectDiagonal.insert(5, Vector::Ones(3));
290  expectDiagonal.insert(10, 4*Vector::Ones(3));
291  expectDiagonal.insert(15, 9*Vector::Ones(3));
292  EXPECT(assert_equal(expectDiagonal, factor.hessianDiagonal()));
293 
294  // hessianBlockDiagonal
295  map<Key,Matrix> actualBD = factor.hessianBlockDiagonal();
296  LONGS_EQUAL(3,actualBD.size());
297  EXPECT(assert_equal(1*I_3x3,actualBD[5]));
298  EXPECT(assert_equal(4*I_3x3,actualBD[10]));
299  EXPECT(assert_equal(9*I_3x3,actualBD[15]));
300 }
301 
302 /* ************************************************************************* */
303 TEST(JacobianFactor, matrices)
304 {
305  // And now witgh a non-unit noise model
306  JacobianFactor factor(simple::terms, simple::b, simple::noise);
307 
308  Matrix jacobianExpected(3, 9);
309  jacobianExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second;
310  Vector rhsExpected = simple::b;
311  Matrix augmentedJacobianExpected(3, 10);
312  augmentedJacobianExpected << jacobianExpected, rhsExpected;
313 
314  Matrix augmentedHessianExpected =
315  augmentedJacobianExpected.transpose() * simple::noise->R().transpose()
316  * simple::noise->R() * augmentedJacobianExpected;
317 
318  // Hessian
319  EXPECT(assert_equal(Matrix(augmentedHessianExpected.topLeftCorner(9,9)), factor.information()));
320  EXPECT(assert_equal(augmentedHessianExpected, factor.augmentedInformation()));
321 
322  // Whitened Jacobian
323  EXPECT(assert_equal(simple::noise->R() * jacobianExpected, factor.jacobian().first));
324  EXPECT(assert_equal(simple::noise->R() * rhsExpected, factor.jacobian().second));
325  EXPECT(assert_equal(simple::noise->R() * augmentedJacobianExpected, factor.augmentedJacobian()));
326 
327  // Unwhitened Jacobian
328  EXPECT(assert_equal(jacobianExpected, factor.jacobianUnweighted().first));
329  EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second));
330  EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted()));
331 
332  // hessianDiagonal
333  VectorValues expectDiagonal;
334  // below we divide by the variance 0.5^2
335  expectDiagonal.insert(5, Vector3(1, 1, 1)/0.25);
336  expectDiagonal.insert(10, Vector3(4, 4, 4)/0.25);
337  expectDiagonal.insert(15, Vector3(9, 9, 9)/0.25);
338  EXPECT(assert_equal(expectDiagonal, factor.hessianDiagonal()));
339 
340  // hessianBlockDiagonal
341  map<Key,Matrix> actualBD = factor.hessianBlockDiagonal();
342  LONGS_EQUAL(3,actualBD.size());
343  EXPECT(assert_equal(4*I_3x3,actualBD[5]));
344  EXPECT(assert_equal(16*I_3x3,actualBD[10]));
345  EXPECT(assert_equal(36*I_3x3,actualBD[15]));
346 }
347 
348 /* ************************************************************************* */
349 TEST(JacobianFactor, operators )
350 {
351  const double sigma = 0.1;
352  SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2, sigma);
353 
354  Matrix I = I_2x2;
355  Vector b = Vector2(0.2,-0.1);
356  JacobianFactor lf(1, -I, 2, I, b, sigma0_1);
357 
358  VectorValues x;
359  Vector2 x1(10,20), x2(30,60);
360  x.insert(1, x1);
361  x.insert(2, x2);
362 
363  // test A*x
364  Vector expectedE = (x2 - x1)/sigma;
365  Vector actualE = lf * x;
366  EXPECT(assert_equal(expectedE, actualE));
367 
368  // test A^e
369  VectorValues expectedX;
370  const double alpha = 0.5;
371  expectedX.insert(1, - alpha * expectedE /sigma);
372  expectedX.insert(2, alpha * expectedE /sigma);
373  VectorValues actualX = VectorValues::Zero(expectedX);
374  lf.transposeMultiplyAdd(alpha, actualE, actualX);
375  EXPECT(assert_equal(expectedX, actualX));
376 
377  // test gradient at zero
378  const auto [A, b2] = lf.jacobian();
379  VectorValues expectedG;
380  expectedG.insert(1, Vector2(20,-10));
381  expectedG.insert(2, Vector2(-20, 10));
382  KeyVector keys {1, 2};
383  EXPECT(assert_equal(-A.transpose()*b2, expectedG.vector(keys)));
384  VectorValues actualG = lf.gradientAtZero();
385  EXPECT(assert_equal(expectedG, actualG));
386 }
387 
388 /* ************************************************************************* */
389 TEST(JacobianFactor, default_error )
390 {
391  JacobianFactor f;
392  double actual = f.error(VectorValues());
393  DOUBLES_EQUAL(0.0, actual, 1e-15);
394 }
395 
396 //* ************************************************************************* */
397 TEST(JacobianFactor, empty )
398 {
399  // create an empty factor
400  JacobianFactor f;
401  EXPECT(f.empty());
402 }
403 
404 /* ************************************************************************* */
405 TEST(JacobianFactor, eliminate)
406 {
407  Matrix A01 = (Matrix(3, 3) <<
408  1.0, 0.0, 0.0,
409  0.0, 1.0, 0.0,
410  0.0, 0.0, 1.0).finished();
411  Vector3 b0(1.5, 1.5, 1.5);
412  Vector3 s0(1.6, 1.6, 1.6);
413 
414  Matrix A10 = (Matrix(3, 3) <<
415  2.0, 0.0, 0.0,
416  0.0, 2.0, 0.0,
417  0.0, 0.0, 2.0).finished();
418  Matrix A11 = (Matrix(3, 3) <<
419  -2.0, 0.0, 0.0,
420  0.0, -2.0, 0.0,
421  0.0, 0.0, -2.0).finished();
422  Vector3 b1(2.5, 2.5, 2.5);
423  Vector3 s1(2.6, 2.6, 2.6);
424 
425  Matrix A21 = (Matrix(3, 3) <<
426  3.0, 0.0, 0.0,
427  0.0, 3.0, 0.0,
428  0.0, 0.0, 3.0).finished();
429  Vector3 b2(3.5, 3.5, 3.5);
430  Vector3 s2(3.6, 3.6, 3.6);
431 
432  GaussianFactorGraph gfg;
433  gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
434  gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
435  gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
436 
437  Matrix zero3x3 = Matrix::Zero(3,3);
438  Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
439  Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
440  Vector9 b; b << b1, b0, b2;
441  Vector9 sigmas; sigmas << s1, s0, s2;
442 
443  JacobianFactor combinedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
444  GaussianFactorGraph::EliminationResult expected = combinedFactor.eliminate(Ordering{0});
445  JacobianFactor::shared_ptr expectedJacobian = std::dynamic_pointer_cast<
446  JacobianFactor>(expected.second);
447 
448  GaussianFactorGraph::EliminationResult actual = EliminateQR(gfg, Ordering{0});
449  JacobianFactor::shared_ptr actualJacobian = std::dynamic_pointer_cast<
450  JacobianFactor>(actual.second);
451 
452  EXPECT(assert_equal(*expected.first, *actual.first));
453  EXPECT(assert_equal(*expectedJacobian, *actualJacobian));
454 }
455 
456 /* ************************************************************************* */
457 TEST(JacobianFactor, eliminate2 )
458 {
459  // sigmas
460  double sigma1 = 0.2;
461  double sigma2 = 0.1;
462  Vector sigmas = (Vector(4) << sigma1, sigma1, sigma2, sigma2).finished();
463 
464  // the combined linear factor
465  Matrix Ax2 = (Matrix(4, 2) <<
466  // x2
467  -1., 0.,
468  +0.,-1.,
469  1., 0.,
470  +0.,1.
471  ).finished();
472 
473  Matrix Al1x1 = (Matrix(4, 4) <<
474  // l1 x1
475  1., 0., 0.00, 0., // f4
476  0., 1., 0.00, 0., // f4
477  0., 0., -1., 0., // f2
478  0., 0., 0.00,-1. // f2
479  ).finished();
480 
481  // the RHS
482  Vector b2(4);
483  b2(0) = -0.2;
484  b2(1) = 0.3;
485  b2(2) = 0.2;
486  b2(3) = -0.1;
487 
488  vector<pair<Key, Matrix> > meas;
489  meas.push_back(make_pair(2, Ax2));
490  meas.push_back(make_pair(11, Al1x1));
491  JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));
492 
493  // eliminate the combined factor
494  pair<GaussianConditional::shared_ptr, JacobianFactor::shared_ptr>
495  actual = combined.eliminate(Ordering{2});
496 
497  // create expected Conditional Gaussian
498  double oldSigma = 0.0894427; // from when R was made unit
499  Matrix R11 = (Matrix(2, 2) <<
500  1.00, 0.00,
501  0.00, 1.00
502  ).finished()/oldSigma;
503  Matrix S12 = (Matrix(2, 4) <<
504  -0.20, 0.00,-0.80, 0.00,
505  +0.00,-0.20,+0.00,-0.80
506  ).finished()/oldSigma;
507  Vector d = Vector2(0.2,-0.14)/oldSigma;
508  GaussianConditional expectedCG(2, d, R11, 11, S12, noiseModel::Unit::Create(2));
509 
510  EXPECT(assert_equal(expectedCG, *actual.first, 1e-4));
511 
512  // the expected linear factor
513  double sigma = 0.2236;
514  Matrix Bl1x1 = (Matrix(2, 4) <<
515  // l1 x1
516  1.00, 0.00, -1.00, 0.00,
517  0.00, 1.00, +0.00, -1.00
518  ).finished()/sigma;
519  Vector b1 = Vector2(0.0, 0.894427);
520  JacobianFactor expectedLF(11, Bl1x1, b1, noiseModel::Unit::Create(2));
521  EXPECT(assert_equal(expectedLF, *actual.second,1e-3));
522 }
523 
524 /* ************************************************************************* */
525 TEST(JacobianFactor, EliminateQR)
526 {
527  // Augmented Ab test case for whole factor graph
528  Matrix Ab = (Matrix(14, 11) <<
529  4., 0., 1., 4., 1., 0., 3., 6., 8., 8., 1.,
530  9., 2., 0., 1., 6., 3., 9., 6., 6., 9., 4.,
531  5., 3., 7., 9., 5., 5., 9., 1., 3., 7., 0.,
532  5., 6., 5., 7., 9., 4., 0., 1., 1., 3., 5.,
533  0., 0., 4., 5., 6., 6., 7., 9., 4., 5., 4.,
534  0., 0., 9., 4., 8., 6., 2., 1., 4., 1., 6.,
535  0., 0., 6., 0., 4., 2., 4., 0., 1., 9., 6.,
536  0., 0., 6., 6., 4., 4., 5., 5., 5., 8., 6.,
537  0., 0., 0., 0., 8., 0., 9., 8., 2., 8., 0.,
538  0., 0., 0., 0., 0., 9., 4., 6., 3., 2., 0.,
539  0., 0., 0., 0., 1., 1., 9., 1., 5., 5., 3.,
540  0., 0., 0., 0., 1., 1., 3., 3., 2., 0., 5.,
541  0., 0., 0., 0., 0., 0., 0., 0., 2., 4., 6.,
542  0., 0., 0., 0., 0., 0., 0., 0., 6., 3., 4.).finished();
543 
544  // Create factor graph
545  const SharedDiagonal sig_4D = noiseModel::Isotropic::Sigma(4, 0.5);
546  const SharedDiagonal sig_2D = noiseModel::Isotropic::Sigma(2, 0.5);
547  GaussianFactorGraph factors = {
548  std::make_shared<JacobianFactor>(KeyVector{3, 5, 7, 9, 11}, VerticalBlockMatrix(Dims{2, 2, 2, 2, 2, 1}, Ab.block(0, 0, 4, 11)), sig_4D),
549  std::make_shared<JacobianFactor>(KeyVector{5, 7, 9, 11}, VerticalBlockMatrix(Dims{2, 2, 2, 2, 1}, Ab.block(4, 2, 4, 9)), sig_4D),
550  std::make_shared<JacobianFactor>(KeyVector{7, 9, 11}, VerticalBlockMatrix(Dims{2, 2, 2, 1}, Ab.block(8, 4, 4, 7)), sig_4D),
551  std::make_shared<JacobianFactor>(KeyVector{11}, VerticalBlockMatrix(Dims{2, 1}, Ab.block(12, 8, 2, 3)), sig_2D)};
552 
553  // extract the dense matrix for the graph
554  Matrix actualDense = factors.augmentedJacobian();
555  EXPECT(assert_equal(2.0 * Ab, actualDense));
556 
557  // Expected augmented matrix, both GaussianConditional (first 6 rows) and remaining factor (next 4 rows)
558  Matrix R = 2.0 * (Matrix(10, 11) <<
559  -12.1244, -5.1962, -5.2786, -8.6603, -10.5573, -5.9385, -11.3820, -7.2581, -8.7427, -13.4440, -5.3611,
560  0., 4.6904, 5.0254, 5.5432, 5.5737, 3.0153, -3.0153, -3.5635, -3.9290, -2.7412, 2.1625,
561  0., 0., -13.8160, -8.7166, -10.2245, -8.8666, -8.7632, -5.2544, -6.9192, -10.5537, -9.3250,
562  0., 0., 0., 6.5033, -1.1453, 1.3179, 2.5768, 5.5503, 3.6524, 1.3491, -2.5676,
563  0., 0., 0., 0., -9.6242, -2.1148, -9.3509, -10.5846, -3.5366, -6.8561, -3.2277,
564  0., 0., 0., 0., 0., 9.7887, 4.3551, 5.7572, 2.7876, 0.1611, 1.1769,
565  0., 0., 0., 0., 0., 0., -11.1139, -0.6521, -2.1943, -7.5529, -0.9081,
566  0., 0., 0., 0., 0., 0., 0., -4.6479, -1.9367, -6.5170, -3.7685,
567  0., 0., 0., 0., 0., 0., 0., 0., 8.2503, 3.3757, 6.8476,
568  0., 0., 0., 0., 0., 0., 0., 0., 0., -5.7095, -0.0090).finished();
569 
570  GaussianConditional expectedFragment(KeyVector{3,5,7,9,11}, 3,
571  VerticalBlockMatrix(Dims{2, 2, 2, 2, 2, 1}, R.topRows(6)),
572  noiseModel::Unit::Create(6));
573 
574  // Eliminate (3 frontal variables, 6 scalar columns) using QR !!!!
575  GaussianFactorGraph::EliminationResult actual = EliminateQR(factors, Ordering{3, 5, 7});
576  const JacobianFactor &actualJF = dynamic_cast<const JacobianFactor&>(*actual.second);
577 
578  EXPECT(assert_equal(expectedFragment, *actual.first, 0.001));
579  EXPECT_LONGS_EQUAL(size_t(2), actualJF.keys().size());
580  EXPECT(assert_equal(Key(9), actualJF.keys()[0]));
581  EXPECT(assert_equal(Key(11), actualJF.keys()[1]));
582  EXPECT(assert_equal(Matrix(R.block(6, 6, 4, 2)), actualJF.getA(actualJF.begin()), 0.001));
583  EXPECT(assert_equal(Matrix(R.block(6, 8, 4, 2)), actualJF.getA(actualJF.begin()+1), 0.001));
584  EXPECT(assert_equal(Vector(R.col(10).segment(6, 4)), actualJF.getb(), 0.001));
585  EXPECT(assert_equal(*noiseModel::Diagonal::Sigmas(Vector4::Ones()), *actualJF.get_model(), 0.001));
586 }
587 
588 /* ************************************************************************* */
589 TEST ( JacobianFactor, constraint_eliminate1 )
590 {
591  // construct a linear constraint
592  Vector v(2); v(0)=1.2; v(1)=3.4;
593  JacobianFactor lc(1, I_2x2, v, noiseModel::Constrained::All(2));
594 
595  // eliminate it
596  pair<GaussianConditional::shared_ptr, JacobianFactor::shared_ptr>
597  actual = lc.eliminate(Ordering{1});
598 
599  // verify linear factor is a null ptr
600  EXPECT(actual.second->empty());
601 
602  // verify conditional Gaussian
603  Vector sigmas = Vector2(0.0, 0.0);
604  GaussianConditional expCG(1, v, I_2x2, noiseModel::Diagonal::Sigmas(sigmas));
605  EXPECT(assert_equal(expCG, *actual.first));
606 }
607 
608 /* ************************************************************************* */
609 TEST ( JacobianFactor, constraint_eliminate2 )
610 {
611  // Construct a linear constraint
612  // RHS
613  Vector b(2); b(0)=3.0; b(1)=4.0;
614 
615  // A1 - invertible
616  Matrix2 A1; A1 << 2,4, 2,1;
617 
618  // A2 - not invertible
619  Matrix2 A2; A2 << 2,4, 2,4;
620 
621  JacobianFactor lc(1, A1, 2, A2, b, noiseModel::Constrained::All(2));
622 
623  // eliminate x and verify results
624  pair<GaussianConditional::shared_ptr, JacobianFactor::shared_ptr>
625  actual = lc.eliminate(Ordering{1});
626 
627  // LF should be empty
628  // It's tricky to create Eigen matrices that are only zero along one dimension
629  Matrix m(1,2);
630  Matrix Aempty = m.topRows(0);
631  Vector bempty = m.block(0,0,0,1);
632  JacobianFactor expectedLF(2, Aempty, bempty, noiseModel::Constrained::All(0));
633  EXPECT(assert_equal(expectedLF, *actual.second));
634 
635  // verify CG
636  Matrix2 R; R << 1,2, 0,1;
637  Matrix2 S; S << 1,2, 0,0;
638  Vector d = Vector2(3.0, 0.6666);
639  Vector sigmas = Vector2(0.0, 0.0);
640  GaussianConditional expectedCG(1, d, R, 2, S, noiseModel::Diagonal::Sigmas(sigmas));
641  EXPECT(assert_equal(expectedCG, *actual.first, 1e-4));
642 }
643 
644 /* ************************************************************************* */
645 TEST(JacobianFactor, OverdeterminedEliminate) {
646  Matrix Ab(9, 4);
647  Ab << 0, 1, 0, 0, //
648  0, 0, 1, 0, //
649  Matrix74::Ones();
650 
651  // Call Gaussian version
652  Vector9 sigmas = Vector9::Ones();
653  SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);
654 
655  JacobianFactor factor(0, Ab.leftCols(3), Ab.col(3), diagonal);
656  GaussianFactorGraph::EliminationResult actual = factor.eliminate(Ordering{0});
657 
658  Matrix expectedRd(3, 4);
659  expectedRd << -2.64575131, -2.64575131, -2.64575131, -2.64575131, //
660  0.0, -1, 0, 0, //
661  0.0, 0.0, -1, 0;
662  GaussianConditional expectedConditional(0, expectedRd.col(3), expectedRd.leftCols(3),
663  noiseModel::Unit::Create(3));
664  EXPECT(assert_equal(expectedConditional, *actual.first, 1e-4));
665  EXPECT(actual.second->empty());
666 }
667 
668 /* ************************************************************************* */
669 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
670 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
simple_graph::sigma1
double sigma1
Definition: testJacobianFactor.cpp:193
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
GaussianConditional.h
Conditional Gaussian Base class.
simple_graph::b1
Vector2 b1(2, -1)
gtsam::JacobianFactor::unweighted_error
Vector unweighted_error(const VectorValues &c) const
Definition: JacobianFactor.cpp:471
A3
static const double A3[]
Definition: expn.h:8
simple_graph::factor2
auto factor2
Definition: testJacobianFactor.cpp:203
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:100
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
test_constructor::sigmas
Vector1 sigmas
Definition: testHybridNonlinearFactor.cpp:52
TestHarness.h
gtsam::JacobianFactor::get_model
const SharedDiagonal & get_model() const
Definition: JacobianFactor.h:292
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Definition: VectorValues.cpp:90
b
Scalar * b
Definition: benchVecAdd.cpp:17
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
simple_graph::b2
Vector2 b2(4, -5)
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::GaussianFactorGraph::augmentedJacobian
Matrix augmentedJacobian(const Ordering &ordering) const
Definition: GaussianFactorGraph.cpp:217
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::JacobianFactor::hessianDiagonal
void hessianDiagonal(double *d) const override
Raw memory access version of hessianDiagonal.
Definition: JacobianFactor.cpp:545
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
simple_graph::A33
Matrix A33
Definition: testJacobianFactor.cpp:208
gtsam::JacobianFactor::jacobian
std::pair< Matrix, Vector > jacobian() const override
Returns (dense) A,b pair associated with factor, bakes in the weights.
Definition: JacobianFactor.cpp:714
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
A0
static const double A0[]
Definition: expn.h:5
diagonal
void diagonal(const MatrixType &m)
Definition: diagonal.cpp:12
gtsam::JacobianFactor::getb
const constBVector getb() const
Definition: JacobianFactor.h:298
TestableAssertions.h
Provides additional testing facilities for common data structures.
sampling::sigma
static const double sigma
Definition: testGaussianBayesNet.cpp:170
gtsam::JacobianFactor::eliminate
std::pair< std::shared_ptr< GaussianConditional >, shared_ptr > eliminate(const Ordering &keys)
Definition: JacobianFactor.cpp:760
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
gtsam::Dims
std::vector< Key > Dims
Definition: HessianFactor.cpp:41
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
simple_graph
Definition: testJacobianFactor.cpp:189
gtsam::VerticalBlockMatrix
Definition: VerticalBlockMatrix.h:42
gtsam::VectorValues
Definition: VectorValues.h:74
I
#define I
Definition: main.h:112
gtsam::JacobianFactor::information
Matrix information() const override
Definition: JacobianFactor.cpp:505
x1
Pose3 x1
Definition: testPose3.cpp:663
gtsam::JacobianFactor::jacobianUnweighted
std::pair< Matrix, Vector > jacobianUnweighted() const
Returns (dense) A,b pair associated with factor, does not bake in weights.
Definition: JacobianFactor.cpp:723
gtsam::JacobianFactor::gradientAtZero
VectorValues gradientAtZero() const override
A'*b for Jacobian.
Definition: JacobianFactor.cpp:690
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
gtsam::stack
Matrix stack(size_t nrMatrices,...)
Definition: Matrix.cpp:395
gtsam::GaussianConditional
Definition: GaussianConditional.h:40
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
A2
static const double A2[]
Definition: expn.h:7
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::JacobianFactor::augmentedJacobian
Matrix augmentedJacobian() const override
Definition: JacobianFactor.cpp:730
simple_graph::A32
Matrix A32
Definition: testJacobianFactor.cpp:207
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
Vector2
Definition: test_operator_overloading.cpp:18
VectorValues.h
Factor Graph Values.
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::Factor::end
const_iterator end() const
Definition: Factor.h:149
simple_graph::keyY
Key keyY(8)
simple_graph::b3
Vector2 b3(3, -6)
simple_graph::A21
Matrix A21
Definition: testJacobianFactor.cpp:200
ordering
static enum @1096 ordering
gtsam::JacobianFactor::error_vector
Vector error_vector(const VectorValues &c) const
Definition: JacobianFactor.cpp:479
TestResult
Definition: TestResult.h:26
JacobianFactor.h
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
simple_graph::factor3
auto factor3
Definition: testJacobianFactor.cpp:210
A10
static const double A10[]
Definition: expn.h:15
VariableSlots.h
VariableSlots describes the structure of a combined factor in terms of where each block comes from in...
empty
Definition: test_copy_move.cpp:19
gtsam
traits
Definition: SFMdata.h:40
error
static double error
Definition: testRot3.cpp:37
gtsam::JacobianFactor::getA
constABlock getA(const_iterator variable) const
Definition: JacobianFactor.h:301
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:143
gtsam::JacobianFactor::hessianBlockDiagonal
std::map< Key, Matrix > hessianBlockDiagonal() const override
Return the block diagonal of the Hessian for this factor.
Definition: JacobianFactor.cpp:550
std
Definition: BFloat16.h:88
simple_graph::A22
Matrix A22
Definition: testJacobianFactor.cpp:201
A1
static const double A1[]
Definition: expn.h:6
exampleQR::Ab
Matrix Ab
Definition: testNoiseModel.cpp:207
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
A11
static const double A11[]
Definition: expn.h:16
simple_graph::sigma3
double sigma3
Definition: testJacobianFactor.cpp:206
TEST
TEST(JacobianFactor, constructors_and_accessors)
Definition: testJacobianFactor.cpp:47
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
main
int main()
Definition: testJacobianFactor.cpp:669
gtsam::JacobianFactor::error
double error(const VectorValues &c) const override
Definition: JacobianFactor.cpp:486
gtsam::JacobianFactor::transposeMultiplyAdd
void transposeMultiplyAdd(double alpha, const Vector &e, VectorValues &x) const
Definition: JacobianFactor.cpp:618
simple_graph::keyX
Key keyX(10)
gtsam::JacobianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: JacobianFactor.h:97
simple_graph::sigma2
double sigma2
Definition: testJacobianFactor.cpp:199
simple
Definition: testJacobianFactor.cpp:34
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
simple_graph::keyZ
Key keyZ(12)
gtsam::Ordering
Definition: inference/Ordering.h:33
gtsam::JacobianFactor::augmentedJacobianUnweighted
Matrix augmentedJacobianUnweighted() const
Definition: JacobianFactor.cpp:738
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
gtsam::JacobianFactor::augmentedInformation
Matrix augmentedInformation() const override
Definition: JacobianFactor.cpp:494
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::GaussianFactorGraph::add
void add(const GaussianFactor &factor)
Definition: GaussianFactorGraph.h:125
simple_graph::factor1
auto factor1
Definition: testJacobianFactor.cpp:196
S
DiscreteKey S(1, 2)
gtsam::VectorValues::vector
Vector vector() const
Definition: VectorValues.cpp:175
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: JacobianFactor.cpp:778

gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:07:47