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

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:33