diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m
index 7cdd99fd3..83bf45fbd 100644
--- a/matlab/+gtsam/points2DTrackMonocular.m
+++ b/matlab/+gtsam/points2DTrackMonocular.m
@@ -21,43 +21,58 @@ cameraPosesNum = length(cameraPoses);
 %% add measurements and initial camera & points values
 pointsNum = 0;
 cylinderNum = length(cylinders);
+points3d = cell(0);
 for i = 1:cylinderNum
-    pointsNum = pointsNum + length(cylinders{i}.Points);
+    cylinderPointsNum = length(cylinders{i}.Points);
+    pointsNum = pointsNum + cylinderPointsNum; 
+    for j = 1:cylinderPointsNum
+        points3d{end+1}.data = cylinders{i}.Points{j};
+        points3d{end}.Z = cell(0);
+        points3d{end}.cameraConstraint = cell(0);
+        points3d{end}.visiblity = false;
+    end
 end
 
+graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
+
 pts3d = cell(cameraPosesNum, 1);
 initialEstimate = Values;
-initialized = false;
 for i = 1:cameraPosesNum 
     
     cameraPose = cameraPoses{i};    
     pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders);
-   
-    if ~initialized
-        graph.add(PriorFactorPose3(symbol('x', 1), cameraPose, posePriorNoise));
-        initialized = true;
-    end
     
     measurementNum = length(pts3d{i}.Z);
     for j = 1:measurementNum
-        graph.add(GenericProjectionFactorCal3_S2(pts3d{i}.Z{j}, ...
-            measurementNoise, symbol('x', i), symbol('p', pts3d{i}.overallIdx{j}), K) );    
+        index = pts3d{i}.overallIdx{j};
+        points3d{index}.Z{end+1} = pts3d{i}.Z{j};
+        points3d{index}.cameraConstraint{end+1} = i;
+        points3d{index}.visiblity = true;
     end
-
+    
 end
 
-%% initialize cameras and points close to ground truth 
+%% initialize graph and values
 for i = 1:cameraPosesNum
     pose_i = cameraPoses{i}.retract(0.1*randn(6,1));
     initialEstimate.insert(symbol('x', i), pose_i);    
 end
-ptsIdx = 0;
-for i = 1:length(cylinders)
-    for j = 1:length(cylinders{i}.Points)
-        ptsIdx = ptsIdx + 1;
-        point_j = cylinders{i}.Points{j}.retract(0.1*randn(3,1));
-        initialEstimate.insert(symbol('p', ptsIdx), point_j);
+
+for i = 1:pointsNum
+    % single measurement. not added to graph
+    factorNum = length(points3d{i}.Z);
+    if factorNum > 1
+        for j = 1:factorNum
+            cameraIdx = points3d{i}.cameraConstraint{j};
+            graph.add(GenericProjectionFactorCal3_S2(points3d{i}.Z{j}, ...
+                measurementNoise, symbol('x', cameraIdx), symbol('p', points3d{i}.cameraConstraint{j}), K) );    
+        end
     end
+   
+    % add in values
+    point_j = points3d{i}.data.retract(0.1*randn(3,1));
+    initialEstimate.insert(symbol('p', i), point_j);
+    
 end
 
 %% Print the graph
@@ -67,6 +82,19 @@ marginals = Marginals(graph, initialEstimate);
 
 %% get all the points track information
 % currently throws the Indeterminant linear system exception
+for i = 1:pointsNum
+    if points3d{i}.visiblity
+        pts2dTracksMono.pt3d{i} = points3d{i}.data;
+        pts2dTracksMono.Z = points3d{i}.Z;
+
+        if length(points3d{i}.Z) == 1
+            %pts2dTracksMono.cov{i} singular matrix 
+        else 
+            pts2dTracksMono.cov{i} = marginals.marginalCovariance(symbol('p', i));    
+        end
+    end
+end
+
 for k = 1:cameraPosesNum
     num = length(pts3d{k}.data);
     for i = 1:num