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

Ability to switch between plaza1 and plaza2

release/4.3a0
Frank Dellaert 2013-06-21 14:17:58 +00:00
parent f1ee3b353c
commit 49338228a5
1 changed files with 12 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
matlab/gtsam_examples/RangeISAMExample_plaza.m
View File

@ -27,11 +27,18 @@ 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'); if true % switch between data files
datafile = findExampleDataFile('Plaza1_.mat');
headingOffset=0;
minK=200; % minimum number of range measurements to process initially
else
datafile = findExampleDataFile('Plaza2_.mat');
headingOffset=pi;
minK=150; % needs less for init
end
load(datafile) load(datafile)
M=size(DR,1); M=size(DR,1);
K=size(TD,1); K=size(TD,1);
minK=200; % minimum number of range measurements to process initially
incK=5; % minimum number of range measurements to process after incK=5; % minimum number of range measurements to process after
sigmaR = 100; % range standard deviation sigmaR = 100; % range standard deviation
sigmaInitial = 1; % draw initial landmark guess from Gaussian sigmaInitial = 1; % draw initial landmark guess from Gaussian
@ -55,7 +62,7 @@ end
isam = ISAM2; isam = ISAM2;
%% 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),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));
@ -126,14 +133,14 @@ for i=1:M % M
end end
k=k+1; countK=countK+1; update = true; k=k+1; countK=countK+1; update = true;
end end
% Check whether to update iSAM 2 % Check whether to update iSAM 2
if update && k>minK && countK>incK if update && k>minK && countK>incK
if batchInitialization % Do a full optimize for first minK ranges if batchInitialization % Do a full optimize for first minK ranges
tic tic
batchOptimizer = LevenbergMarquardtOptimizer(newFactors, initial); batchOptimizer = LevenbergMarquardtOptimizer(newFactors, initial);
toc
initial = batchOptimizer.optimize(); initial = batchOptimizer.optimize();
toc
batchInitialization = false; % only once batchInitialization = false; % only once
end end
isam.update(newFactors, initial); isam.update(newFactors, initial);