From ee51dfd68bf0fe3c05dee1d0a29f6c1ce7eb5590 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 9 Jul 2012 20:04:06 +0000 Subject: [PATCH] Fixing usage of globals in matlab examples/tests. Currently, tests fail due to handling of noisemodel --- matlab/VisualISAMGenerateData.m | 2 +- matlab/VisualISAMInitialize.m | 8 ++++---- matlab/examples/LocalizationExample.m | 4 ++-- matlab/examples/OdometryExample.m | 4 ++-- matlab/examples/PlanarSLAMExample.m | 6 +++--- matlab/examples/PlanarSLAMExample_sampling.m | 8 ++++---- matlab/examples/Pose2SLAMExample.m | 6 +++--- matlab/examples/Pose2SLAMExample_advanced.m | 6 +++--- matlab/examples/Pose2SLAMExample_circle.m | 4 ++-- matlab/examples/Pose2SLAMExample_graph.m | 4 ++-- matlab/examples/Pose2SLAMwSPCG.m | 6 +++--- matlab/examples/Pose3SLAMExample.m | 4 ++-- matlab/examples/Pose3SLAMExample_graph.m | 2 +- matlab/examples/SBAExample.m | 6 +++--- matlab/examples/SFMExample.m | 6 +++--- matlab/examples/StereoVOExample.m | 2 +- matlab/examples/StereoVOExample_large.m | 2 +- matlab/load3D.m | 4 ++-- matlab/tests/testJacobianFactor.m | 4 ++-- matlab/tests/testKalmanFilter.m | 4 ++-- matlab/tests/testLocalizationExample.m | 4 ++-- matlab/tests/testOdometryExample.m | 4 ++-- matlab/tests/testPlanarSLAMExample.m | 6 +++--- matlab/tests/testPose2SLAMExample.m | 6 +++--- matlab/tests/testPose3SLAMExample.m | 4 ++-- matlab/tests/testSFMExample.m | 6 +++--- matlab/tests/testStereoVOExample.m | 2 +- 27 files changed, 62 insertions(+), 62 deletions(-) diff --git a/matlab/VisualISAMGenerateData.m b/matlab/VisualISAMGenerateData.m index 3c55417d9..1c71185e2 100644 --- a/matlab/VisualISAMGenerateData.m +++ b/matlab/VisualISAMGenerateData.m @@ -29,7 +29,7 @@ data.K = truth.K; for i=1:options.nrCameras theta = (i-1)*2*pi/options.nrCameras; t = gtsamPoint3([r*cos(theta), r*sin(theta), height]'); - truth.cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), truth.K); + truth.cameras{i} = gtsamSimpleCamera.Lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), truth.K); % Create measurements for j=1:nrPoints % All landmarks seen in every frame diff --git a/matlab/VisualISAMInitialize.m b/matlab/VisualISAMInitialize.m index 139d40e65..b2b960a7b 100644 --- a/matlab/VisualISAMInitialize.m +++ b/matlab/VisualISAMInitialize.m @@ -6,10 +6,10 @@ function [noiseModels,isam,result] = VisualISAMInitialize(data,truth,options) isam = visualSLAMISAM(options.reorderInterval); %% Set Noise parameters -noiseModels.pose = gtsamnoiseModelDiagonal_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); -noiseModels.odometry = gtsamnoiseModelDiagonal_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); -noiseModels.point = gtsamnoiseModelIsotropic_Sigma(3, 0.1); -noiseModels.measurement = gtsamnoiseModelIsotropic_Sigma(2, 1.0); +noiseModels.pose = gtsamnoiseModelDiagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); +noiseModels.odometry = gtsamnoiseModelDiagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); +noiseModels.point = gtsamnoiseModelIsotropic.Sigma(3, 0.1); +noiseModels.measurement = gtsamnoiseModelIsotropic.Sigma(2, 1.0); %% Add constraints/priors % TODO: should not be from ground truth! diff --git a/matlab/examples/LocalizationExample.m b/matlab/examples/LocalizationExample.m index d3e563dd4..3f48bb8e4 100644 --- a/matlab/examples/LocalizationExample.m +++ b/matlab/examples/LocalizationExample.m @@ -20,13 +20,13 @@ graph = pose2SLAMGraph; %% Add two odometry factors odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) -odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta +odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta graph.addRelativePose(1, 2, odometry, odometryNoise); graph.addRelativePose(2, 3, odometry, odometryNoise); %% Add three "GPS" measurements % We use Pose2 Priors here with high variance on theta -noiseModel = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.1; 10]); +noiseModel = gtsamnoiseModelDiagonal.Sigmas([0.1; 0.1; 10]); graph.addPosePrior(1, gtsamPose2(0.0, 0.0, 0.0), noiseModel); graph.addPosePrior(2, gtsamPose2(2.0, 0.0, 0.0), noiseModel); graph.addPosePrior(3, gtsamPose2(4.0, 0.0, 0.0), noiseModel); diff --git a/matlab/examples/OdometryExample.m b/matlab/examples/OdometryExample.m index b3cbff6a2..e90550152 100644 --- a/matlab/examples/OdometryExample.m +++ b/matlab/examples/OdometryExample.m @@ -20,12 +20,12 @@ graph = pose2SLAMGraph; %% Add a Gaussian prior on pose x_1 priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin -priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta +priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph %% Add two odometry factors odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) -odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta +odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta graph.addRelativePose(1, 2, odometry, odometryNoise); graph.addRelativePose(2, 3, odometry, odometryNoise); diff --git a/matlab/examples/PlanarSLAMExample.m b/matlab/examples/PlanarSLAMExample.m index 499845eda..7557bd0ba 100644 --- a/matlab/examples/PlanarSLAMExample.m +++ b/matlab/examples/PlanarSLAMExample.m @@ -28,18 +28,18 @@ graph = planarSLAMGraph; %% Add prior priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin -priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]); +priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]); graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph %% Add odometry odometry = gtsamPose2(2.0, 0.0, 0.0); -odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); +odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); graph.addRelativePose(i1, i2, odometry, odometryNoise); graph.addRelativePose(i2, i3, odometry, odometryNoise); %% Add bearing/range measurement factors degrees = pi/180; -noiseModel = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.2]); +noiseModel = gtsamnoiseModelDiagonal.Sigmas([0.1; 0.2]); graph.addBearingRange(i1, j1, gtsamRot2(45*degrees), sqrt(4+4), noiseModel); graph.addBearingRange(i2, j1, gtsamRot2(90*degrees), 2, noiseModel); graph.addBearingRange(i3, j2, gtsamRot2(90*degrees), 2, noiseModel); diff --git a/matlab/examples/PlanarSLAMExample_sampling.m b/matlab/examples/PlanarSLAMExample_sampling.m index 861e56db9..560ab7ace 100644 --- a/matlab/examples/PlanarSLAMExample_sampling.m +++ b/matlab/examples/PlanarSLAMExample_sampling.m @@ -15,22 +15,22 @@ i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3); graph = planarSLAMGraph; priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin -priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]); +priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]); graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph odometry = gtsamPose2(2.0, 0.0, 0.0); -odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); +odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); graph.addRelativePose(i1, i2, odometry, odometryNoise); graph.addRelativePose(i2, i3, odometry, odometryNoise); %% Except, for measurements we offer a choice j1 = symbol('l',1); j2 = symbol('l',2); degrees = pi/180; -noiseModel = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.2]); +noiseModel = gtsamnoiseModelDiagonal.Sigmas([0.1; 0.2]); if 1 graph.addBearingRange(i1, j1, gtsamRot2(45*degrees), sqrt(4+4), noiseModel); graph.addBearingRange(i2, j1, gtsamRot2(90*degrees), 2, noiseModel); else - bearingModel = gtsamnoiseModelDiagonal_Sigmas(0.1); + bearingModel = gtsamnoiseModelDiagonal.Sigmas(0.1); graph.addBearing(i1, j1, gtsamRot2(45*degrees), bearingModel); graph.addBearing(i2, j1, gtsamRot2(90*degrees), bearingModel); end diff --git a/matlab/examples/Pose2SLAMExample.m b/matlab/examples/Pose2SLAMExample.m index 776260e3f..9cb5b8af4 100644 --- a/matlab/examples/Pose2SLAMExample.m +++ b/matlab/examples/Pose2SLAMExample.m @@ -24,19 +24,19 @@ graph = pose2SLAMGraph; %% Add prior % gaussian for prior priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin -priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]); +priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]); graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph %% Add odometry % general noisemodel for odometry -odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); +odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); graph.addRelativePose(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise); graph.addRelativePose(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); graph.addRelativePose(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); graph.addRelativePose(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); %% Add pose constraint -model = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); +model = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); graph.addRelativePose(5, 2, gtsamPose2(2.0, 0.0, pi/2), model); % print diff --git a/matlab/examples/Pose2SLAMExample_advanced.m b/matlab/examples/Pose2SLAMExample_advanced.m index 843ae7e1c..45d288e50 100644 --- a/matlab/examples/Pose2SLAMExample_advanced.m +++ b/matlab/examples/Pose2SLAMExample_advanced.m @@ -25,20 +25,20 @@ graph = pose2SLAMGraph; %% Add prior % gaussian for prior -priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]); +priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]); priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph %% Add odometry % general noisemodel for odometry -odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); +odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) graph.addRelativePose(1, 2, odometry, odometryNoise); graph.addRelativePose(2, 3, odometry, odometryNoise); %% Add measurements % general noisemodel for measurements -measurementNoise = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.2]); +measurementNoise = gtsamnoiseModelDiagonal.Sigmas([0.1; 0.2]); % print graph.print('full graph'); diff --git a/matlab/examples/Pose2SLAMExample_circle.m b/matlab/examples/Pose2SLAMExample_circle.m index e05a77186..c089f5949 100644 --- a/matlab/examples/Pose2SLAMExample_circle.m +++ b/matlab/examples/Pose2SLAMExample_circle.m @@ -11,7 +11,7 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Create a hexagon of poses -hexagon = pose2SLAMValues_Circle(6,1.0); +hexagon = pose2SLAMValues.Circle(6,1.0); p0 = hexagon.pose(0); p1 = hexagon.pose(1); @@ -19,7 +19,7 @@ p1 = hexagon.pose(1); fg = pose2SLAMGraph; fg.addPoseConstraint(0, p0); delta = p0.between(p1); -covariance = gtsamnoiseModelDiagonal_Sigmas([0.05; 0.05; 5*pi/180]); +covariance = gtsamnoiseModelDiagonal.Sigmas([0.05; 0.05; 5*pi/180]); fg.addRelativePose(0,1, delta, covariance); fg.addRelativePose(1,2, delta, covariance); fg.addRelativePose(2,3, delta, covariance); diff --git a/matlab/examples/Pose2SLAMExample_graph.m b/matlab/examples/Pose2SLAMExample_graph.m index a2645a593..258730aaa 100644 --- a/matlab/examples/Pose2SLAMExample_graph.m +++ b/matlab/examples/Pose2SLAMExample_graph.m @@ -11,13 +11,13 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Initialize graph, initial estimate, and odometry noise -model = gtsamnoiseModelDiagonal_Sigmas([0.05; 0.05; 5*pi/180]); +model = gtsamnoiseModelDiagonal.Sigmas([0.05; 0.05; 5*pi/180]); [graph,initial]=load2D('../../examples/Data/w100-odom.graph',model); initial.print(sprintf('Initial estimate:\n')); %% Add a Gaussian prior on pose x_1 priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin -priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.01; 0.01; 0.01]); +priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.01; 0.01; 0.01]); graph.addPosePrior(0, priorMean, priorNoise); % add directly to graph %% Plot Initial Estimate diff --git a/matlab/examples/Pose2SLAMwSPCG.m b/matlab/examples/Pose2SLAMwSPCG.m index 3d0ce8da7..3e773689b 100644 --- a/matlab/examples/Pose2SLAMwSPCG.m +++ b/matlab/examples/Pose2SLAMwSPCG.m @@ -22,19 +22,19 @@ graph = pose2SLAMGraph; %% Add prior % gaussian for prior priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin -priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]); +priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]); graph.addPrior(1, priorMean, priorNoise); % add directly to graph %% Add odometry % general noisemodel for odometry -odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); +odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); graph.addOdometry(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise); graph.addOdometry(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); graph.addOdometry(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); %% Add pose constraint -model = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); +model = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); graph.addRelativePose(5, 2, gtsamPose2(2.0, 0.0, pi/2), model); % print diff --git a/matlab/examples/Pose3SLAMExample.m b/matlab/examples/Pose3SLAMExample.m index c335203eb..c1328585e 100644 --- a/matlab/examples/Pose3SLAMExample.m +++ b/matlab/examples/Pose3SLAMExample.m @@ -11,7 +11,7 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Create a hexagon of poses -hexagon = pose3SLAMValues_Circle(6,1.0); +hexagon = pose3SLAMValues.Circle(6,1.0); p0 = hexagon.pose(0); p1 = hexagon.pose(1); @@ -19,7 +19,7 @@ p1 = hexagon.pose(1); fg = pose3SLAMGraph; fg.addPoseConstraint(0, p0); delta = p0.between(p1); -covariance = gtsamnoiseModelDiagonal_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); +covariance = gtsamnoiseModelDiagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); fg.addRelativePose(0,1, delta, covariance); fg.addRelativePose(1,2, delta, covariance); fg.addRelativePose(2,3, delta, covariance); diff --git a/matlab/examples/Pose3SLAMExample_graph.m b/matlab/examples/Pose3SLAMExample_graph.m index f2c411d08..f39fc306b 100644 --- a/matlab/examples/Pose3SLAMExample_graph.m +++ b/matlab/examples/Pose3SLAMExample_graph.m @@ -16,7 +16,7 @@ N = 2500; filename = '../../examples/Data/sphere2500.txt'; %% Initialize graph, initial estimate, and odometry noise -model = gtsamnoiseModelDiagonal_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); +model = gtsamnoiseModelDiagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); [graph,initial]=load3D(filename,model,true,N); %% Plot Initial Estimate diff --git a/matlab/examples/SBAExample.m b/matlab/examples/SBAExample.m index 9050d011d..82fdd4646 100644 --- a/matlab/examples/SBAExample.m +++ b/matlab/examples/SBAExample.m @@ -34,7 +34,7 @@ graph = sparseBAGraph; %% Add factors for all measurements -measurementNoise = gtsamnoiseModelIsotropic_Sigma(2,measurementNoiseSigma); +measurementNoise = gtsamnoiseModelIsotropic.Sigma(2,measurementNoiseSigma); for i=1:length(data.Z) for k=1:length(data.Z{i}) j = data.J{i}{k}; @@ -43,11 +43,11 @@ for i=1:length(data.Z) end %% Add Gaussian priors for a pose and a landmark to constrain the system -cameraPriorNoise = gtsamnoiseModelDiagonal_Sigmas(cameraNoiseSigmas); +cameraPriorNoise = gtsamnoiseModelDiagonal.Sigmas(cameraNoiseSigmas); firstCamera = gtsamSimpleCamera(truth.cameras{1}.pose, truth.K); graph.addSimpleCameraPrior(symbol('c',1), firstCamera, cameraPriorNoise); -pointPriorNoise = gtsamnoiseModelIsotropic_Sigma(3,pointNoiseSigma); +pointPriorNoise = gtsamnoiseModelIsotropic.Sigma(3,pointNoiseSigma); graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise); %% Print the graph diff --git a/matlab/examples/SFMExample.m b/matlab/examples/SFMExample.m index 277f31648..3eef9f916 100644 --- a/matlab/examples/SFMExample.m +++ b/matlab/examples/SFMExample.m @@ -32,7 +32,7 @@ poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; graph = visualSLAMGraph; %% Add factors for all measurements -measurementNoise = gtsamnoiseModelIsotropic_Sigma(2,measurementNoiseSigma); +measurementNoise = gtsamnoiseModelIsotropic.Sigma(2,measurementNoiseSigma); for i=1:length(data.Z) for k=1:length(data.Z{i}) j = data.J{i}{k}; @@ -41,9 +41,9 @@ for i=1:length(data.Z) end %% Add Gaussian priors for a pose and a landmark to constrain the system -posePriorNoise = gtsamnoiseModelDiagonal_Sigmas(poseNoiseSigmas); +posePriorNoise = gtsamnoiseModelDiagonal.Sigmas(poseNoiseSigmas); graph.addPosePrior(symbol('x',1), truth.cameras{1}.pose, posePriorNoise); -pointPriorNoise = gtsamnoiseModelIsotropic_Sigma(3,pointNoiseSigma); +pointPriorNoise = gtsamnoiseModelIsotropic.Sigma(3,pointNoiseSigma); graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise); %% Print the graph diff --git a/matlab/examples/StereoVOExample.m b/matlab/examples/StereoVOExample.m index 99adaa82b..9cf838e94 100644 --- a/matlab/examples/StereoVOExample.m +++ b/matlab/examples/StereoVOExample.m @@ -31,7 +31,7 @@ graph.addPoseConstraint(x1, first_pose); %% Create realistic calibration and measurement noise model % format: fx fy skew cx cy baseline K = gtsamCal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2); -stereo_model = gtsamnoiseModelDiagonal_Sigmas([1.0; 1.0; 1.0]); +stereo_model = gtsamnoiseModelDiagonal.Sigmas([1.0; 1.0; 1.0]); %% Add measurements % pose 1 diff --git a/matlab/examples/StereoVOExample_large.m b/matlab/examples/StereoVOExample_large.m index ef1211e2a..d57628fa1 100644 --- a/matlab/examples/StereoVOExample_large.m +++ b/matlab/examples/StereoVOExample_large.m @@ -14,7 +14,7 @@ % format: fx fy skew cx cy baseline calib = dlmread('../../examples/Data/VO_calibration.txt'); K = gtsamCal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6)); -stereo_model = gtsamnoiseModelDiagonal_Sigmas([1.0; 1.0; 1.0]); +stereo_model = gtsamnoiseModelDiagonal.Sigmas([1.0; 1.0; 1.0]); %% create empty graph and values graph = visualSLAMGraph; diff --git a/matlab/load3D.m b/matlab/load3D.m index 6ba861a92..b457ea1af 100644 --- a/matlab/load3D.m +++ b/matlab/load3D.m @@ -29,7 +29,7 @@ for i=1:n i=v{2}; if (~successive & i