diff --git a/examples/matlab/VisualISAMExample.m b/examples/matlab/VisualISAMExample.m index e16ffee6a..bbe67bd6b 100644 --- a/examples/matlab/VisualISAMExample.m +++ b/examples/matlab/VisualISAMExample.m @@ -24,10 +24,11 @@ nCameras = 10; height = 10; r = 30; poses = {}; +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]'), gtsamCal3_S2()) + camera = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K) poses{i} = camera.pose(); end odometry = poses{1}.between(poses{2}); @@ -35,7 +36,6 @@ odometry = poses{1}.between(poses{2}); poseNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); pointNoise = gtsamSharedNoiseModel_Sigma(3, 0.1); measurementNoise = gtsamSharedNoiseModel_Sigma(2, 1.0); -K = gtsamCal3_S2(50,50,0,50,50); %% Create an ISAM object for inference isam = visualSLAMISAM; diff --git a/examples/matlab/VisualSLAMExample.m b/examples/matlab/VisualSLAMExample.m index 99d1a39a7..54ccd2b14 100644 --- a/examples/matlab/VisualSLAMExample.m +++ b/examples/matlab/VisualSLAMExample.m @@ -32,10 +32,11 @@ nCameras = 6; height = 0; r = 30; poses = {}; +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]'), gtsamCal3_S2()) + camera = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K) poses{i} = camera.pose(); end @@ -48,7 +49,6 @@ graph = visualSLAMGraph; %% Add factors for all measurements measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma); -K = gtsamCal3_S2(500,500,0,640/2,480/2); for i=1:nCameras camera = gtsamSimpleCamera(K,poses{i}); for j=1:size(points,2) diff --git a/examples/matlab/VisualSLAMExample_triangle.m b/examples/matlab/VisualSLAMExample_triangle.m index 4fc1db320..dc9d8364c 100644 --- a/examples/matlab/VisualSLAMExample_triangle.m +++ b/examples/matlab/VisualSLAMExample_triangle.m @@ -23,10 +23,11 @@ nCameras = 6; height = 10; r = 30; poses = {}; +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]'), gtsamCal3_S2()) + camera = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K) poses{i} = camera.pose(); end @@ -34,7 +35,6 @@ end graph = visualSLAMGraph; %% Add factors for all measurements -K = gtsamCal3_S2(500,500,0,640/2,480/2); measurementNoiseSigma=1; % in pixels measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma); for i=1:nCameras