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

update all the matlab tests to add missing arguments

release/4.3a0
Varun Agrawal 2021-12-06 11:04:37 -05:00
parent 04c2f80877
commit b5bf0ca537
11 changed files with 28 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
matlab/+gtsam/VisualISAMInitialize.m
View File

@ -12,11 +12,11 @@ end
isam = ISAM2(params); isam = ISAM2(params);
%% Set Noise parameters %% Set Noise parameters
noiseModels.pose = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); noiseModels.pose = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]', true);
%noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); %noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.05 0.05 0.2 0.2 0.2]'); noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.05 0.05 0.2 0.2 0.2]', true);
noiseModels.point = noiseModel.Isotropic.Sigma(3, 0.1); noiseModels.point = noiseModel.Isotropic.Sigma(3, 0.1, true);
noiseModels.measurement = noiseModel.Isotropic.Sigma(2, 1.0); noiseModels.measurement = noiseModel.Isotropic.Sigma(2, 1.0, true);
%% Add constraints/priors %% Add constraints/priors
% TODO: should not be from ground truth! % TODO: should not be from ground truth!

4
matlab/gtsam_tests/testJacobianFactor.m
View File

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

4
matlab/gtsam_tests/testKalmanFilter.m
View File

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

4
matlab/gtsam_tests/testLocalizationExample.m
View File

@ -17,7 +17,7 @@ graph = NonlinearFactorGraph;
%% Add two odometry factors %% Add two odometry factors
odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true); % 20cm std on x,y, 0.1 rad on theta
graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise));
graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise));
@ -27,7 +27,7 @@ groundTruth = Values;
groundTruth.insert(1, Pose2(0.0, 0.0, 0.0)); groundTruth.insert(1, Pose2(0.0, 0.0, 0.0));
groundTruth.insert(2, Pose2(2.0, 0.0, 0.0)); groundTruth.insert(2, Pose2(2.0, 0.0, 0.0));
groundTruth.insert(3, Pose2(4.0, 0.0, 0.0)); groundTruth.insert(3, Pose2(4.0, 0.0, 0.0));
model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]); model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10], true);
for i=1:3 for i=1:3
graph.add(PriorFactorPose2(i, groundTruth.atPose2(i), model)); graph.add(PriorFactorPose2(i, groundTruth.atPose2(i), model));
end end

4
matlab/gtsam_tests/testOdometryExample.m
View File

@ -17,12 +17,12 @@ graph = NonlinearFactorGraph;
%% Add a Gaussian prior on pose x_1 %% Add a Gaussian prior on pose x_1
priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true); % 30cm std on x,y, 0.1 rad on theta
graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph
%% Add two odometry factors %% Add two odometry factors
odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true); % 20cm std on x,y, 0.1 rad on theta
graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise));
graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise));

6
matlab/gtsam_tests/testPlanarSLAMExample.m
View File

@ -30,18 +30,18 @@ graph = NonlinearFactorGraph;
%% Add prior %% Add prior
priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true);
graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph
%% Add odometry %% Add odometry
odometry = Pose2(2.0, 0.0, 0.0); odometry = Pose2(2.0, 0.0, 0.0);
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true);
graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
%% Add bearing/range measurement factors %% Add bearing/range measurement factors
degrees = pi/180; degrees = pi/180;
brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2], true);
graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise));
graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise)); graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise)); graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));

6
matlab/gtsam_tests/testPose2SLAMExample.m
View File

@ -26,19 +26,19 @@ graph = NonlinearFactorGraph;
%% Add prior %% Add prior
% gaussian for prior % gaussian for prior
priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true);
graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph
%% Add odometry %% Add odometry
% general noisemodel for odometry % general noisemodel for odometry
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true);
graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise)); graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise));
graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise)); graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise));
graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise)); graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise));
graph.add(BetweenFactorPose2(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise)); graph.add(BetweenFactorPose2(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise));
%% Add pose constraint %% Add pose constraint
model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true);
graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model)); graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model));
%% Initialize to noisy points %% Initialize to noisy points

2
matlab/gtsam_tests/testPose3SLAMExample.m
View File

@ -21,7 +21,7 @@ p1 = hexagon.atPose3(1);
fg = NonlinearFactorGraph; fg = NonlinearFactorGraph;
fg.add(NonlinearEqualityPose3(0, p0)); fg.add(NonlinearEqualityPose3(0, p0));
delta = p0.between(p1); delta = p0.between(p1);
covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180], true);
fg.add(BetweenFactorPose3(0,1, delta, covariance)); fg.add(BetweenFactorPose3(0,1, delta, covariance));
fg.add(BetweenFactorPose3(1,2, delta, covariance)); fg.add(BetweenFactorPose3(1,2, delta, covariance));
fg.add(BetweenFactorPose3(2,3, delta, covariance)); fg.add(BetweenFactorPose3(2,3, delta, covariance));

6
matlab/gtsam_tests/testSFMExample.m
View File

@ -25,7 +25,7 @@ poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
graph = NonlinearFactorGraph; graph = NonlinearFactorGraph;
%% Add factors for all measurements %% Add factors for all measurements
measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma); measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma, true);
for i=1:length(data.Z) for i=1:length(data.Z)
for k=1:length(data.Z{i}) for k=1:length(data.Z{i})
j = data.J{i}{k}; j = data.J{i}{k};
@ -33,9 +33,9 @@ for i=1:length(data.Z)
end end
end end
posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas, true);
graph.add(PriorFactorPose3(symbol('x',1), truth.cameras{1}.pose, posePriorNoise)); graph.add(PriorFactorPose3(symbol('x',1), truth.cameras{1}.pose, posePriorNoise));
pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma, true);
graph.add(PriorFactorPoint3(symbol('p',1), truth.points{1}, pointPriorNoise)); graph.add(PriorFactorPoint3(symbol('p',1), truth.points{1}, pointPriorNoise));
%% Initial estimate %% Initial estimate

10
matlab/gtsam_tests/testSerialization.m
View File

@ -45,30 +45,30 @@ graph = NonlinearFactorGraph;
% Prior factor % Prior factor
priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true);
graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph
% Between Factors % Between Factors
odometry = Pose2(2.0, 0.0, 0.0); odometry = Pose2(2.0, 0.0, 0.0);
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true);
graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
% Range Factors % Range Factors
rNoise = noiseModel.Diagonal.Sigmas([0.2]); rNoise = noiseModel.Diagonal.Sigmas([0.2], true);
graph.add(RangeFactor2D(i1, j1, sqrt(4+4), rNoise)); graph.add(RangeFactor2D(i1, j1, sqrt(4+4), rNoise));
graph.add(RangeFactor2D(i2, j1, 2, rNoise)); graph.add(RangeFactor2D(i2, j1, 2, rNoise));
graph.add(RangeFactor2D(i3, j2, 2, rNoise)); graph.add(RangeFactor2D(i3, j2, 2, rNoise));
% Bearing Factors % Bearing Factors
degrees = pi/180; degrees = pi/180;
bNoise = noiseModel.Diagonal.Sigmas([0.1]); bNoise = noiseModel.Diagonal.Sigmas([0.1], true);
graph.add(BearingFactor2D(i1, j1, Rot2(45*degrees), bNoise)); graph.add(BearingFactor2D(i1, j1, Rot2(45*degrees), bNoise));
graph.add(BearingFactor2D(i2, j1, Rot2(90*degrees), bNoise)); graph.add(BearingFactor2D(i2, j1, Rot2(90*degrees), bNoise));
graph.add(BearingFactor2D(i3, j2, Rot2(90*degrees), bNoise)); graph.add(BearingFactor2D(i3, j2, Rot2(90*degrees), bNoise));
% BearingRange Factors % BearingRange Factors
brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2], true);
graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise));
graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise)); graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise)); graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));

2
matlab/gtsam_tests/testStereoVOExample.m
View File

@ -33,7 +33,7 @@ graph.add(NonlinearEqualityPose3(x1, first_pose));
%% Create realistic calibration and measurement noise model %% Create realistic calibration and measurement noise model
% format: fx fy skew cx cy baseline % format: fx fy skew cx cy baseline
K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2); K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]); stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0], true);
%% Add measurements %% Add measurements
% pose 1 % pose 1