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

Switched datasets

release/4.3a0
Frank Dellaert 2013-06-25 17:12:01 +00:00
parent 26df712592
commit e9a8782a51
2 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
matlab/gtsam_examples/RangeISAMExample_plaza.m
View File

@ -27,7 +27,7 @@ import gtsam.*
% Time (sec) X_pose (m) Y_pose (m) % Time (sec) X_pose (m) Y_pose (m)
% TD % TD
% Time (sec) Sender / Antenna ID Receiver Node ID Range (m) % Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
if false % switch between data files if true % switch between data files
datafile = findExampleDataFile('Plaza1_.mat'); datafile = findExampleDataFile('Plaza1_.mat');
headingOffset=0; headingOffset=0;
minK=200; % minimum number of range measurements to process initially minK=200; % minimum number of range measurements to process initially
@ -65,7 +65,7 @@ isam = ISAM2;
%% Add prior on first pose %% Add prior on first pose
pose0 = Pose2(GT(1,2),GT(1,3),headingOffset+GT(1,4)); pose0 = Pose2(GT(1,2),GT(1,3),headingOffset+GT(1,4));
newFactors = NonlinearFactorGraph; newFactors = NonlinearFactorGraph;
if ~addRange | ~useGroundTruth if ~addRange || ~useGroundTruth
newFactors.add(PriorFactorPose2(0,pose0,noiseModels.prior)); newFactors.add(PriorFactorPose2(0,pose0,noiseModels.prior));
end end
initial = Values; initial = Values;

6
matlab/gtsam_examples/RangeSLAMExample_plaza.m
View File

@ -27,7 +27,7 @@ import gtsam.*
% Time (sec) X_pose (m) Y_pose (m) % Time (sec) X_pose (m) Y_pose (m)
% TD % TD
% Time (sec) Sender / Antenna ID Receiver Node ID Range (m) % Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
datafile = findExampleDataFile('Plaza1_.mat'); datafile = findExampleDataFile('Plaza2_.mat');
load(datafile) load(datafile)
M=size(DR,1); M=size(DR,1);
K=size(TD,1); K=size(TD,1);
@ -42,7 +42,7 @@ base = noiseModel.mEstimator.Tukey(5);
noiseModels.range = noiseModel.Robust(base,noiseModel.Isotropic.Sigma(1, sigmaR)); noiseModels.range = noiseModel.Robust(base,noiseModel.Isotropic.Sigma(1, sigmaR));
%% Add prior on first pose %% Add prior on first pose
pose0 = Pose2(GT(1,2),GT(1,3),GT(1,4)); pose0 = Pose2(GT(1,2),GT(1,3),pi+GT(1,4));
graph = NonlinearFactorGraph; graph = NonlinearFactorGraph;
graph.add(PriorFactorPose2(0,pose0,noiseModels.prior)); graph.add(PriorFactorPose2(0,pose0,noiseModels.prior));
initial = Values; initial = Values;
@ -84,7 +84,7 @@ for i=1:M
end end
toc toc
%% GRaph was built, optimize ! %% Graph was built, optimize !
tic tic
batchOptimizer = LevenbergMarquardtOptimizer(graph, initial); batchOptimizer = LevenbergMarquardtOptimizer(graph, initial);
result = batchOptimizer.optimize(); result = batchOptimizer.optimize();