ploting trajectories animation

matlab/+gtsam/covarianceEllipse3D.m

@@ -1,4 +1,4 @@
-function covarianceEllipse3D(c,P)
+function sc = covarianceEllipse3D(c,P)
 % covarianceEllipse3D plots a Gaussian as an uncertainty ellipse
 % Based on Maybeck Vol 1, page 366
 % k=2.296 corresponds to 1 std, 68.26% of all probability

matlab/+gtsam/plotFlyingResults.m

@@ -1,42 +1,90 @@
-function plotFlyingResults(pts3d, covariance, values, marginals)
+function plotFlyingResults(pts3d, pts3dCov, poses, posesCov, cylinders, options)
 % plot the visible points on the cylinders and trajectories
 % author: Zhaoyang Lv
 
 import gtsam.*
 
-haveMarginals = exist('marginals', 'var');
-keys = KeyVector(values.keys);
-
 holdstate = ishold;
 hold on
 
-keys = KeyVector(values.keys);
+if(options.writeVideo)
+    videoObj = VideoWriter('Camera_Flying_Example.avi');
+    videoObj.Quality = 100;
+    videoObj.FrameRate = options.camera.fps;
+end
+
+%% plot all the cylinders and sampled points
+% now is plotting on a 100 * 100 field
+figID = 1;
+figure(figID);
+
+axis equal
+axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]);
+
+view(3);
+
+sampleDensity = 120;
+cylinderNum = length(cylinders);
+for i = 1:cylinderNum                
+    [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;
+
+    cylinderHandle = surf(X,Y,Z);
+    set(cylinderHandle, 'FaceAlpha', 0.5);
+    hold on
+end
 
 %% plot trajectories
-for i = 0:keys.size - 1
+posesSize = length(poses);
-    if exist('h_result', 'var')
+for i = 1:posesSize
-        delete(h_result);
+    if i > 1
+        plot3([poses{i}.x; poses{i-1}.x], [poses{i}.y; poses{i-1}.y], [poses{i}.z; poses{i-1}.z], '-b');
     end
-
-    key = keys.at(i);
-    pose = keys.at(key);
     
-    P = marginals.marginalCovariance(key);
+    if exist('h_cov', 'var')
+        delete(h_cov);
+    end
     
-    h_result = gtsam.plotPose3(pose, P, 1);
+    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_cov = gtsam.covarianceEllipse3D(C,gPp); 
+    
+    drawnow;
 end
 
 %% plot point covariance
 
-
+pointSize = length(pts3d);
-
+for i = 1:pointSize
-if exist('h_result', 'var')
+    
-    delete(h_result);
 end
 
 if ~holdstate
     hold off
 end
-    
+
+%%close video
+if(options.writeVideo)
+    close(videoObj);
+end
+
 end

matlab/+gtsam/points2DTrackStereo.m

@@ -1,4 +1,4 @@
-function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(K, cameraPoses, imageSize, cylinders)
+function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(K, cameraPoses, options, cylinders)
 % 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
@@ -36,7 +36,7 @@ graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
 pts3d = cell(cameraPosesNum, 1);
 initialEstimate = Values;
 for i = 1:cameraPosesNum 
-    pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, imageSize, cylinders);
+    pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, options.camera.resolution, cylinders);
     
     if isempty(pts3d{i}.Z)
         continue;
@@ -48,12 +48,11 @@ for i = 1:cameraPosesNum
         points3d{index}.Z{end+1} = pts3d{i}.Z{j};
         points3d{index}.cameraConstraint{end+1} = i;
         points3d{index}.visiblity = true;
+  
+        graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ...
+            stereoNoise, symbol('x', i), symbol('p', index), K));    
     end
     
-    for j = 1:length(pts3d{i}.Z)
-        graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ...
-            stereoNoise, symbol('x', i), symbol('p', pts3d{i}.overallIdx{j}), K));    
-    end
 end
 
 %% initialize graph and values
@@ -64,18 +63,7 @@ end
 
 for i = 1:pointsNum
     point_j = points3d{i}.data.retract(0.1*randn(3,1));
-    initialEstimate.insert(symbol('p', i), point_j);
+    initialEstimate.insert(symbol('p', i), point_j);    
-    
-    if ~points3d{i}.visiblity
-        continue;
-    end
-    
-    factorNum = length(points3d{i}.Z);
-    for j = 1:factorNum
-        cameraIdx = points3d{i}.cameraConstraint{j};
-        graph.add(GenericStereoFactor3D(StereoPoint2(points3d{i}.Z{j}.uL, points3d{i}.Z{j}.uR, points3d{i}.Z{j}.v), ...
-            stereoNoise, symbol('x', cameraIdx), symbol('p', points3d{i}.cameraConstraint{j}), K));            
-    end    
 end
 
 
@@ -96,12 +84,16 @@ for i = 1:pointsNum
     
     pts2dTracksStereo.pt3d{end+1} = points3d{i}.data;
     pts2dTracksStereo.Z{end+1} = points3d{i}.Z;
-    pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i));    
+    pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i));       
-   
+end
+
+cameraPosesCov = cell(cameraPosesNum, 1);
+for i = 1:cameraPosesNum
+    cameraPosesCov{i} = marginals.marginalCovariance(symbol('x', i));
 end
 
 %% plot the result with covariance ellipses
-plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, initialEstimate, marginals);
+plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, cameraPoses, cameraPosesCov, cylinders, options);
 
 %plot3DTrajectory(initialEstimate, '*', 1, 8, marginals);
 %view(3);

matlab/gtsam_examples/CameraFlyingExample.m

@@ -37,7 +37,7 @@ options.camera.IS_MONO = false;
 % the field of view of camera
 options.camera.fov = 120;
 % fps for image
-options.camera.fps = 25;
+options.camera.fps = 10;
 % camera pixel resolution
 options.camera.resolution = Point2(752, 480);
 % camera horizon
@@ -69,12 +69,6 @@ options.poseNum = options.fieldSize.x / (options.speed / options.camera.fps);
 % display covariance scaling factor
 options.scale = 1;
 
-% if(options.writeVideo)
-%     videoObj = VideoWriter('Camera_Flying_Example.avi');
-%     videoObj.Quality = 100;
-%     videoObj.FrameRate = options.fps;
-% end
-
 
 %% This is for tests
 if options.enableTests
@@ -133,12 +127,6 @@ while i <= cylinderNum
     i = i+1;
 end
 
-%% plot all the cylinders and sampled points
-% now is plotting on a 100 * 100 field
-figID = 1;
-figure(figID);
-plotCylinderSamples(cylinders, options, figID);
-
 %% generate ground truth camera trajectories: a line
 KMono = Cal3_S2(525,525,0,320,240);
 cameraPoses = cell(options.poseNum, 1);
@@ -161,22 +149,11 @@ if options.camera.IS_MONO
 else 
     % use Stereo Camera
     [pts2dTracksStereo, estimateValuesStereo] = points2DTrackStereo(options.camera.stereoK, ...
-        cameraPoses, options.camera.resolution, cylinders);
+        cameraPoses, options, cylinders);
 end
 
-%% plot all the projected points
 
 
-% plot the 2D tracks
-
-% ToDo: plot the trajectories
-%plot3DTrajectory();
-
-
-%%close video
-% if(options.writeVideo)
-%     close(videoObj);
-% end