diff --git a/examples/matlab/VisualISAMExample.m b/examples/matlab/VisualISAMExample.m index bbe67bd6b..c1c4aefeb 100644 --- a/examples/matlab/VisualISAMExample.m +++ b/examples/matlab/VisualISAMExample.m @@ -19,19 +19,18 @@ for j=1:nPoints points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]'); end -%% Create camera poses on a circle around the triangle +%% Create camera cameras on a circle around the triangle nCameras = 10; height = 10; r = 30; -poses = {}; +cameras = {}; K = gtsamCal3_S2(500,500,0,640/2,480/2); for i=1:nCameras theta = (i-1)*2*pi/nCameras; t = gtsamPoint3([r*cos(theta), r*sin(theta), height]'); - camera = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K) - poses{i} = camera.pose(); + cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K); end -odometry = poses{1}.between(poses{2}); +odometry = cameras{1}.pose.between(cameras{2}.pose); poseNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); pointNoise = gtsamSharedNoiseModel_Sigma(3, 0.1); @@ -45,9 +44,9 @@ newFactors = visualSLAMGraph; initialEstimates = visualSLAMValues; for i=1:nCameras - % Prior for the first pose or odometry for subsequent poses + % Prior for the first pose or odometry for subsequent cameras if (i==1) - newFactors.addPosePrior(symbol('x',1), poses{1}, poseNoise); + newFactors.addPosePrior(symbol('x',1), cameras{1}.pose, poseNoise); for j=1:nPoints newFactors.addPointPrior(symbol('l',j), points{j}, pointNoise); end @@ -57,22 +56,21 @@ for i=1:nCameras % Visual measurement factors for j=1:nPoints - camera = gtsamSimpleCamera(K,poses{i}); - zij = camera.project(points{j}); + zij = cameras{i}.project(points{j}); newFactors.addMeasurement(zij, measurementNoise, symbol('x',i), symbol('l',j), K); end % Initial estimates for the new pose. Also initialize points while in % the first frame. if (i==1) - initialEstimates.insertPose(symbol('x',i), poses{i}); + initialEstimates.insertPose(symbol('x',i), cameras{i}.pose); for j=1:size(points,2) initialEstimates.insertPoint(symbol('l',j), points{j}); end else %TODO: this might be suboptimal since "result" is not the fully %optimized result - if (i==2), prevPose = poses{1}; + if (i==2), prevPose = cameras{1}.pose; else, prevPose = result.pose(symbol('x',i-1)); end initialEstimates.insertPose(symbol('x',i), prevPose.compose(odometry)); end diff --git a/examples/matlab/VisualSLAMExample.m b/examples/matlab/VisualSLAMExample.m index 54ccd2b14..f6ef2063b 100644 --- a/examples/matlab/VisualSLAMExample.m +++ b/examples/matlab/VisualSLAMExample.m @@ -27,17 +27,16 @@ points = {gtsamPoint3([10 10 10]'),... gtsamPoint3([-10 -10 -10]'),... gtsamPoint3([10 -10 -10]')}; -% Camera poses on a circle around the cube, pointing at the world origin +% Camera cameras on a circle around the cube, pointing at the world origin nCameras = 6; height = 0; r = 30; -poses = {}; +cameras = {}; K = gtsamCal3_S2(500,500,0,640/2,480/2); for i=1:nCameras theta = (i-1)*2*pi/nCameras; t = gtsamPoint3([r*cos(theta), r*sin(theta), height]'); - camera = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K) - poses{i} = camera.pose(); + cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K); end measurementNoiseSigma = 1.0; @@ -50,26 +49,25 @@ graph = visualSLAMGraph; %% Add factors for all measurements measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma); for i=1:nCameras - camera = gtsamSimpleCamera(K,poses{i}); for j=1:size(points,2) - zij = camera.project(points{j}); % you can add noise here if desired + zij = cameras{i}.project(points{j}); % you can add noise here if desired graph.addMeasurement(zij, measurementNoise, symbol('x',i), symbol('l',j), K); end end %% Add Gaussian priors for a pose and a landmark to constrain the system posePriorNoise = gtsamSharedNoiseModel_Sigmas(poseNoiseSigmas); -graph.addPosePrior(symbol('x',1), poses{1}, posePriorNoise); +graph.addPosePrior(symbol('x',1), cameras{1}.pose, posePriorNoise); pointPriorNoise = gtsamSharedNoiseModel_Sigma(3,pointNoiseSigma); graph.addPointPrior(symbol('l',1), points{1}, pointPriorNoise); %% Print the graph graph.print(sprintf('\nFactor graph:\n')); -%% Initialize to noisy poses and points +%% Initialize to noisy cameras and points initialEstimate = visualSLAMValues; -for i=1:size(poses,2) - initialEstimate.insertPose(symbol('x',i), poses{i}); +for i=1:size(cameras,2) + initialEstimate.insertPose(symbol('x',i), cameras{i}.pose); end for j=1:size(points,2) initialEstimate.insertPoint(symbol('l',j), points{j}); @@ -91,10 +89,11 @@ for j=1:size(points,2) covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P); end -for i=1:size(poses,2) +for i=1:size(cameras,2) P = marginals.marginalCovariance(symbol('x',i)) pose_i = result.pose(symbol('x',i)) plotPose3(pose_i,P,10); end +axis([-35 35 -35 35 -15 15]); axis equal - +view(-37,40) diff --git a/examples/matlab/VisualSLAMExample_triangle.m b/examples/matlab/VisualSLAMExample_triangle.m index dc9d8364c..92d885a86 100644 --- a/examples/matlab/VisualSLAMExample_triangle.m +++ b/examples/matlab/VisualSLAMExample_triangle.m @@ -18,17 +18,16 @@ for j=1:3 points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]'); end -%% Create camera poses on a circle around the triangle +%% Create camera cameras on a circle around the triangle nCameras = 6; height = 10; r = 30; -poses = {}; +cameras = {}; K = gtsamCal3_S2(500,500,0,640/2,480/2); for i=1:nCameras theta = (i-1)*2*pi/nCameras; t = gtsamPoint3([r*cos(theta), r*sin(theta), height]'); - camera = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K) - poses{i} = camera.pose(); + cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K) end %% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph) @@ -38,9 +37,8 @@ graph = visualSLAMGraph; measurementNoiseSigma=1; % in pixels measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma); for i=1:nCameras - camera = gtsamSimpleCamera(K,poses{i}); for j=1:3 - zij = camera.project(points{j}); % you can add noise here if desired + zij = cameras{i}.project(points{j}); % you can add noise here if desired graph.addMeasurement(zij, measurementNoise, symbol('x',i), symbol('l',j), K); end end @@ -54,10 +52,10 @@ end %% Print the graph graph.print(sprintf('\nFactor graph:\n')); -%% Initialize to noisy poses and points +%% Initialize to noisy cameras and points initialEstimate = visualSLAMValues; -for i=1:size(poses,2) - initialEstimate.insertPose(symbol('x',i), poses{i}); +for i=1:size(cameras,2) + initialEstimate.insertPose(symbol('x',i), cameras{i}.pose); end for j=1:size(points,2) initialEstimate.insertPoint(symbol('l',j), points{j}); @@ -79,10 +77,11 @@ for j=1:size(points,2) covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P); end -for i=1:size(poses,2) +for i=1:size(cameras,2) P = marginals.marginalCovariance(symbol('x',i)) pose_i = result.pose(symbol('x',i)) plotPose3(pose_i,P,10); end +axis([-20 20 -20 20 -1 15]); axis equal - +view(-37,40)