testHessianFactor.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 
18 #include <gtsam/linear/HessianFactor.h>
19 #include <gtsam/linear/JacobianFactor.h>
20 #include <gtsam/linear/GaussianFactorGraph.h>
21 #include <gtsam/linear/GaussianConditional.h>
22 #include <gtsam/linear/VectorValues.h>
23 #include <gtsam/base/debug.h>
24 #include <gtsam/base/TestableAssertions.h>
25 
26 #include <CppUnitLite/TestHarness.h>
27 
28 #include <boost/assign/list_of.hpp>
29 #include <boost/assign/std/vector.hpp>
30 #include <boost/assign/std/map.hpp>
31 using namespace boost::assign;
32 
33 #include <vector>
34 #include <utility>
35 
36 using namespace std;
37 using namespace gtsam;
38 
39 const double tol = 1e-5;
40 
41 /* ************************************************************************* */
42 TEST(HessianFactor, Slot)
43 {
44  KeyVector keys {2, 4, 1};
45  EXPECT_LONGS_EQUAL(0, GaussianFactor::Slot(keys,2));
46  EXPECT_LONGS_EQUAL(1, GaussianFactor::Slot(keys,4));
47  EXPECT_LONGS_EQUAL(2, GaussianFactor::Slot(keys,1));
48  EXPECT_LONGS_EQUAL(3, GaussianFactor::Slot(keys,5)); // does not exist
49 }
50 
51 /* ************************************************************************* */
52 TEST(HessianFactor, emptyConstructor)
53 {
54  HessianFactor f;
55  DOUBLES_EQUAL(0.0, f.constantTerm(), 1e-9); // Constant term should be zero
56  //EXPECT(assert_equal(Vector(), f.linearTerm())); // Linear term should be empty
57  EXPECT(assert_equal((Matrix) Z_1x1, f.info().selfadjointView())); // Full matrix should be 1-by-1 zero matrix
58  DOUBLES_EQUAL(0.0, f.error(VectorValues()), 1e-9); // Should have zero error
59 }
60 
61 /* ************************************************************************* */
62 TEST(HessianFactor, ConversionConstructor)
63 {
64  HessianFactor expected(list_of(0)(1),
65  SymmetricBlockMatrix(list_of(2)(4)(1), (Matrix(7,7) <<
66  125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000,
67  0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000,
68  -25.0000, 0.0, 25.0000, 0.0, 0.0, 0.0, -5.0000,
69  0.0, -25.0000, 0.0, 25.0000, 0.0, 0.0, 7.5000,
70  -100.0000, 0.0, 0.0, 0.0, 100.0000, 0.0, -20.0000,
71  0.0, -100.0000, 0.0, 0.0, 0.0, 100.0000, 10.0000,
72  25.0000, -17.5000, -5.0000, 7.5000, -20.0000, 10.0000, 8.2500).finished()));
73 
74  JacobianFactor jacobian(
75  0, (Matrix(4,2) << -1., 0.,
76  +0.,-1.,
77  1., 0.,
78  +0.,1.).finished(),
79  1, (Matrix(4,4) << 1., 0., 0.00, 0., // f4
80  0., 1., 0.00, 0., // f4
81  0., 0., -1., 0., // f2
82  0., 0., 0.00, -1.).finished(), // f2
83  (Vector(4) << -0.2, 0.3, 0.2, -0.1).finished(),
84  noiseModel::Diagonal::Sigmas((Vector(4) << 0.2, 0.2, 0.1, 0.1).finished()));
85 
86  HessianFactor actual(jacobian);
87 
88  VectorValues values = pair_list_of<Key, Vector>
89  (0, Vector2(1.0, 2.0))
90  (1, (Vector(4) << 3.0, 4.0, 5.0, 6.0).finished());
91 
92  EXPECT_LONGS_EQUAL(2, (long)actual.size());
93  EXPECT(assert_equal(expected, actual, 1e-9));
94  EXPECT_DOUBLES_EQUAL(jacobian.error(values), actual.error(values), 1e-9);
95 }
96 
97 /* ************************************************************************* */
98 TEST(HessianFactor, Constructor1)
99 {
100  Matrix G = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished();
101  Vector g = Vector2(-8.0, -9.0);
102  double f = 10.0;
103  HessianFactor factor(0, G, g, f);
104 
105  // extract underlying parts
106  EXPECT(assert_equal(G, Matrix(factor.info().diagonalBlock(0))));
107  EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10);
108  EXPECT(assert_equal(g, Vector(factor.linearTerm())));
109  EXPECT_LONGS_EQUAL(1, (long)factor.size());
110 
111  VectorValues dx = pair_list_of<Key, Vector>(0, Vector2(1.5, 2.5));
112 
113  // error 0.5*(f - 2*x'*g + x'*G*x)
114  double expected = 80.375;
115  double actual = factor.error(dx);
116  double expected_manual = 0.5 * (f - 2.0 * dx[0].dot(g) + dx[0].transpose() * G.selfadjointView<Eigen::Upper>() * dx[0]);
117  EXPECT_DOUBLES_EQUAL(expected, expected_manual, 1e-10);
118  EXPECT_DOUBLES_EQUAL(expected, actual, 1e-10);
119 }
120 
121 /* ************************************************************************* */
122 TEST(HessianFactor, Constructor1b)
123 {
124  Vector mu = Vector2(1.0,2.0);
125  Matrix Sigma = I_2x2;
126 
127  HessianFactor factor(0, mu, Sigma);
128 
129  Matrix G = I_2x2;
130  Vector g = G*mu;
131  double f = dot(g,mu);
132 
133  // Check
134  EXPECT(assert_equal(G, Matrix(factor.info().diagonalBlock(0))));
135  EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10);
136  EXPECT(assert_equal(g, Vector(factor.linearTerm())));
137  EXPECT_LONGS_EQUAL(1, (long)factor.size());
138 }
139 
140 /* ************************************************************************* */
141 TEST(HessianFactor, Constructor2)
142 {
143  Matrix G11 = (Matrix(1,1) << 1.0).finished();
144  Matrix G12 = (Matrix(1,2) << 2.0, 4.0).finished();
145  Matrix G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished();
146  Vector g1 = (Vector(1) << -7.0).finished();
147  Vector g2 = Vector2(-8.0, -9.0);
148  double f = 10.0;
149 
150  Vector dx0 = (Vector(1) << 0.5).finished();
151  Vector dx1 = Vector2(1.5, 2.5);
152 
153  VectorValues dx = pair_list_of
154  (0, dx0)
155  (1, dx1);
156 
157  HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f);
158 
159  double expected = 90.5;
160  double actual = factor.error(dx);
161 
162  DOUBLES_EQUAL(expected, actual, 1e-10);
163  LONGS_EQUAL(4, (long)factor.rows());
164  DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10);
165 
166  Vector linearExpected(3); linearExpected << g1, g2;
167  EXPECT(assert_equal(linearExpected, factor.linearTerm()));
168 
169  EXPECT(assert_equal(G11, factor.info().diagonalBlock(0)));
170  EXPECT(assert_equal(G12, factor.info().aboveDiagonalBlock(0, 1)));
171  EXPECT(assert_equal(G22, factor.info().diagonalBlock(1)));
172 
173  // Check case when vector values is larger than factor
174  VectorValues dxLarge = pair_list_of<Key, Vector>
175  (0, dx0)
176  (1, dx1)
177  (2, Vector2(0.1, 0.2));
178  EXPECT_DOUBLES_EQUAL(expected, factor.error(dxLarge), 1e-10);
179 }
180 
181 /* ************************************************************************* */
182 TEST(HessianFactor, Constructor3)
183 {
184  Matrix G11 = (Matrix(1,1) << 1.0).finished();
185  Matrix G12 = (Matrix(1,2) << 2.0, 4.0).finished();
186  Matrix G13 = (Matrix(1,3) << 3.0, 6.0, 9.0).finished();
187 
188  Matrix G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished();
189  Matrix G23 = (Matrix(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0).finished();
190 
191  Matrix G33 = (Matrix(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0).finished();
192 
193  Vector g1 = (Vector(1) << -7.0).finished();
194  Vector g2 = Vector2(-8.0, -9.0);
195  Vector g3 = Vector3(1.0, 2.0, 3.0);
196 
197  double f = 10.0;
198 
199  Vector dx0 = (Vector(1) << 0.5).finished();
200  Vector dx1 = Vector2(1.5, 2.5);
201  Vector dx2 = Vector3(1.5, 2.5, 3.5);
202 
203  VectorValues dx = pair_list_of
204  (0, dx0)
205  (1, dx1)
206  (2, dx2);
207 
208  HessianFactor factor(0, 1, 2, G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
209 
210  double expected = 371.3750;
211  double actual = factor.error(dx);
212 
213  DOUBLES_EQUAL(expected, actual, 1e-10);
214  LONGS_EQUAL(7, (long)factor.rows());
215  DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10);
216 
217  Vector linearExpected(6); linearExpected << g1, g2, g3;
218  EXPECT(assert_equal(linearExpected, factor.linearTerm()));
219 
220  EXPECT(assert_equal(G11, factor.info().diagonalBlock(0)));
221  EXPECT(assert_equal(G12, factor.info().aboveDiagonalBlock(0, 1)));
222  EXPECT(assert_equal(G13, factor.info().aboveDiagonalBlock(0, 2)));
223  EXPECT(assert_equal(G22, factor.info().diagonalBlock(1)));
224  EXPECT(assert_equal(G23, factor.info().aboveDiagonalBlock(1, 2)));
225  EXPECT(assert_equal(G33, factor.info().diagonalBlock(2)));
226 }
227 
228 /* ************************************************************************* */
229 TEST(HessianFactor, ConstructorNWay)
230 {
231  Matrix G11 = (Matrix(1,1) << 1.0).finished();
232  Matrix G12 = (Matrix(1,2) << 2.0, 4.0).finished();
233  Matrix G13 = (Matrix(1,3) << 3.0, 6.0, 9.0).finished();
234 
235  Matrix G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished();
236  Matrix G23 = (Matrix(2,3) << 4.0, 6.0, 8.0, 1.0, 2.0, 4.0).finished();
237 
238  Matrix G33 = (Matrix(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0).finished();
239 
240  Vector g1 = (Vector(1) << -7.0).finished();
241  Vector g2 = Vector2(-8.0, -9.0);
242  Vector g3 = Vector3(1.0, 2.0, 3.0);
243 
244  double f = 10.0;
245 
246  Vector dx0 = (Vector(1) << 0.5).finished();
247  Vector dx1 = Vector2(1.5, 2.5);
248  Vector dx2 = Vector3(1.5, 2.5, 3.5);
249 
250  VectorValues dx = pair_list_of
251  (0, dx0)
252  (1, dx1)
253  (2, dx2);
254 
255  KeyVector js {0, 1, 2};
256  std::vector<Matrix> Gs;
257  Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33);
258  std::vector<Vector> gs;
259  gs.push_back(g1); gs.push_back(g2); gs.push_back(g3);
260  HessianFactor factor(js, Gs, gs, f);
261 
262  double expected = 371.3750;
263  double actual = factor.error(dx);
264 
265  DOUBLES_EQUAL(expected, actual, 1e-10);
266  LONGS_EQUAL(7, (long)factor.rows());
267  DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10);
268 
269  Vector linearExpected(6); linearExpected << g1, g2, g3;
270  EXPECT(assert_equal(linearExpected, factor.linearTerm()));
271 
272  EXPECT(assert_equal(G11, factor.info().diagonalBlock(0)));
273  EXPECT(assert_equal(G12, factor.info().aboveDiagonalBlock(0, 1)));
274  EXPECT(assert_equal(G13, factor.info().aboveDiagonalBlock(0, 2)));
275  EXPECT(assert_equal(G22, factor.info().diagonalBlock(1)));
276  EXPECT(assert_equal(G23, factor.info().aboveDiagonalBlock(1, 2)));
277  EXPECT(assert_equal(G33, factor.info().diagonalBlock(2)));
278 }
279 
280 /* ************************************************************************* */
281 TEST(HessianFactor, CombineAndEliminate1) {
282  Matrix3 A01 = 3.0 * I_3x3;
283  Vector3 b0(1, 0, 0);
284 
285  Matrix3 A21 = 4.0 * I_3x3;
286  Vector3 b2 = Vector3::Zero();
287 
288  GaussianFactorGraph gfg;
289  gfg.add(1, A01, b0);
290  gfg.add(1, A21, b2);
291 
292  Matrix63 A1;
293  A1 << A01, A21;
294  Vector6 b;
295  b << b0, b2;
296 
297  // create a full, uneliminated version of the factor
298  JacobianFactor jacobian(1, A1, b);
299 
300  // Make sure combining works
301  HessianFactor hessian(gfg);
302  VectorValues v;
303  v.insert(1, Vector3(1, 0, 0));
304  EXPECT_DOUBLES_EQUAL(jacobian.error(v), hessian.error(v), 1e-9);
305  EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6));
306  EXPECT(assert_equal(25.0 * I_3x3, hessian.information(), 1e-9));
307  EXPECT(
308  assert_equal(jacobian.augmentedInformation(),
309  hessian.augmentedInformation(), 1e-9));
310 
311  // perform elimination on jacobian
312  Ordering ordering = list_of(1);
313  GaussianConditional::shared_ptr expectedConditional;
314  JacobianFactor::shared_ptr expectedFactor;
315  boost::tie(expectedConditional, expectedFactor) = jacobian.eliminate(ordering);
316  CHECK(expectedFactor);
317 
318  // Eliminate
319  GaussianConditional::shared_ptr actualConditional;
320  HessianFactor::shared_ptr actualHessian;
321  boost::tie(actualConditional, actualHessian) = //
322  EliminateCholesky(gfg, ordering);
323  actualConditional->setModel(false,Vector3(1,1,1)); // add a unit model for comparison
324 
325  EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
326 #ifdef TEST_ERROR_EQUIVALENCE
327  // these tests disabled because QR cuts off the all zeros + error row
328  EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9);
329  EXPECT(
330  assert_equal(expectedFactor->augmentedInformation(),
331  actualHessian->augmentedInformation(), 1e-9));
332  EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6));
333 #endif
334  }
335 
336 /* ************************************************************************* */
337 TEST(HessianFactor, CombineAndEliminate2) {
338  Matrix A01 = I_3x3;
339  Vector3 b0(1.5, 1.5, 1.5);
340  Vector3 s0(1.6, 1.6, 1.6);
341 
342  Matrix A10 = 2.0 * I_3x3;
343  Matrix A11 = -2.0 * I_3x3;
344  Vector3 b1(2.5, 2.5, 2.5);
345  Vector3 s1(2.6, 2.6, 2.6);
346 
347  Matrix A21 = 3.0 * I_3x3;
348  Vector3 b2(3.5, 3.5, 3.5);
349  Vector3 s2(3.6, 3.6, 3.6);
350 
351  GaussianFactorGraph gfg;
352  gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
353  gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
354  gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
355 
356  Matrix93 A0, A1;
357  A0 << A10, Z_3x3, Z_3x3;
358  A1 << A11, A01, A21;
359  Vector9 b, sigmas;
360  b << b1, b0, b2;
361  sigmas << s1, s0, s2;
362 
363  // create a full, uneliminated version of the factor
364  JacobianFactor jacobian(0, A0, 1, A1, b,
365  noiseModel::Diagonal::Sigmas(sigmas, true));
366 
367  // Make sure combining works
368  HessianFactor hessian(gfg);
369  EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6));
370  EXPECT(
371  assert_equal(jacobian.augmentedInformation(),
372  hessian.augmentedInformation(), 1e-9));
373 
374  // perform elimination on jacobian
375  Ordering ordering = list_of(0);
376  GaussianConditional::shared_ptr expectedConditional;
377  JacobianFactor::shared_ptr expectedFactor;
378  boost::tie(expectedConditional, expectedFactor) = //
379  jacobian.eliminate(ordering);
380 
381  // Eliminate
382  GaussianConditional::shared_ptr actualConditional;
383  HessianFactor::shared_ptr actualHessian;
384  boost::tie(actualConditional, actualHessian) = //
385  EliminateCholesky(gfg, ordering);
386  actualConditional->setModel(false,Vector3(1,1,1)); // add a unit model for comparison
387 
388  EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
389 #ifdef TEST_ERROR_EQUIVALENCE
390  // these tests disabled because QR cuts off the all zeros + error row
391  VectorValues v;
392  v.insert(1, Vector3(1, 2, 3));
393  EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9);
394  EXPECT(
395  assert_equal(expectedFactor->augmentedInformation(),
396  actualHessian->augmentedInformation(), 1e-9));
397  EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6));
398 #endif
399 }
400 
401 /* ************************************************************************* */
402 TEST(HessianFactor, eliminate2 )
403 {
404  // sigmas
405  double sigma1 = 0.2;
406  double sigma2 = 0.1;
407  Vector sigmas = (Vector(4) << sigma1, sigma1, sigma2, sigma2).finished();
408 
409  // the combined linear factor
410  Matrix Ax2 = (Matrix(4,2) <<
411  // x2
412  -1., 0.,
413  +0.,-1.,
414  1., 0.,
415  +0.,1.
416  ).finished();
417 
418  Matrix Al1x1 = (Matrix(4,4) <<
419  // l1 x1
420  1., 0., 0.00, 0., // f4
421  0., 1., 0.00, 0., // f4
422  0., 0., -1., 0., // f2
423  0., 0., 0.00,-1. // f2
424  ).finished();
425 
426  // the RHS
427  Vector b2(4);
428  b2(0) = -0.2;
429  b2(1) = 0.3;
430  b2(2) = 0.2;
431  b2(3) = -0.1;
432 
433  vector<pair<Key, Matrix> > meas;
434  meas.push_back(make_pair(0, Ax2));
435  meas.push_back(make_pair(1, Al1x1));
436  JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));
437 
438  // eliminate the combined factor
439  HessianFactor::shared_ptr combinedLF_Chol(new HessianFactor(combined));
440  GaussianFactorGraph combinedLFG_Chol = list_of(combinedLF_Chol);
441 
442  std::pair<GaussianConditional::shared_ptr, HessianFactor::shared_ptr> actual_Chol =
443  EliminateCholesky(combinedLFG_Chol, Ordering(list_of(0)));
444 
445  // create expected Conditional Gaussian
446  double oldSigma = 0.0894427; // from when R was made unit
447  Matrix R11 = (Matrix(2,2) <<
448  1.00, 0.00,
449  0.00, 1.00
450  ).finished()/oldSigma;
451  Matrix S12 = (Matrix(2,4) <<
452  -0.20, 0.00,-0.80, 0.00,
453  +0.00,-0.20,+0.00,-0.80
454  ).finished()/oldSigma;
455  Vector d = Vector2(0.2,-0.14)/oldSigma;
456  GaussianConditional expectedCG(0, d, R11, 1, S12);
457  EXPECT(assert_equal(expectedCG, *actual_Chol.first, 1e-4));
458 
459  // the expected linear factor
460  double sigma = 0.2236;
461  Matrix Bl1x1 = (Matrix(2,4) <<
462  // l1 x1
463  1.00, 0.00, -1.00, 0.00,
464  0.00, 1.00, +0.00, -1.00
465  ).finished()/sigma;
466  Vector b1 = Vector2(0.0,0.894427);
467  JacobianFactor expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0));
468  EXPECT(assert_equal(HessianFactor(expectedLF), *actual_Chol.second, 1.5e-3));
469 }
470 
471 /* ************************************************************************* */
472 TEST(HessianFactor, combine) {
473 
474  // update the information matrix with a single jacobian factor
475  Matrix A0 = (Matrix(2, 2) <<
476  11.1803399, 0.0,
477  0.0, 11.1803399).finished();
478  Matrix A1 = (Matrix(2, 2) <<
479  -2.23606798, 0.0,
480  0.0, -2.23606798).finished();
481  Matrix A2 = (Matrix(2, 2) <<
482  -8.94427191, 0.0,
483  0.0, -8.94427191).finished();
484  Vector b = Vector2(2.23606798,-1.56524758);
485  SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector::Ones(2));
486  GaussianFactor::shared_ptr f(new JacobianFactor(0, A0, 1, A1, 2, A2, b, model));
487  GaussianFactorGraph factors = list_of(f);
488 
489  // Form Ab' * Ab
490  HessianFactor actual(factors);
491 
492  Matrix expected = (Matrix(7, 7) <<
493  125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000,
494  0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000,
495  -25.0000, 0.0, 5.0000, 0.0, 20.0000, 0.0, -5.0000,
496  0.0, -25.0000, 0.0, 5.0000, 0.0, 20.0000, 3.5000,
497  -100.0000, 0.0, 20.0000, 0.0, 80.0000, 0.0, -20.0000,
498  0.0, -100.0000, 0.0, 20.0000, 0.0, 80.0000, 14.0000,
499  25.0000, -17.5000, -5.0000, 3.5000, -20.0000, 14.0000, 7.4500).finished();
500  EXPECT(assert_equal(expected, Matrix(actual.info().selfadjointView()), tol));
501 
502 }
503 
504 /* ************************************************************************* */
505 TEST(HessianFactor, gradientAtZero)
506 {
507  Matrix G11 = (Matrix(1, 1) << 1).finished();
508  Matrix G12 = (Matrix(1, 2) << 0, 0).finished();
509  Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1).finished();
510  Vector g1 = (Vector(1) << -7).finished();
511  Vector g2 = Vector2(-8, -9);
512  double f = 194;
513 
514  HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f);
515 
516  // test gradient at zero
517  VectorValues expectedG = pair_list_of<Key, Vector>(0, -g1) (1, -g2);
518  Matrix A; Vector b; boost::tie(A,b) = factor.jacobian();
519  KeyVector keys {0, 1};
520  EXPECT(assert_equal(-A.transpose()*b, expectedG.vector(keys)));
521  VectorValues actualG = factor.gradientAtZero();
522  EXPECT(assert_equal(expectedG, actualG));
523 }
524 
525 /* ************************************************************************* */
526 TEST(HessianFactor, gradient)
527 {
528  Matrix G11 = (Matrix(1, 1) << 1).finished();
529  Matrix G12 = (Matrix(1, 2) << 0, 0).finished();
530  Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1).finished();
531  Vector g1 = (Vector(1) << -7).finished();
532  Vector g2 = (Vector(2) << -8, -9).finished();
533  double f = 194;
534 
535  HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f);
536 
537  // test gradient
538  Vector x0 = (Vector(1) << 3.0).finished();
539  Vector x1 = (Vector(2) << -3.5, 7.1).finished();
540  VectorValues x = pair_list_of<Key, Vector>(0, x0) (1, x1);
541 
542  Vector expectedGrad0 = (Vector(1) << 10.0).finished();
543  Vector expectedGrad1 = (Vector(2) << 4.5, 16.1).finished();
544  Vector grad0 = factor.gradient(0, x);
545  Vector grad1 = factor.gradient(1, x);
546 
547  EXPECT(assert_equal(expectedGrad0, grad0));
548  EXPECT(assert_equal(expectedGrad1, grad1));
549 }
550 
551 /* ************************************************************************* */
552 TEST(HessianFactor, hessianDiagonal)
553 {
554  Matrix G11 = (Matrix(1, 1) << 1).finished();
555  Matrix G12 = (Matrix(1, 2) << 0, 0).finished();
556  Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1).finished();
557  Vector g1 = (Vector(1) << -7).finished();
558  Vector g2 = Vector2(-8, -9);
559  double f = 194;
560 
561  HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f);
562 
563  // hessianDiagonal
564  VectorValues expected;
565  expected.insert(0, (Vector(1) << 1).finished());
566  expected.insert(1, Vector2(1,1));
567  EXPECT(assert_equal(expected, factor.hessianDiagonal()));
568 
569  // hessianBlockDiagonal
570  map<Key,Matrix> actualBD = factor.hessianBlockDiagonal();
571  LONGS_EQUAL(2,actualBD.size());
572  EXPECT(assert_equal(G11,actualBD[0]));
573  EXPECT(assert_equal(G22,actualBD[1]));
574 }
575 
576 /* ************************************************************************* */
577 TEST(HessianFactor, Solve)
578 {
579  Matrix2 A;
580  A << 1, 2, 3, 4;
581  Matrix2 G = A.transpose() * A;
582  Vector2 b(5, 6);
583  Vector2 g = A.transpose() * b;
584  double f = 0;
585  Key key(55);
586  HessianFactor factor(key, G, g, f);
587 
588  VectorValues expected;
589  expected.insert(key, A.inverse() * b);
590  EXPECT(assert_equal(expected, factor.solve()));
591 }
592 
593 /* ************************************************************************* */
594 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
595 /* ************************************************************************* */
test_docs.d
d
Definition: test_docs.py:28
TestableAssertions.h
Provides additional testing facilities for common data structures.
CHECK
#define CHECK(condition)
Definition: Test.h:109
gtsam::HessianFactor::gradientAtZero
VectorValues gradientAtZero() const override
eta for Hessian
Definition: HessianFactor.cpp:437
gtsam::HessianFactor::solve
VectorValues solve()
Solve the system A&#39;*A delta = A&#39;*b in-place, return delta as VectorValues.
Definition: HessianFactor.cpp:505
b
Scalar * b
Definition: benchVecAdd.cpp:17
simple_graph::sigma1
double sigma1
Definition: testJacobianFactor.cpp:191
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
debug.h
Global debugging flags.
gtsam::JacobianFactor::eliminate
std::pair< boost::shared_ptr< GaussianConditional >, shared_ptr > eliminate(const Ordering &keys)
Definition: JacobianFactor.cpp:779
G
JacobiRotation< float > G
Definition: Jacobi_makeGivens.cpp:2
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
ordering
static enum @843 ordering
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:974
gtsam::HessianFactor::linearTerm
SymmetricBlockMatrix::constBlock linearTerm(const_iterator j) const
Definition: HessianFactor.h:244
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:142
mu
double mu
Definition: testBoundingConstraint.cpp:37
v
ArrayXcf v
Definition: Cwise_arg.cpp:1
sigma
static const double sigma
Definition: testSmartProjectionPoseFactor.cpp:36
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
leaf::values
leaf::MyValues values
std
Definition: Half.h:150
gtsam::HessianFactor::rows
size_t rows() const
Definition: HessianFactor.h:215
simple_graph::factors
GaussianFactorGraph factors(list_of(factor1)(factor2)(factor3))
simple_graph::A11
Matrix A11
Definition: testJacobianFactor.cpp:192
tol
const double tol
Definition: testHessianFactor.cpp:39
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:162
g
void g(const string &key, int i)
Definition: testBTree.cpp:43
gtsam::HessianFactor::augmentedInformation
Matrix augmentedInformation() const override
Definition: HessianFactor.cpp:289
gtsam::HessianFactor::info
const SymmetricBlockMatrix & info() const
Return underlying information matrix.
Definition: HessianFactor.h:265
gtsam::HessianFactor::error
double error(const VectorValues &c) const override
Definition: HessianFactor.cpp:344
gtsam::HessianFactor::constantTerm
double constantTerm() const
Definition: HessianFactor.h:230
VectorValues.h
Factor Graph Values.
gtsam::GaussianFactorGraph::add
void add(const GaussianFactor &factor)
Definition: GaussianFactorGraph.h:102
gtsam::EliminateCholesky
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< HessianFactor > > EliminateCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: HessianFactor.cpp:532
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
dot
Scalar EIGEN_BLAS_FUNC() dot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy)
Definition: level1_real_impl.h:27
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
TEST
TEST(HessianFactor, Slot)
Definition: testHessianFactor.cpp:42
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
simple_graph::b2
Vector2 b2(4,-5)
gtsam::HessianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
A shared_ptr to this class.
Definition: HessianFactor.h:110
gtsam::HessianFactor::information
Matrix information() const override
Definition: HessianFactor.cpp:300
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:211
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::HessianFactor::hessianDiagonal
void hessianDiagonal(double *d) const override
Raw memory access version of hessianDiagonal.
Definition: HessianFactor.cpp:317
simple_graph::A21
Matrix A21
Definition: testJacobianFactor.cpp:197
GaussianConditional.h
Conditional Gaussian Base class.
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::SymmetricBlockMatrix::aboveDiagonalBlock
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
Definition: SymmetricBlockMatrix.h:155
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
Eigen::Upper
Definition: Constants.h:206
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:135
x0
static Symbol x0('x', 0)
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:736
gtsam
traits
Definition: chartTesting.h:28
simple_graph::b1
Vector2 b1(2,-1)
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
gtsam::Factor::size
size_t size() const
Definition: Factor.h:135
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:155
gtsam::HessianFactor::jacobian
std::pair< Matrix, Vector > jacobian() const override
Return (dense) matrix associated with factor.
Definition: HessianFactor.cpp:339
gtsam::JacobianFactor::augmentedInformation
Matrix augmentedInformation() const override
Definition: JacobianFactor.cpp:513
x1
Pose3 x1
Definition: testPose3.cpp:588
g1
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition: SymmetricBlockMatrix.h:161
gtsam::JacobianFactor::error
double error(const VectorValues &c) const override
Definition: JacobianFactor.cpp:505
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:101
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
gtsam::SymmetricBlockMatrix::diagonalBlock
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J&#39;th diagonal block as a self adjoint view.
Definition: SymmetricBlockMatrix.h:140
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:45
gtsam::JacobianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: JacobianFactor.h:96
gtsam::HessianFactor::gradient
Vector gradient(Key key, const VectorValues &x) const override
Definition: HessianFactor.cpp:453
gtsam::GaussianConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianConditional.h:42
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
g2
Pose3 g2(g1.expmap(h *V1_g1))
simple_graph::sigma2
double sigma2
Definition: testJacobianFactor.cpp:196
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Definition: VectorValues.cpp:90
gtsam::HessianFactor::hessianBlockDiagonal
std::map< Key, Matrix > hessianBlockDiagonal() const override
Return the block diagonal of the Hessian for this factor.
Definition: HessianFactor.cpp:323
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
main
int main()
Definition: testHessianFactor.cpp:594
gtsam::VectorValues::vector
Vector vector() const
Definition: VectorValues.cpp:173

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:47:15