diff --git a/matlab/gtsam_examples/RangeISAMExample_plaza.m b/matlab/gtsam_examples/RangeISAMExample_plaza.m index 788745a3d..cffe81c30 100644 --- a/matlab/gtsam_examples/RangeISAMExample_plaza.m +++ b/matlab/gtsam_examples/RangeISAMExample_plaza.m @@ -27,7 +27,7 @@ import gtsam.* % Time (sec) X_pose (m) Y_pose (m) % TD % 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'); headingOffset=0; minK=200; % minimum number of range measurements to process initially @@ -65,7 +65,7 @@ isam = ISAM2; %% Add prior on first pose pose0 = Pose2(GT(1,2),GT(1,3),headingOffset+GT(1,4)); newFactors = NonlinearFactorGraph; -if ~addRange | ~useGroundTruth +if ~addRange || ~useGroundTruth newFactors.add(PriorFactorPose2(0,pose0,noiseModels.prior)); end initial = Values; diff --git a/matlab/gtsam_examples/RangeSLAMExample_plaza.m b/matlab/gtsam_examples/RangeSLAMExample_plaza.m index 24619b69c..bd643d854 100644 --- a/matlab/gtsam_examples/RangeSLAMExample_plaza.m +++ b/matlab/gtsam_examples/RangeSLAMExample_plaza.m @@ -27,7 +27,7 @@ import gtsam.* % Time (sec) X_pose (m) Y_pose (m) % TD % Time (sec) Sender / Antenna ID Receiver Node ID Range (m) -datafile = findExampleDataFile('Plaza1_.mat'); +datafile = findExampleDataFile('Plaza2_.mat'); load(datafile) M=size(DR,1); K=size(TD,1); @@ -42,7 +42,7 @@ base = noiseModel.mEstimator.Tukey(5); noiseModels.range = noiseModel.Robust(base,noiseModel.Isotropic.Sigma(1, sigmaR)); %% 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.add(PriorFactorPose2(0,pose0,noiseModels.prior)); initial = Values; @@ -84,7 +84,7 @@ for i=1:M end toc -%% GRaph was built, optimize ! +%% Graph was built, optimize ! tic batchOptimizer = LevenbergMarquardtOptimizer(graph, initial); result = batchOptimizer.optimize();