diff --git a/matlab/+gtsam/covarianceEllipse3D.m b/matlab/+gtsam/covarianceEllipse3D.m index 99a164068..c99b19117 100644 --- a/matlab/+gtsam/covarianceEllipse3D.m +++ b/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)); diff --git a/matlab/+gtsam/plotCamera.m b/matlab/+gtsam/plotCamera.m index f5f164678..d0d1f45b7 100644 --- a/matlab/+gtsam/plotCamera.m +++ b/matlab/+gtsam/plotCamera.m @@ -1,4 +1,6 @@ function plotCamera(pose, axisLength) + hold on + C = pose.translation().vector(); R = pose.rotation().matrix(); diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m index 57a656a7f..85042f5a8 100644 --- a/matlab/+gtsam/plotFlyingResults.m +++ b/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); -for i = 1:cylinderNum +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 - -drawnow; - -if options.writeVideo - currFrame = getframe(gcf); - writeVideo(videoObj, currFrame); + h_cylinder{i} = surf(X,Y,Z); + set(h_cylinder{i}, 'FaceColor', [0 0 1], 'FaceAlpha', 0.2); + h_cylinder{i}.AmbientStrength = 0.8; + end %% plot trajectories and points @@ -51,35 +63,35 @@ 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 @@ -96,15 +108,16 @@ 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 end axis equal - axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); + axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); - drawnow; + drawnow if options.writeVideo currFrame = getframe(gcf); @@ -120,16 +133,31 @@ 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 diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 295b4d18e..60c9f1295 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/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.*