add labels

matlab/+gtsam/covarianceEllipse3D.m

@@ -6,6 +6,8 @@ function sc = covarianceEllipse3D(c,P)
 %
 % Modified from http://www.mathworks.com/matlabcentral/newsreader/view_thread/42966
 
+hold on
+
 [e,s] = svd(P);
 k = 11.82; 
 radii = k*sqrt(diag(s));

matlab/+gtsam/plotCamera.m

@@ -1,4 +1,6 @@
 function plotCamera(pose, axisLength)
+	hold on
+
     C = pose.translation().vector();
     R = pose.rotation().matrix();
     

matlab/+gtsam/plotFlyingResults.m

@@ -1,11 +1,35 @@
 function plotFlyingResults(pts3d, poses, posesCov, cylinders, options)
 % plot the visible points on the cylinders and trajectories
+%
 % author: Zhaoyang Lv
 
 import gtsam.*
 
-holdstate = ishold;
-hold on
+figID = 1;
+figure(figID);
+set(gcf, 'Position', [80,1,1800,1000]);
+
+
+%% plot all the cylinders and sampled points
+
+axis equal
+axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 30]);
+xlabel('X (m)');
+ylabel('Y (m)');
+zlabel('Height (m)');
+
+if options.camera.IS_MONO
+    h_title = title('Quadrotor Flight Simulation with Monocular Camera');
+else
+    h_title = title('Quadrotor Flight Simulation with Stereo Camera');
+end
+
+text(100,1750,0, sprintf('Flying Speed: %0.1f\n', options.speed))
+
+view([30, 30]);
+
+hlight = camlight('headlight'); 
+lighting gouraud
 
 if(options.writeVideo)
     videoObj = VideoWriter('Camera_Flying_Example.avi');
@@ -14,36 +38,24 @@ if(options.writeVideo)
     open(videoObj);
 end
 
-%% plot all the cylinders and sampled points
-% now is plotting on a 100 * 100 field
-figID = 1;
-figure(figID);
-
-view([30, 30]);
-
-hlight = camlight('headlight'); 
-lighting gouraud
 
 sampleDensity = 120;
 cylinderNum = length(cylinders);
+h_cylinder = cell(cylinderNum);
 for i = 1:cylinderNum
+    
+    hold on
+    
     [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height);
 
     X = X + cylinders{i}.centroid.x;
     Y = Y + cylinders{i}.centroid.y;
     Z = Z * cylinders{i}.height;
 
-    h_cylinder = surf(X,Y,Z);
-    set(h_cylinder, 'FaceColor', [0 0 1], 'FaceAlpha', 0.2);
-    h_cylinder.AmbientStrength = 1;
-    hold on
-end
+    h_cylinder{i} = surf(X,Y,Z);
+    set(h_cylinder{i}, 'FaceColor', [0 0 1], 'FaceAlpha', 0.2);
+    h_cylinder{i}.AmbientStrength = 0.8;
     
-drawnow;
-
-if options.writeVideo
-    currFrame = getframe(gcf);
-    writeVideo(videoObj, currFrame);
 end
 
 %% plot trajectories and points 
@@ -51,36 +63,36 @@ posesSize = length(poses);
 pointSize = length(pts3d);
 for i = 1:posesSize
     if i > 1
+        hold on
         plot3([poses{i}.x; poses{i-1}.x], [poses{i}.y; poses{i-1}.y], [poses{i}.z; poses{i-1}.z], '-b');
     end
     
-    if exist('h_cov', 'var')
+    if exist('h_pose_cov', 'var')
         delete(h_pose_cov);
     end
        
-    plotCamera(poses{i}, 2);
+    %plotCamera(poses{i}, 3);
+    
+    gRp = poses{i}.rotation().matrix();  % rotation from pose to global
+    C = poses{i}.translation().vector();
+    axisLength = 3;
+    
+    xAxis = C+gRp(:,1)*axisLength;
+    L = [C xAxis]';
+    line(L(:,1),L(:,2),L(:,3),'Color','r');
+    
+    yAxis = C+gRp(:,2)*axisLength;
+    L = [C yAxis]';
+    line(L(:,1),L(:,2),L(:,3),'Color','g');
+    
+    zAxis = C+gRp(:,3)*axisLength;
+    L = [C zAxis]';
+    line(L(:,1),L(:,2),L(:,3),'Color','b');
     
-     gRp = poses{i}.rotation().matrix();  % rotation from pose to global
-     C = poses{i}.translation().vector();
-%     axisLength = 2;
-%     
-%     xAxis = C+gRp(:,1)*axisLength;
-%     L = [C xAxis]';
-%     line(L(:,1),L(:,2),L(:,3),'Color','r');
-%     
-%     yAxis = C+gRp(:,2)*axisLength;
-%     L = [C yAxis]';
-%     line(L(:,1),L(:,2),L(:,3),'Color','g');
-%     
-%     zAxis = C+gRp(:,3)*axisLength;
-%     L = [C zAxis]';
-%     line(L(:,1),L(:,2),L(:,3),'Color','b');
-%     
     pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame    
     gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
     h_pose_cov = gtsam.covarianceEllipse3D(C,gPp); 
     
-    
     if exist('h_point', 'var')
         for j = 1:pointSize
             delete(h_point{j});
@@ -96,6 +108,7 @@ for i = 1:posesSize
     h_point_cov = cell(pointSize, 1);
     for j = 1:pointSize
         if ~isempty(pts3d{j}.cov{i})
+            hold on
             h_point{j} = plot3(pts3d{j}.data.x, pts3d{j}.data.y, pts3d{j}.data.z);
             h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], pts3d{j}.cov{i});
         end
@@ -104,7 +117,7 @@ for i = 1:posesSize
     axis equal
     axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); 
     
-    drawnow;
+    drawnow
     
     if options.writeVideo
         currFrame = getframe(gcf);
@@ -120,17 +133,32 @@ end
 % wait for two seconds
 pause(2);
 
-% change views angle
-for i = 0 : 0.5 : 60
-    view([i + 30, i]);
+%% change views angle
+for i = 30 : i : 90
+    view([30, i]);
+
+    if options.writeVideo
+        currFrame = getframe(gcf);
+        writeVideo(videoObj, currFrame);
+    end
+    
+    drawnow
 end
 
-% camera flying through
+% changing perspective
+
+
+%% camera flying through video
 for i = 1 : posesSize
     campos([poses{i}.x, poses{i}.y, poses{i}.z]);
     camtarget([options.fieldSize.x/2, options.fieldSize.y/2, 0]);
     camlight(hlight, 'headlight');
 
+    if options.writeVideo
+        currFrame = getframe(gcf);
+        writeVideo(videoObj, currFrame);
+    end
+    
     drawnow
 end
 

matlab/+gtsam/points2DTrackStereo.m

@@ -2,6 +2,7 @@ function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(K, cameraPos
 % Assess how accurately we can reconstruct points from a particular monocular camera setup. 
 % After creation of the factor graph for each track, linearize it around ground truth. 
 % There is no optimization
+%
 % @author: Zhaoyang Lv
 
 import gtsam.*