diff --git a/matlab/VisualISAMInitialize.m b/matlab/VisualISAMInitialize.m
index 13a4258c9..2227aa0f3 100644
--- a/matlab/VisualISAMInitialize.m
+++ b/matlab/VisualISAMInitialize.m
@@ -1,6 +1,6 @@
 function [noiseModels,isam,result] = VisualInitialize(data,truth,options)
 % VisualInitialize: initialize visualSLAM::iSAM object and noise parameters
-% Authors: Duy Nguyen Ta and Frank Dellaert
+% Authors: Duy Nguyen Ta, Frank Dellaert and Alex Cunningham
 
 %% Initialize iSAM
 isam = visualSLAMISAM(options.reorderInterval);
@@ -25,25 +25,25 @@ for i=1:2
     initialEstimates.insertPose(ii,truth.cameras{i}.pose);
 end
 
-%% Add visual measurement factors from two first poses
+%% 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);
-        newFactors.addMeasurement(data.z{i,j}, noiseModels.measurement, ii, jj, data.K);
+        % 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
+        end
     end
 end
 
-%% Initialize points, possibly add priors on them
-% TODO: should not be from ground truth!
-for j=1:size(data.z,2)
-    jj = symbol('l',j);
-    if options.pointPriors % add point priors
-        newFactors.addPointPrior(jj, truth.points{j}, noiseModels.point);
-    end
-    initialEstimates.insertPoint(jj, truth.points{j});
-end
-
 %% Update ISAM
 if options.batchInitialization % Do a full optimize for first two poses
     fullyOptimized = newFactors.optimize(initialEstimates);
diff --git a/matlab/VisualISAMPlot.m b/matlab/VisualISAMPlot.m
index 8e8dce6f1..6d3dde225 100644
--- a/matlab/VisualISAMPlot.m
+++ b/matlab/VisualISAMPlot.m
@@ -11,11 +11,14 @@ hold on;
 
 %% Plot points
 for j=1:N
+    %% TODO: use the actual set of keys present
     jj = symbol('l',j);
-    point_j = result.point(jj);
-    plot3(point_j.x, point_j.y, point_j.z,'marker','o');
-    P = isam.marginalCovariance(jj);
-    covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P);
+    if result.exists(jj)
+        point_j = result.point(jj);
+        plot3(point_j.x, point_j.y, point_j.z,'marker','o');
+        P = isam.marginalCovariance(jj);
+        covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P);
+    end
 end
 
 %% Plot cameras