diff --git a/examples/matlab/VO_Example.m b/examples/matlab/VO_Example.m
index e95893898..fa1512e00 100644
--- a/examples/matlab/VO_Example.m
+++ b/examples/matlab/VO_Example.m
@@ -23,55 +23,60 @@ initial = visualSLAMValues;
 
 %% load the initial poses from VO
 % row format: camera_id 4x4 pose (row, major)
-c = dlmread('../Data/VO_camera_poses.txt');
-
-for i=1:size(c,1)
-    pose = gtsamPose3(reshape(c(i,2:17),4,4)');
-    initial.insertPose(symbol('x',c(i,1)),pose);
+fprintf(1,'Reading data\n');
+cameras = dlmread('../Data/VO_camera_poses.txt');
+for i=1:size(cameras,1)
+    pose = gtsamPose3(reshape(cameras(i,2:17),4,4)');
+    initial.insertPose(symbol('x',cameras(i,1)),pose);
 end
 
 %% load stereo measurements and initialize landmarks
 % camera_id landmark_id uL uR v X Y Z
-m = dlmread('../Data/VO_stereo_factors.txt');
+measurements = dlmread('../Data/VO_stereo_factors.txt');
 
-for i=1:size(m,1)
-    sf = m(i,:);
+fprintf(1,'Creating Graph\n'); tic
+for i=1:size(measurements,1)
+    sf = measurements(i,:);
     graph.addStereoMeasurement(gtsamStereoPoint2(sf(3),sf(4),sf(5)), stereo_model, ...
-    symbol('x', sf(1)), symbol('l', sf(2)), K);
-
+        symbol('x', sf(1)), symbol('l', sf(2)), K);
+    
     if ~initial.exists(symbol('l',sf(2)))
-        % 3D landmarks are stored in camera coordinates: transform 
+        % 3D landmarks are stored in camera coordinates: transform
         % to world coordinates using the respective initial pose
         pose = initial.pose(symbol('x', sf(1)));
         world_point = pose.transform_from(gtsamPoint3(sf(6),sf(7),sf(8)));
         initial.insertPoint(symbol('l',sf(2)), world_point);
     end
 end
+toc
 
 %% add a constraint on the starting pose
 key = symbol('x',1);
 first_pose = initial.pose(key);
-graph.addPoseConstraint(symbol('x',1), first_pose);
+graph.addPoseConstraint(key, first_pose);
 
 %% optimize
+fprintf(1,'Optimizing\n'); tic
 result = graph.optimize(initial);
+toc
 
 %% visualize initial trajectory, final trajectory, and final points
-figure(1); clf;
+figure(1); clf; hold on;
 
-% initial trajectory in red
-plot3(initial.xs(),initial.ys(),initial.zs(), '-*r','LineWidth',2);
-hold on;
+% initial trajectory in red (rotated so Z is up)
+plot3(initial.zs(),-initial.xs(),-initial.ys(), '-*r','LineWidth',2);
 
-% final trajectory in green
-plot3(result.xs(),result.ys(),result.zs(), '-*g','LineWidth',2);
+% final trajectory in green (rotated so Z is up)
+plot3(result.zs(),-result.xs(),-result.ys(), '-*g','LineWidth',2);
 xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
 
 % switch to XZ view
 view([0 0]);
 
-% optimized 3D points
+% optimized 3D points (rotated so Z is up)
 points = result.points();
-plot3(points(:,1),points(:,2),points(:,3),'.');
+plot3(points(:,3),-points(:,1),-points(:,2),'.');
 
-axis([-30 30 -30 30 0 60]);
+axis([0 100 -20 20 -5 20]);
+axis equal
+view(3)