ISAM2: Add variables before computing delta

[varunagrawal]

Fixes #301

A gtsam::ValuesKeyDoesNotExist error is thrown from the function nonlinearFactors_.error(theta_).
The issue is that nonlinearFactors_.error(theta_) is computed as an argument to DoglegOptimizerImpl::Iterate, where a factor with the new key exists but theta_ has not been updated with the new key-value pairs yet.

It is a simple quirk that Dogleg requires the updated Values since it needs them to compute the delta update via the dogleg point.

The issue in ISAM2::update is that the delta update is computed first to check for relinearization before the new factors and Values are added. The delta update calls the above nonlinearFactors_.error(theta_) method with missing keys in theta_ leading to the exception.

Thus the fix is to add in the new variables newTheta with addVariable(newTheta) before the call to updateDelta(updateParams.forceFullSolve);. AFAIAA this step is to do with fluid relinearization and shouldn't affect the LM path since we call addVariable(newTheta) before the actual update step in both the LM and Dogleg cases.

Update: Piggybacking on this PR to also fix #1672.

[dellaert]

Touching the internals of iSAM2 needs a bit more explanation. So, two requests:

• Outline the changes you made in iSAM2, and say why they were needed for the Dogleg path, and why the don't disturb the LM path. Just a matter of providing your understanding in the MR for tracking and reviewing purposes.
• if the test was grabbed from an external contributor, you might want to add comments in the code to that effect and also a link to the issue.

[varunagrawal]

@dellaert I updated the PR comment to reflect the understanding behind the fix.

About the unit test, @kvmanohar22 was kind enough to create a script illustrating the problem, but I came up with the unit test for it based on my understanding of his script and debugging the issue. His script is instead based on the SFM SmartFactor example you wrote, so I am a bit tangled up about the credit assignment here.

[dellaert]

Awesome, thanks both!