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

Updated two code files

release/4.3a0
yao 2016-06-15 14:26:28 -04:00
parent ed7e9af2a4
commit 0472b14ebf
2 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
doc/Code/PlanarSLAMExample.m
View File

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

6
doc/Code/Pose3SLAMExample-graph.m
View File

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