From 0472b14ebf5abd3497357865f685345432530748 Mon Sep 17 00:00:00 2001 From: yao Date: Wed, 15 Jun 2016 14:26:28 -0400 Subject: [PATCH] Updated two code files --- doc/Code/PlanarSLAMExample.m | 8 ++++---- doc/Code/Pose3SLAMExample-graph.m | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) 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;