From 87fe727838cd7a29d0a6f2dbfe6ffc5a7925ce34 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 13 Jun 2012 17:42:06 +0000
Subject: [PATCH] Fixed logic error, added odometry

---
 matlab/VisualISAMInitialize.m | 13 +++++++++----
 1 file changed, 9 insertions(+), 4 deletions(-)

diff --git a/matlab/VisualISAMInitialize.m b/matlab/VisualISAMInitialize.m
index 8873e3c4d..4d0b13931 100644
--- a/matlab/VisualISAMInitialize.m
+++ b/matlab/VisualISAMInitialize.m
@@ -17,10 +17,12 @@ newFactors = visualSLAMGraph;
 initialEstimates = visualSLAMValues;
 for i=1:2
     ii = symbol('x',i);
-    if i==1 && options.hardConstraint % add hard constraint
-        newFactors.addPoseConstraint(ii,truth.cameras{1}.pose);
-    else
-        newFactors.addPosePrior(ii,truth.cameras{i}.pose, noiseModels.pose);
+    if i==1
+        if options.hardConstraint % add hard constraint
+            newFactors.addPoseConstraint(ii,truth.cameras{1}.pose);
+        else
+            newFactors.addPosePrior(ii,truth.cameras{i}.pose, noiseModels.pose);
+        end
     end
     initialEstimates.insertPose(ii,truth.cameras{i}.pose);
 end
@@ -42,6 +44,9 @@ for i=1:2
     end
 end
 
+%% Add odometry between frames 1 and 2
+newFactors.addOdometry(symbol('x',1), symbol('x',2), data.odometry{1}, noiseModels.odometry);
+
 %% Update ISAM
 if options.batchInitialization % Do a full optimize for first two poses
     fullyOptimized = newFactors.optimize(initialEstimates);