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

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