diff --git a/matlab/+gtsam/covarianceEllipse3D.m b/matlab/+gtsam/covarianceEllipse3D.m index 9682a9fc1..99a164068 100644 --- a/matlab/+gtsam/covarianceEllipse3D.m +++ b/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 diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m index c214d948d..3c3e1e670 100644 --- a/matlab/+gtsam/plotFlyingResults.m +++ b/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 - if exist('h_result', 'var') - delete(h_result); +posesSize = length(poses); +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 - - 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 - - -if exist('h_result', 'var') - delete(h_result); +pointSize = length(pts3d); +for i = 1:pointSize + end if ~holdstate hold off end - + +%%close video +if(options.writeVideo) + close(videoObj); +end + end \ No newline at end of file diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 3651bff6d..c80c90d67 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/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); - - 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 + initialEstimate.insert(symbol('p', i), point_j); 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); diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 99fe4ca80..bcb3861f3 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/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