diff --git a/doc/Code/PlanarSLAMExample.m b/doc/Code/PlanarSLAMExample.m index 0231d52d0..8f63853e7 100644 --- a/doc/Code/PlanarSLAMExample.m +++ b/doc/Code/PlanarSLAMExample.m @@ -19,7 +19,7 @@ graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); % Add bearing/range measurement factors degrees = pi/180; -noiseModel = noiseModel.Diagonal.Sigmas([0.1; 0.2]); -graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(8), noiseModel)); -graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, noiseModel)); -graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, noiseModel)); +brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); +graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(8), brNoise)); +graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise)); +graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise)); diff --git a/doc/Code/Pose3SLAMExample-graph.m b/doc/Code/Pose3SLAMExample-graph.m index dde5ba85e..702928759 100644 --- a/doc/Code/Pose3SLAMExample-graph.m +++ b/doc/Code/Pose3SLAMExample-graph.m @@ -1,12 +1,12 @@ %% Initialize graph, initial estimate, and odometry noise datafile = findExampleDataFile('sphere2500.txt'); -model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.1; 0.1; 0.1]); +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, 2500); -plot3DTrajectory(initial,'g-',false); % Plot Initial Estimate +plot3DTrajectory(initial, 'g-', false); % Plot Initial Estimate %% Read again, now with all constraints, and optimize -graph = load3D(datafile, model); -graph.add(NonlinearEqualityPose3(0, initial.at(0))); +graph = load3D(datafile, model, false, 2500); +graph.add(NonlinearEqualityPose3(0, initial.atPose3(0))); optimizer = LevenbergMarquardtOptimizer(graph, initial); result = optimizer.optimizeSafely(); plot3DTrajectory(result, 'r-', false); axis equal;