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

Corrected order in sigma vector for Pose3SLAM matlab examples

release/4.3a0
Richard Roberts 2013-03-06 22:07:13 +00:00
parent dd685f0f52
commit 499a58359c
2 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
matlab/gtsam_examples/Pose3SLAMExample.m
View File

@ -21,7 +21,7 @@ p1 = hexagon.at(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([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(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));

2
matlab/gtsam_examples/Pose3SLAMExample_graph.m
View File

@ -21,7 +21,7 @@ dataset = 'sphere2500.txt';
datafile = findExampleDataFile(dataset); datafile = findExampleDataFile(dataset);
%% Initialize graph, initial estimate, and odometry noise %% 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); [graph,initial]=load3D(datafile,model,true,N);
%% Plot Initial Estimate %% Plot Initial Estimate