12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixing usage of globals in matlab examples/tests. Currently, tests fail due to handling of noisemodel

release/4.3a0
Alex Cunningham 2012-07-09 20:04:06 +00:00
parent 280bbbb54e
commit ee51dfd68b
27 changed files with 62 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
matlab/VisualISAMGenerateData.m
View File

@ -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

8
matlab/VisualISAMInitialize.m
View File

@ -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!

4
matlab/examples/LocalizationExample.m
View File

@ -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);

4
matlab/examples/OdometryExample.m
View File

@ -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);

6
matlab/examples/PlanarSLAMExample.m
View File

@ -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);

8
matlab/examples/PlanarSLAMExample_sampling.m
View File

@ -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

6
matlab/examples/Pose2SLAMExample.m
View File

@ -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

6
matlab/examples/Pose2SLAMExample_advanced.m
View File

@ -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');

4
matlab/examples/Pose2SLAMExample_circle.m
View File

@ -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);

4
matlab/examples/Pose2SLAMExample_graph.m
View File

@ -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

6
matlab/examples/Pose2SLAMwSPCG.m
View File

@ -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

4
matlab/examples/Pose3SLAMExample.m
View File

@ -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);

2
matlab/examples/Pose3SLAMExample_graph.m
View File

@ -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

6
matlab/examples/SBAExample.m
View File

@ -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

6
matlab/examples/SFMExample.m
View File

@ -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

2
matlab/examples/StereoVOExample.m
View File

@ -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

2
matlab/examples/StereoVOExample_large.m
View File

@ -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;

4
matlab/load3D.m
View File

@ -29,7 +29,7 @@ for i=1:n
i=v{2};
if (~successive & i<N | successive & i==0)
t = gtsamPoint3(v{3}, v{4}, v{5});
R = gtsamRot3_ypr(v{8}, -v{7}, v{6});
R = gtsamRot3.Ypr(v{8}, -v{7}, v{6});
initial.insertPose(i, gtsamPose3(R,t));
end
elseif strcmp('EDGE3',line_i(1:5))
@ -39,7 +39,7 @@ for i=1:n
if i1<N & i2<N
if ~successive | abs(i2-i1)==1
t = gtsamPoint3(e{4}, e{5}, e{6});
R = gtsamRot3_ypr(e{9}, e{8}, e{7});
R = gtsamRot3.Ypr(e{9}, e{8}, e{7});
dpose = gtsamPose3(R,t);
graph.addRelativePose(i1, i2, dpose, model);
if successive

4
matlab/tests/testJacobianFactor.m
View File

@ -30,7 +30,7 @@ x1 = 3;
% the RHS
b2=[-1;1.5;2;-1];
sigmas = [1;1;1;1];
model4 = gtsamnoiseModelDiagonal_Sigmas(sigmas);
model4 = gtsamnoiseModelDiagonal.Sigmas(sigmas);
combined = gtsamJacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4);
% eliminate the first variable (x2) in the combined factor, destructive !
@ -69,7 +69,7 @@ Bx1 = [
% the RHS
b1= [0.0;0.894427];
model2 = gtsamnoiseModelDiagonal_Sigmas([1;1]);
model2 = gtsamnoiseModelDiagonal.Sigmas([1;1]);
expectedLF = gtsamJacobianFactor(l1, Bl1, x1, Bx1, b1, model2);
% check if the result matches the combined (reduced) factor

4
matlab/tests/testKalmanFilter.m
View File

@ -22,13 +22,13 @@
F = eye(2,2);
B = eye(2,2);
u = [1.0; 0.0];
modelQ = gtsamnoiseModelDiagonal_Sigmas([0.1;0.1]);
modelQ = gtsamnoiseModelDiagonal.Sigmas([0.1;0.1]);
Q = 0.01*eye(2,2);
H = eye(2,2);
z1 = [1.0, 0.0]';
z2 = [2.0, 0.0]';
z3 = [3.0, 0.0]';
modelR = gtsamnoiseModelDiagonal_Sigmas([0.1;0.1]);
modelR = gtsamnoiseModelDiagonal.Sigmas([0.1;0.1]);
R = 0.01*eye(2,2);
%% Create the set of expected output TestValues

4
matlab/tests/testLocalizationExample.m
View File

@ -15,7 +15,7 @@ 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);
@ -25,7 +25,7 @@ groundTruth = pose2SLAMValues;
groundTruth.insertPose(1, gtsamPose2(0.0, 0.0, 0.0));
groundTruth.insertPose(2, gtsamPose2(2.0, 0.0, 0.0));
groundTruth.insertPose(3, gtsamPose2(4.0, 0.0, 0.0));
noiseModel = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.1; 10]);
noiseModel = gtsamnoiseModelDiagonal.Sigmas([0.1; 0.1; 10]);
for i=1:3
graph.addPosePrior(i, groundTruth.pose(i), noiseModel);
end

4
matlab/tests/testOdometryExample.m
View File

@ -15,12 +15,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);

6
matlab/tests/testPlanarSLAMExample.m
View File

@ -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);

6
matlab/tests/testPose2SLAMExample.m
View File

@ -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);
%% Initialize to noisy points

4
matlab/tests/testPose3SLAMExample.m
View File

@ -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);

6
matlab/tests/testSFMExample.m
View File

@ -23,7 +23,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};
@ -31,9 +31,9 @@ for i=1:length(data.Z)
end
end
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);
%% Initial estimate

2
matlab/tests/testStereoVOExample.m
View File

@ -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