Fix initialization in range iSAM example

[dellaert]

I cleaned up this old example and initialized the landmarks randomly as iSAM proceeds.
Fixes #1130

[jerredchen]

@dellaert I took at look at the issue #1130 and when I built the branch, I'm actually running into the same issue:

terminate called after throwing an instance of 'gtsam::IndeterminantLinearSystemException'
  what():  
Indeterminant linear system detected while working near variable
3086 (Symbol: 3086).

I'm still going through the code and haven't found anything in particular. My understanding is that if you're not getting an ill-posed solution on your Mac, there might possibly be OS differences (?) - I'm not sure though.

[dellaert]

@dellaert I took at look at the issue #1130 and when I built the branch, I'm actually running into the same issue:

terminate called after throwing an instance of 'gtsam::IndeterminantLinearSystemException'
  what():  
Indeterminant linear system detected while working near variable
3086 (Symbol: 3086).

I'm still going through the code and haven't found anything in particular. My understanding is that if you're not getting an ill-posed solution on your Mac, there might possibly be OS differences (?) - I'm not sure though.

What OS are you on? Is it also on L3?

[jerredchen]

@dellaert I'm on Ubuntu 20.04 (not sure what you mean by L3).

[dellaert]

@dellaert I'm on Ubuntu 20.04 (not sure what you mean by L3).

The exception refers to a variable. What is it?

[dellaert]

Oh wait, just saw: it's pose 3086, not landmark L3.

[jerredchen]

Right, just realized that L3 referred to the landmark symbol. For my error it's actually not occurring with a landmark variable:

terminate called after throwing an instance of 'gtsam::IndeterminantLinearSystemException'
  what():  
Indeterminant linear system detected while working near variable
3086 (Symbol: 3086).

[dellaert]

Thanks!! Can I trouble you to try to set the standard deviation for initializing the landmark to something less than 1000? Try 100, 10. Check several random seeds for each setting, and see whether 10 or 100 reliably succeeds. It could be a numeric issue in computing the determinant - should not be the case but that would be another issue to investigate.

[jerredchen]

Okay sure, I'll let you know if I find anything interesting that comes up.

[varunagrawal]

The Windows CI is failing because the scoop changed its default install process. It annoys the heck out of me that the repo and the build commands are so intertwined.
Anyway, my last PR has the fix for it, so please merge in develop.

[dellaert]

Okay sure, I'll let you know if I find anything interesting that comes up.

Ping @jerredchen ?

[jerredchen]

@dellaert Even with changing the loose prior sigma and trying different seeds, it still states that the system is ill-posed. I've only had success with one set of parameters, where looseNoise = NM::Isotropic::Sigma(2, 1) and std::normal_distribution<double> normal(0.0, 10), which will yield:

-Total: 0 CPU (0 times, 0 wall, 49.39 children, min: 0 max: 0)
|   -iSAM: 49.49 CPU (1 times, 28.1184 wall, 49.39 children, min: 0 max: 0)
|   |   -batchInitialization: 1.04 CPU (1 times, 0.229335 wall, 1.04 children, min: 0 max: 0)
|   |   -update: 26.38 CPU (82 times, 5.82597 wall, 26.38 children, min: 0 max: 0)
|   |   -calculateEstimate: 21.97 CPU (82 times, 21.9526 wall, 21.97 children, min: 0 max: 0)
L0:-0.58908 0.627698
L1:-0.867836  0.919635
L2:-2.54579  2.81404
L3: 6.26313 -6.91674
L4: -0.2766 0.303318
L5:-0.888326  0.974642
L6:-0.784498  0.899885
L7:-0.264646  0.220543
L8:-0.174773  0.242514
L9:-0.0343196  0.0533935

But trying different random seeds with the same parameters will still result in a supposedly underdetermined system so I could only conclude that this worked by coincidence

[dellaert]

Update, after all this, it turned out to be a parsing error :-( The TD file has floats in it, and one of them was parsed as an int, so the data association was all messed up. I figured this out only after adding a python notebook (now also part of this PR) where everything worked nicely as I used np.loadtxt. Result in there looks like:

Which looks exactly like trajectory in https://www.ri.cmu.edu/pub_files/2009/9/Final_5datasetsRangingRadios.pdf.

Note, the range noise has to be made artificially high for iSAM to succeed. To get good uncertainties in line with the UWB standard deviation of 0.5 m (see the paper) a fine-tuning step would be needed. Or, SmartRangeFactor can be used as well.

[dellaert]

@jerredchen check on Linux then approve and merge if it works?

[jerredchen]

@dellaert Sounds great, I will build the changes and if everything works and the code looks fine then I will merge.

[jerredchen]

@dellaert Tested on Linux - the landmark locations of the C++ and the notebook versions match and look good. Other than the small suggestion I made, it looks good to merge.

[dellaert]

@jerredchen I don't see a suggestion. Did you use the review functionality but forgot to hit approve? If agree with your suggestion I'll edit before merging (but I can't merge without an approve).

[jerredchen]

The wording of this comment confuses me, should it be "iSAM2 requires as input a set of new factors to be added and stored in a factor graph" ?

[dellaert]

It's a bit confusing but I'm not gonna restart the CI for this :-) CO2 emissions and all that. I'll slip it into the other PR that I'm working on.

[jerredchen]

@dellaert You're right, I completely forgot to submit the review. It's a really small thing, so other than that I think it's good to be merged.