plot incremental position

matlab/+gtsam/plotFlyingResults.m

@@ -1,4 +1,4 @@
-function plotFlyingResults(pts3d, pts3dCov, poses, posesCov, cylinders, options)
+function plotFlyingResults(pts3d, poses, posesCov, cylinders, options)
 % plot the visible points on the cylinders and trajectories
 % author: Zhaoyang Lv
 
@@ -45,15 +45,16 @@ if options.writeVideo
     writeVideo(videoObj, currFrame);
 end
 
-%% plot trajectories
+%% plot trajectories and points 
 posesSize = length(poses);
+pointSize = length(pts3d);
 for i = 1:posesSize
     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
     
     if exist('h_cov', 'var')
-        delete(h_cov);
+        delete(h_pose_cov);
     end
     
     gRp = poses{i}.rotation().matrix();  % rotation from pose to global
@@ -74,7 +75,28 @@ for i = 1:posesSize
     
     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); 
+    h_pose_cov = gtsam.covarianceEllipse3D(C,gPp); 
+      
+    
+    if exist('h_point', 'var')
+        for j = 1:pointSize
+            delete(h_point{j});
+        end
+    end
+    if exist('h_point_cov', 'var')
+        for j = 1:pointSize
+            delete(h_point_cov{j});
+        end
+    end
+    
+    h_point = cell(pointSize, 1);
+    h_point_cov = cell(pointSize, 1);
+    for j = 1:pointSize
+        if ~isempty(pts3d{j}.cov{i})
+            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
+    end
     
     drawnow;
     
@@ -85,8 +107,8 @@ for i = 1:posesSize
 end
 
 
-if exist('h_cov', 'var')
-    delete(h_cov);
+if exist('h_pose_cov', 'var')
+    delete(h_pose_cov);
 end
 
 % wait for two seconds
@@ -99,17 +121,7 @@ pause(2);
 %     delete(h_cylinder);
 % end
 
-pointSize = length(pts3d);
-for i = 1:pointSize
-    plot3(pts3d{i}.x, pts3d{i}.y, pts3d{i}.z);
-    gtsam.covarianceEllipse3D([pts3d{i}.x; pts3d{i}.y; pts3d{i}.z], pts3dCov{i});
-    %drawnow;
-    
-    if options.writeVideo
-        currFrame = getframe(gcf);
-        writeVideo(videoObj, currFrame);
-    end
-end
+
 
 if ~holdstate
     hold off

matlab/+gtsam/points2DTrackStereo.m

@@ -12,7 +12,7 @@ graph = NonlinearFactorGraph;
 %% create the noise factors
 poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
 posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
-stereoNoise = noiseModel.Isotropic.Sigma(3,0.2);
+stereoNoise = noiseModel.Isotropic.Sigma(3, 0.05);
 
 cameraPosesNum = length(cameraPoses);
 
@@ -28,13 +28,21 @@ for i = 1:cylinderNum
         points3d{end}.Z = cell(0);
         points3d{end}.cameraConstraint = cell(0);
         points3d{end}.visiblity = false;
+        points3d{end}.cov = cell(cameraPosesNum);
     end
 end
 
 graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
 
-pts3d = cell(cameraPosesNum, 1);
+%% initialize graph and values
 initialEstimate = Values;
+for i = 1:pointsNum
+    point_j = points3d{i}.data.retract(0.05*randn(3,1));
+    initialEstimate.insert(symbol('p', i), point_j);    
+end
+
+pts3d = cell(cameraPosesNum, 1);
+cameraPosesCov = cell(cameraPosesNum, 1);
 for i = 1:cameraPosesNum 
     pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, options.camera.resolution, cylinders);
     
@@ -52,26 +60,24 @@ for i = 1:cameraPosesNum
         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
+
+    pose_i = cameraPoses{i}.retract(poseNoiseSigmas);
+    initialEstimate.insert(symbol('x', i), pose_i);
+
+    %% linearize the graph
+    marginals = Marginals(graph, initialEstimate);
+
+    for j = 1:pointsNum
+        if points3d{j}.visiblity
+            points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p', j));
+        end       
+    end
     
+    cameraPosesCov{i} = marginals.marginalCovariance(symbol('x', i));    
 end
 
-%% 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
-
-for i = 1:pointsNum
-    point_j = points3d{i}.data.retract(0.1*randn(3,1));
-    initialEstimate.insert(symbol('p', i), point_j);    
-end
-
-
-%% Print the graph
-graph.print(sprintf('\nFactor graph:\n'));
-
-%% linearize the graph
-marginals = Marginals(graph, initialEstimate);
+%% Plot the result
+plotFlyingResults(points3d, cameraPoses, cameraPosesCov, cylinders, options);
 
 %% get all the 2d points track information
 pts2dTracksStereo.pt3d = cell(0);
@@ -87,15 +93,8 @@ for i = 1:pointsNum
     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, cameraPoses, cameraPosesCov, cylinders, options);
-
-%plot3DTrajectory(initialEstimate, '*', 1, 8, marginals);
-%view(3);
+% 
+% %% plot the result with covariance ellipses
+% plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, cameraPoses, cameraPosesCov, cylinders, options);
 
 end

matlab/gtsam_examples/CameraFlyingExample.m

@@ -20,6 +20,7 @@ import gtsam.*
 % test or run
 options.enableTests = false;
 
+%% cylinder options
 % the number of cylinders in the field
 options.cylinder.cylinderNum = 15; % pls be smaller than 20
 % cylinder size
@@ -28,7 +29,7 @@ options.cylinder.height = 10;
 % point density on cylinder
 options.cylinder.pointDensity = 0.05;
 
-%% set up the camera
+%% camera options
 %   parameters set according to the stereo camera:
 %   http://www.matrix-vision.com/USB2.0-single-board-camera-mvbluefox-mlc.html
 
@@ -66,9 +67,14 @@ options.fieldSize = Point2([100, 100]');
 options.speed = 20;
 % number of camera poses in the simulated trajectory
 options.poseNum = options.fieldSize.x / (options.speed / options.camera.fps);
-% display covariance scaling factor
-options.scale = 1;
 
+%% ploting options
+% display covariance scaling factor
+options.plot.scale = 1;
+% plot the trajectory covariance
+options.plot.DISP_TRAJ_COV = true;
+% plot points covariance
+options.plot.POINTS_COV = true;
 
 %% This is for tests
 if options.enableTests
@@ -140,7 +146,6 @@ for i = 1:options.poseNum
     cameraPoses{i} = camera.pose;
 end
 
-
 %% set up camera and get measurements
 if options.camera.IS_MONO 
     % use Monocular Camera
@@ -148,7 +153,7 @@ if options.camera.IS_MONO
         options.camera.resolution, cylinders);
 else 
     % use Stereo Camera
-    [pts2dTracksStereo, estimateValuesStereo] = points2DTrackStereo(options.camera.stereoK, ...
+    pts2dTracksStereo = points2DTrackStereo(options.camera.stereoK, ...
         cameraPoses, options, cylinders);
 end