From 05d5179bc399ab86eaa40b0fe1c8b2d0f0bbc3f4 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sat, 29 Sep 2018 17:31:46 -0400
Subject: [PATCH] Some more cleanup

---
 examples/ISAM2Example_SmartFactor.cpp | 31 ++++++++++++++++-----------
 1 file changed, 18 insertions(+), 13 deletions(-)

diff --git a/examples/ISAM2Example_SmartFactor.cpp b/examples/ISAM2Example_SmartFactor.cpp
index 77e29b8ff..8331ade04 100644
--- a/examples/ISAM2Example_SmartFactor.cpp
+++ b/examples/ISAM2Example_SmartFactor.cpp
@@ -5,11 +5,10 @@
  */
 
 #include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/SmartProjectionPoseFactor.h>
 
-#include <gtsam/nonlinear/ISAM2.h>
-
 #include <iostream>
 #include <vector>
 
@@ -56,27 +55,32 @@ int main(int argc, char* argv[]) {
 
   vector<Pose3> poses = {pose1, pose2, pose3, pose4, pose5};
 
-  SmartFactor::shared_ptr smartFactor(new SmartFactor(measurementNoise, K));
-  graph.push_back(smartFactor);
-
+  // Add first pose
   graph.emplace_shared<PriorFactor<Pose3>>(X(0), poses[0], noise);
   initialEstimate.insert(X(0), poses[0].compose(delta));
-  smartFactor->add(PinholePose<Cal3_S2>(poses[0], K).project(point), X(0));
 
+  // Create smart factor with measurement from first pose only
+  SmartFactor::shared_ptr smartFactor(new SmartFactor(measurementNoise, K));
+  smartFactor->add(PinholePose<Cal3_S2>(poses[0], K).project(point), X(0));
+  graph.push_back(smartFactor);
+
+  // loop over remaining poses
   for (size_t i = 1; i < 5; i++) {
     cout << "****************************************************" << endl;
     cout << "i = " << i << endl;
 
-    // Add measurement to smart factor
-    PinholePose<Cal3_S2> camera(poses[i], K);
-    Point2 measurement = camera.project(point);
-    smartFactor->add(measurement, X(i));
-    cout << "Measurement " << i << "" << measurement << endl;
-
     // Add prior on new pose
     graph.emplace_shared<PriorFactor<Pose3>>(X(i), poses[i], noise);
     initialEstimate.insert(X(i), poses[i].compose(delta));
 
+    // "Simulate" measurement from this pose
+    PinholePose<Cal3_S2> camera(poses[i], K);
+    Point2 measurement = camera.project(point);
+    cout << "Measurement " << i << "" << measurement << endl;
+
+    // Add measurement to smart factor
+    smartFactor->add(measurement, X(i));
+
     // Update iSAM2
     ISAM2Result result = isam.update(graph, initialEstimate);
     result.print();
@@ -84,7 +88,8 @@ int main(int argc, char* argv[]) {
     cout << "Detailed results:" << endl;
     for (auto keyedStatus : result.detail->variableStatus) {
       const auto& status = keyedStatus.second;
-      cout << keyedStatus.first << " {" << endl;
+      PrintKey(keyedStatus.first);
+      cout << " {" << endl;
       cout << "reeliminated: " << status.isReeliminated << endl;
       cout << "relinearized above thresh: " << status.isAboveRelinThreshold
            << endl;