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

gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:40:13