Marginal computation of pose-graph without loop closure

[mbrossar]

Once removing loop closure in pose3example.txt, the Pose3Example_g2o.cpp example is not able to print marginal covariance, whereas optimization is complete.

We get the following IndeterminantLinearSystemException error and message after executing the modifying Pose3Example_g2o (named Pose3Localization):

Optimizing the factor graph
converged
errorThreshold: 1.73638e-18 <? 0
absoluteDecrease: 1.42155874173e-07 <? 1e-05
relativeDecrease: 0.999999999988 <? 1e-05
iterations: 1 >? 100
Optimization complete
initial error=1.42155874175e-07
final error=1.73637578578e-18
resultValues with 5 values:
Value 0: (N5gtsam5Pose3E) R:
[
 	                 1  2.10779579833e-24 -5.03721408293e-24;
    	-2.10779579833e-24                  1  1.99538207329e-23;
    	 5.03721408293e-24 -1.99538207329e-23                  1
  ]
[3.24794409286e-22, 3.26907867624e-22, -3.73340215321e-22]';
Value 1: (N5gtsam5Pose3E) R:
[
 	 0.531811530815  0.778004423987  0.334491986938;
    	-0.562514747295  0.619780859842 -0.547218656482;
    	-0.633050497311  0.102860172967  0.767246044909
  ]
[1.001367, 0.01539, 0.004948]';
Value 2: (N5gtsam5Pose3E) R:
[
 	 -0.397340720524 -0.0718422803676  -0.914853977517;
    	  0.912948841999  0.0700540820616  -0.402014941661;
    	 0.0929705615713  -0.994952404981  0.0377535921067
  ]
[1.99350107389, 0.0232768101016, 0.00379310574137]';
Value 3: (N5gtsam5Pose3E) R:
[
 	 -0.770839027484  -0.256388816973   0.583159758952;
    	-0.0099228106563  -0.910489804842   -0.41341680231;
    	  0.636955310649  -0.324463694634   0.699293793501
  ]
[2.00429005069, 1.02430627776, 0.0180471501601]';
Value 4: (N5gtsam5Pose3E) R:
[
 	 0.174490360444 -0.649693862796 -0.739898641125;
    	 0.715731574939  0.599737946407 -0.357829880799;
    	 0.676225130507 -0.467131454306  0.569655383684
  ]
[0.999908427014, 1.05507313297, 0.0202085885432]';
terminate called after throwing an instance of 'gtsam::IndeterminantLinearSystemException'
  what():  
Indeterminant linear system detected while working near variable
3 (Symbol: 3).

Thrown when a linear system is ill-posed.  The most common cause for this
error is having underconstrained variables.  Mathematically, the system is
underdetermined.  See the GTSAM Doxygen documentation at
http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for
more information.
Abandon (core dumped)

---

This change is 

[dellaert]

will look today...

[ProfFan]

#121

[ProfFan]

Ping @dellaert

[ProfFan]

I think this is intended behavior, as the system is indeed indeterministic (5 var and 4 constraints).
@dellaert Is the above statement correct? If so we can close this...

[dellaert]

No, incorrect: the example adds a prior...

[ProfFan]

@dellaert Oh I see...

[varunagrawal]

Shouldn't the marginals be computed on graphWithPrior instead of graph? We are optimizing on graphWithPrior and it is not underdetermined like graph. That would explain the ILS issue.

I tried it myself with graphWithPrior and I get valid results:

            1e-06  9.40758582146e-24   4.6753712013e-24 -1.27059069136e-22  2.09610107049e-22 -4.12114786741e-22
 9.40758582146e-24              1e-06 -9.80426850816e-24  3.51718654201e-22 -4.69181931897e-22  5.45696821064e-22
  4.6753712013e-24 -9.80426850816e-24              1e-06  4.26325641456e-22 -1.27329258248e-21  4.47599952677e-22
-1.27059069136e-22  3.51718654201e-22  4.26325641456e-22             0.0001  3.04728985101e-20 -1.67663452566e-20
 2.09610107049e-22 -4.69181931897e-22 -1.27329258248e-21  3.04728985101e-20             0.0001  3.00726674465e-20
-4.12114786741e-22  5.45696821064e-22  4.47599952677e-22 -1.67663452566e-20  3.00726674465e-20             0.0001
 0.000100999999277  1.66298444334e-13 -2.30404563152e-13  2.36748118015e-13 -3.30323757075e-07   7.8911467877e-07
 1.66298444334e-13  0.000100999999413 -3.42921551024e-13  3.30323952546e-07 -2.33508437504e-13 -5.20748644527e-07
-2.30404563152e-13 -3.42921551025e-13  0.000100999999641 -7.89115165394e-07  5.20748970388e-07 -3.23970166786e-15
 2.36748118015e-13  3.30323952546e-07 -7.89115165394e-07  0.000200731744505 -4.10914445819e-07 -1.72038748476e-07
-3.30323757075e-07 -2.33508437504e-13  5.20748970388e-07 -4.10914445819e-07  0.000200380234687 -2.60697712975e-07
  7.8911467877e-07 -5.20748644527e-07 -3.23970166778e-15 -1.72038748476e-07 -2.60697712975e-07  0.000200893845948
 0.000200999796571    5.592669407e-11 -5.07410943166e-11 -5.46480062854e-11  9.29199527297e-05 -7.10285646698e-06
 5.59266940701e-11  0.000200999881157 -1.00312464601e-10 -9.29200174752e-05  9.00825063486e-11  3.94826429434e-05
 -5.0741094315e-11   -1.003124646e-10  0.000200999857614  7.10297416308e-06 -3.94826840014e-05  -3.5434500317e-11
-5.46480062854e-11 -9.29200174752e-05  7.10297416308e-06  0.000386833113283 -2.80536614588e-06 -3.66742050224e-05
 9.29199527297e-05  9.00825063485e-11 -3.94826840014e-05 -2.80536614588e-06  0.000401908057015 -6.60404838487e-06
-7.10285646698e-06  3.94826429434e-05  -3.5434500317e-11 -3.66742050224e-05 -6.60404838487e-06  0.000316084850085
 0.000301000421047  2.27586491742e-10  -3.9189583298e-11 -6.07242273947e-11  2.13083361915e-05 -0.000211333918451
 2.27586491744e-10  0.000301000314249  2.62857672488e-10 -2.13081518909e-05  5.56706818236e-12  7.99362561794e-05
-3.91895832981e-11  2.62857672485e-10   0.00030099972198  0.000211333243615 -7.99365574431e-05  5.51569104055e-11
-6.07242273952e-11 -2.13081518909e-05  0.000211333243615  0.000645282648168 -9.45052276536e-05  1.45124940846e-05
 2.13083361915e-05   5.5670681841e-12 -7.99365574431e-05 -9.45052276536e-05  0.000481928530304 -1.45527909217e-05
-0.000211333918451  7.99362561794e-05  5.51569104057e-11  1.45124940846e-05 -1.45527909217e-05  0.000688186471728
 0.000401000518522  8.06111763185e-10 -4.02809349049e-10   8.3865087836e-10 -7.35360781492e-05  0.000254375570643
 8.06111763183e-10  0.000401000207303  1.48666442914e-10  7.35356370594e-05 -5.12463192383e-10 -0.000118807293985
 -4.0280934905e-10  1.48666442922e-10  0.000401001385947 -0.000254376288349  0.000118808578221 -3.26187765627e-10
 8.38650878361e-10  7.35356370594e-05 -0.000254376288349  0.000823139334119 -0.000108778991735   1.7869214804e-05
-7.35360781492e-05 -5.12463192381e-10  0.000118808578221 -0.000108778991735  0.000675491978078  -7.3962708752e-05
 0.000254375570643 -0.000118807293985 -3.26187765626e-10   1.7869214804e-05  -7.3962708752e-05  0.000834204335608

[dellaert]

Yes, I agree. Actually, just remove graphWithPrior (in both cpp and two python examples)

[varunagrawal]

@mbrossar I am sorry for hijacking your PR, but we fixed the issue and I am approving this PR. Can you verify on your end as well?

[varunagrawal]

Thanks for the additional example!

[mbrossar]

@varunagrawal Yes, it now works well. Thank you!