From 0f100e8228a347352e4487b0d89725e94f9a9ab2 Mon Sep 17 00:00:00 2001
From: lvzhaoyang <lvzhaoyang1990@gmail.com>
Date: Thu, 22 Jan 2015 20:39:02 -0500
Subject: [PATCH] add the scale of visualization for covariance matrix

---
 matlab/+gtsam/covarianceEllipse.m           |  4 +++-
 matlab/+gtsam/covarianceEllipse3D.m         |  6 ++++-
 matlab/+gtsam/plotFlyingResults.m           | 26 +++++++++++----------
 matlab/gtsam_examples/CameraFlyingExample.m |  5 ++--
 4 files changed, 25 insertions(+), 16 deletions(-)

diff --git a/matlab/+gtsam/covarianceEllipse.m b/matlab/+gtsam/covarianceEllipse.m
index cdc239bb2..829487dc6 100644
--- a/matlab/+gtsam/covarianceEllipse.m
+++ b/matlab/+gtsam/covarianceEllipse.m
@@ -8,6 +8,8 @@ function h = covarianceEllipse(x,P,color, k)
 % it is assumed x and y are the first two components of state x
 % k is scaling for std deviations, defaults to 1 std
 
+hold on
+
 [e,s] = eig(P(1:2,1:2));
 s1 = s(1,1);
 s2 = s(2,2);
@@ -32,4 +34,4 @@ h = line(ex,ey,'color',color);
         y = c(2) + points(2,:);
     end
 
-end
\ No newline at end of file
+end
diff --git a/matlab/+gtsam/covarianceEllipse3D.m b/matlab/+gtsam/covarianceEllipse3D.m
index c99b19117..4364e0fe4 100644
--- a/matlab/+gtsam/covarianceEllipse3D.m
+++ b/matlab/+gtsam/covarianceEllipse3D.m
@@ -1,4 +1,4 @@
-function sc = covarianceEllipse3D(c,P)
+function sc = covarianceEllipse3D(c,P,scale)
 % 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
@@ -12,6 +12,10 @@ hold on
 k = 11.82; 
 radii = k*sqrt(diag(s));
 
+if exist('scale', 'var')
+    radii = radii * scale;
+end
+
 % generate data for "unrotated" ellipsoid
 [xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3),8);
 
diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m
index 85042f5a8..5d4a287c4 100644
--- a/matlab/+gtsam/plotFlyingResults.m
+++ b/matlab/+gtsam/plotFlyingResults.m
@@ -13,11 +13,13 @@ 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]);
+axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
 xlabel('X (m)');
 ylabel('Y (m)');
 zlabel('Height (m)');
 
+h = cameratoolbar('Show');
+
 if options.camera.IS_MONO
     h_title = title('Quadrotor Flight Simulation with Monocular Camera');
 else
@@ -91,7 +93,7 @@ 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_pose_cov = gtsam.covarianceEllipse3D(C,gPp); 
+    h_pose_cov = gtsam.covarianceEllipse3D(C, gPp, options.plot.covarianceScale); 
     
     if exist('h_point', 'var')
         for j = 1:pointSize
@@ -110,13 +112,14 @@ for i = 1:posesSize
         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});
+            h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], ...
+                pts3d{j}.cov{i}, options.plot.covarianceScale);
         end
-    end
+    end 
     
     axis equal
-    axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); 
-    
+    axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
+
     drawnow
     
     if options.writeVideo
@@ -149,11 +152,15 @@ end
 
 
 %% camera flying through video
+camzoom(0.8);
 for i = 1 : posesSize
+    
+    hold on
+    
     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);
@@ -162,11 +169,6 @@ for i = 1 : posesSize
     drawnow
 end
 
-
-if ~holdstate
-    hold off
-end
-
 %%close video
 if(options.writeVideo)
     close(videoObj);
diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m
index 3538f38b6..36b7635e2 100644
--- a/matlab/gtsam_examples/CameraFlyingExample.m
+++ b/matlab/gtsam_examples/CameraFlyingExample.m
@@ -69,8 +69,9 @@ options.speed = 20;
 options.height = 30;
 
 %% ploting options
-% display covariance scaling factor
-options.plot.scale = 1;
+% display covariance scaling factor, default to be 1.
+% The covariance visualization default models 99% of all probablity 
+options.plot.covarianceScale = 1;
 % plot the trajectory covariance
 options.plot.DISP_TRAJ_COV = true;
 % plot points covariance