diff --git a/matlab/gtsam_examples/Pose3SLAMExample.m b/matlab/gtsam_examples/Pose3SLAMExample.m index 0d2bd237f..1d9c3b579 100644 --- a/matlab/gtsam_examples/Pose3SLAMExample.m +++ b/matlab/gtsam_examples/Pose3SLAMExample.m @@ -21,7 +21,7 @@ p1 = hexagon.at(1); fg = NonlinearFactorGraph; fg.add(NonlinearEqualityPose3(0, p0)); 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([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]); fg.add(BetweenFactorPose3(0,1, delta, covariance)); fg.add(BetweenFactorPose3(1,2, delta, covariance)); fg.add(BetweenFactorPose3(2,3, delta, covariance)); diff --git a/matlab/gtsam_examples/Pose3SLAMExample_graph.m b/matlab/gtsam_examples/Pose3SLAMExample_graph.m index f4eb8dd3a..01df4fc33 100644 --- a/matlab/gtsam_examples/Pose3SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose3SLAMExample_graph.m @@ -21,7 +21,7 @@ dataset = 'sphere2500.txt'; datafile = findExampleDataFile(dataset); %% Initialize graph, initial estimate, and odometry noise -model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); +model = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]); [graph,initial]=load3D(datafile,model,true,N); %% Plot Initial Estimate