diff --git a/matlab/VisualISAMInitialize.m b/matlab/VisualISAMInitialize.m
index 2227aa0f3..8873e3c4d 100644
--- a/matlab/VisualISAMInitialize.m
+++ b/matlab/VisualISAMInitialize.m
@@ -1,4 +1,4 @@
-function [noiseModels,isam,result] = VisualInitialize(data,truth,options)
+function [noiseModels,isam,result] = VisualISAMInitialize(data,truth,options)
 % VisualInitialize: initialize visualSLAM::iSAM object and noise parameters
 % Authors: Duy Nguyen Ta, Frank Dellaert and Alex Cunningham
 
@@ -17,7 +17,7 @@ newFactors = visualSLAMGraph;
 initialEstimates = visualSLAMValues;
 for i=1:2
     ii = symbol('x',i);
-    if i==1 & options.hardConstraint % add hard constraint
+    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);
@@ -28,18 +28,16 @@ end
 %% Add visual measurement factors from two first poses and initialize observed landmarks
 for i=1:2
     ii = symbol('x',i);
-    for j=1:size(data.z,2)
-        jj = symbol('l',j);
-        % Must check whether a landmark was actually observed
-        if ~isempty(data.z{i,j})
-            newFactors.addMeasurement(data.z{i,j}, noiseModels.measurement, ii, jj, data.K);
-            % TODO: initial estimates should not be from ground truth!
-            if ~initialEstimates.exists(jj)
-                initialEstimates.insertPoint(jj, truth.points{j});
-            end
-            if options.pointPriors % add point priors
-                newFactors.addPointPrior(jj, truth.points{j}, noiseModels.point);
-            end
+    for k=1:length(data.Z{i})
+        j = data.J{i}{k};
+        jj = symbol('l',data.J{i}{k});
+        newFactors.addMeasurement(data.Z{i}{k}, noiseModels.measurement, ii, jj, data.K);
+        % TODO: initial estimates should not be from ground truth!
+        if ~initialEstimates.exists(jj)
+            initialEstimates.insertPoint(jj, truth.points{j});
+        end
+        if options.pointPriors % add point priors
+            newFactors.addPointPrior(jj, truth.points{j}, noiseModels.point);
         end
     end
 end
diff --git a/matlab/VisualISAMStep.m b/matlab/VisualISAMStep.m
index 4c044fdda..0f87a66f0 100644
--- a/matlab/VisualISAMStep.m
+++ b/matlab/VisualISAMStep.m
@@ -1,4 +1,4 @@
-function [isam,result] = VisualISAMStep(data,noiseModels,isam,result,options);
+function [isam,result] = VisualISAMStep(data,noiseModels,isam,result,truth, options)
 % VisualISAMStep: execute one update step of visualSLAM::iSAM object
 % Authors: Duy Nguyen Ta and Frank Dellaert
 
@@ -12,9 +12,17 @@ i = result.nrPoses+1;
 odometry = data.odometry{i-1};
 newFactors.addOdometry(symbol('x',i-1), symbol('x',i), odometry, noiseModels.odometry);
 
-%% Add visual measurement factors
-for j=1:size(data.z,2)
-    newFactors.addMeasurement(data.z{i,j}, noiseModels.measurement, symbol('x',i), symbol('l',j), data.K);
+%% Add visual measurement factors and initializations as necessary
+for k=1:length(data.Z{i})
+    zij = data.Z{i}{k};
+    j = data.J{i}{k};
+    jj = symbol('l', j);
+    newFactors.addMeasurement(zij, noiseModels.measurement, symbol('x',i), jj, data.K);
+    % TODO: initialize with something other than truth
+    if ~result.exists(jj) && ~initialEstimates.exists(jj)
+        lmInit = truth.points{j};
+        initialEstimates.insertPoint(jj, lmInit);
+    end
 end
 
 %% Initial estimates for the new pose.
diff --git a/matlab/examples/SFMExample.m b/matlab/examples/SFMExample.m
index 5d454f081..9821b1478 100644
--- a/matlab/examples/SFMExample.m
+++ b/matlab/examples/SFMExample.m
@@ -33,9 +33,10 @@ graph = visualSLAMGraph;
 
 %% Add factors for all measurements
 measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma);
-for i=1:size(data.z,1)
-    for j=1:size(data.z,2)
-        graph.addMeasurement(data.z{i,j}, measurementNoise, symbol('x',i), symbol('l',j), data.K);
+for i=1:length(data.Z)
+    for k=1:length(data.Z{i})
+        j = data.J{i}{k};
+        graph.addMeasurement(data.Z{i}{k}, measurementNoise, symbol('x',i), symbol('l',j), data.K);
     end
 end
 
diff --git a/matlab/examples/VisualISAMExample.m b/matlab/examples/VisualISAMExample.m
index f8423a057..dc6a95ae2 100644
--- a/matlab/examples/VisualISAMExample.m
+++ b/matlab/examples/VisualISAMExample.m
@@ -41,7 +41,7 @@ VisualISAMPlot(truth, data, isam, result, options)
 
 %% Main loop for iSAM: stepping through all poses
 for frame_i=3:options.nrCameras
-    [isam,result] = VisualISAMStep(data,noiseModels,isam,result,options);
+    [isam,result] = VisualISAMStep(data,noiseModels,isam,result,truth,options);
     if mod(frame_i,options.drawInterval)==0
         VisualISAMPlot(truth, data, isam, result, options)
     end
diff --git a/matlab/examples/VisualISAMGenerateData.m b/matlab/examples/VisualISAMGenerateData.m
index fab8185f9..3c55417d9 100644
--- a/matlab/examples/VisualISAMGenerateData.m
+++ b/matlab/examples/VisualISAMGenerateData.m
@@ -32,7 +32,9 @@ for i=1:options.nrCameras
     truth.cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), truth.K);
     % Create measurements
     for j=1:nrPoints
-        data.z{i,j} = truth.cameras{i}.project(truth.points{j});
+        % All landmarks seen in every frame
+        data.Z{i}{j} = truth.cameras{i}.project(truth.points{j});
+        data.J{i}{j} = j;
     end    
 end