Merge branch 'fix/iSAM2' into fix/GenericValue-assignment

Added extra update so converges faster, and matches test in Issue #412

Conflicts:
	tests/testVisualISAM2.cpp

tests/testVisualISAM2.cpp

@@ -39,9 +39,8 @@ TEST(testVisualISAM2, all)
 
     auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
 
+    // Create ground truth data
     vector<Point3> points = createPoints();
-
-    // Create the set of ground-truth poses
     vector<Pose3> poses = createPoses();
 
     // Set the parameters
@@ -98,6 +97,9 @@ TEST(testVisualISAM2, all)
             // Update iSAM with the new factors
             isam.update(graph, initialEstimate);
 
+            // Do an extra update to converge withing these 8 iterations
+            isam.update();
+
             // Optimize
             Values currentEstimate = isam.calculateEstimate();