From 18a72718aa193126eaddd3f1f7db807e32247f05 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Sun, 11 Aug 2013 19:29:01 +0000
Subject: [PATCH] Reenabled ISAM2 in SolverComparer

---
 examples/SolverComparer.cpp | 41 ++++++++++++++++++++++---------------
 1 file changed, 25 insertions(+), 16 deletions(-)

diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp
index b46ea60b8..19281ce0f 100644
--- a/examples/SolverComparer.cpp
+++ b/examples/SolverComparer.cpp
@@ -165,10 +165,15 @@ int main(int argc, char *argv[]) {
   if(!datasetName.empty())
   {
     cout << "Loading dataset " << datasetName << endl;
-    string datasetFile = findExampleDataFile(datasetName);
-    std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data =
-      load2D(datasetFile);
-    datasetMeasurements = *data.first;
+    try {
+      string datasetFile = findExampleDataFile(datasetName);
+      std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data =
+        load2D(datasetFile);
+      datasetMeasurements = *data.first;
+    } catch(std::exception& e) {
+      cout << e.what() << endl;
+      exit(1);
+    }
   }
 
 
@@ -262,21 +267,23 @@ void runIncremental()
 
         // Initialize the new variable
         if(measurement->key1() > measurement->key2()) {
-          if(step == 1)
-            newVariables.insert(measurement->key1(), measurement->measured().inverse());
-          else {
-            Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key2());
-            newVariables.insert(measurement->key1(), prevPose * measurement->measured().inverse());
+          if(!newVariables.exists(measurement->key1())) { // Only need to check newVariables since loop closures come after odometry
+            if(step == 1)
+              newVariables.insert(measurement->key1(), measurement->measured().inverse());
+            else {
+              Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key2());
+              newVariables.insert(measurement->key1(), prevPose * measurement->measured().inverse());
+            }
           }
-          //        cout << "Initializing " << step << endl;
         } else {
-          if(step == 1)
-            newVariables.insert(measurement->key2(), measurement->measured());
-          else {
-            Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key1());
-            newVariables.insert(measurement->key2(), prevPose * measurement->measured());
+          if(!newVariables.exists(measurement->key1())) { // Only need to check newVariables since loop closures come after odometry
+            if(step == 1)
+              newVariables.insert(measurement->key2(), measurement->measured());
+            else {
+              Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key1());
+              newVariables.insert(measurement->key2(), prevPose * measurement->measured());
+            }
           }
-          //        cout << "Initializing " << step << endl;
         }
       }
       else if(BearingRangeFactor<Pose, Point2>::shared_ptr measurement =
@@ -348,6 +355,8 @@ void runIncremental()
     }
   }
 
+  tictoc_print_();
+
   // Compute marginals
   //try {
   //  Marginals marginals(graph, values);