Range SLAM examples in MATLAB, one SAM, one iSAM

matlab/gtsam_examples/RangeISAMExample_plaza.m

@@ -27,18 +27,17 @@ import gtsam.*
 %    Time (sec)	X_pose (m)	Y_pose (m)
 % TD
 %    Time (sec)	Sender / Antenna ID	Receiver Node ID	Range (m)
-datafile = findExampleDataFile('Plaza2_.mat');
+datafile = findExampleDataFile('Plaza1_.mat');
 load(datafile)
 M=size(DR,1);
 K=size(TD,1);
-minK=100; % minimum number of range measurements to process initially
+minK=200; % minimum number of range measurements to process initially
 incK=5; % minimum number of range measurements to process after
 sigmaR = 100; % range standard deviation
 sigmaInitial = 1; % draw initial landmark guess from Gaussian
 useGroundTruth = false;
 useRobust=true;
 addRange=true;
-useResult=true;
 batchInitialization=true;
 
 %% Set Noise parameters
@@ -53,15 +52,10 @@ else
 end
 
 %% Initialize iSAM
-params = gtsam.ISAM2Params;
+isam = ISAM2;
-params.setRelinearizeSkip(1);
-% gnParams = ISAM2GaussNewtonParams;
-% dlParams = ISAM2DoglegParams;
-% params.setOptimizationParams(gnParams);
-isam = ISAM2(params);
 
 %% Add prior on first pose
-pose0 = Pose2(GT(1,2),GT(1,3),pi+GT(1,4));
+pose0 = Pose2(GT(1,2),GT(1,3),GT(1,4));
 newFactors = NonlinearFactorGraph;
 if ~addRange | ~useGroundTruth
   newFactors.add(PriorFactorPose2(0,pose0,noiseModels.prior));
@@ -71,16 +65,19 @@ initial.insert(0,pose0);
 odo = Values;
 odo.insert(0,pose0);
 
+%% initialize points
 if addRange
+  landmarkEstimates = Values;
   for i=1:size(TL,1)
     j=TL(i,1);
     if useGroundTruth
       Lj = Point2(TL(i,2),TL(i,3));
-      initial.insert(symbol('L',j),Lj);
       newFactors.add(PriorFactorPoint2(symbol('L',j),Lj,noiseModels.pointPrior));
     else
-      initial.insert(symbol('L',j),Point2(sigmaInitial*randn,sigmaInitial*randn));
+      Lj = Point2(sigmaInitial*randn,sigmaInitial*randn);
     end
+    initial.insert(symbol('L',j),Lj);
+    landmarkEstimates.insert(symbol('L',j),Lj);
   end
   XY = utilities.extractPoint2(initial);
   plot(XY(:,1),XY(:,2),'g*');
@@ -113,18 +110,25 @@ for i=1:M % M
   predictedPose = lastPose.compose(odometry);
   lastPose = predictedPose;
   initial.insert(i,predictedPose);
+  landmarkEstimates.insert(i,predictedPose);
   
-  while k<=K & t>=TD(k,1)
+  % Check if there are range factors to be added
+  while k<=K && t>=TD(k,1)
     j = TD(k,3);
     range = TD(k,4);
-    %     fprintf(1,'%7g %7g %5d %5d %d %0.2f\n', t,TD(k,1),k, i, j, range);
     if addRange
-      newFactors.add(RangeFactorPosePoint2(i, symbol('L',j), range, noiseModels.range));
+      factor = RangeFactorPosePoint2(i, symbol('L',j), range, noiseModels.range);
+      % Throw out obvious outliers based on current landmark estimates
+      error=factor.unwhitenedError(landmarkEstimates);
+      if k<=minK || abs(error)<5
+        newFactors.add(factor);
+      end
     end
     k=k+1; countK=countK+1; update = true;
   end
   
-  if update & k>minK & countK>incK
+  % Check whether to update iSAM 2
+  if update && k>minK && countK>incK
     if batchInitialization % Do a full optimize for first minK ranges
       tic
       batchOptimizer = LevenbergMarquardtOptimizer(newFactors, initial);
@@ -133,12 +137,16 @@ for i=1:M % M
       batchInitialization = false; % only once
     end
     isam.update(newFactors, initial);
-    if useResult
+    result = isam.calculateEstimate();
-      result = isam.calculateEstimate(); 
+    lastPose = result.at(i);
-      lastPose = result.at(i);
+    % update landmark estimates
-    else
+    if addRange
-      lin = isam.getLinearizationPoint();
+      landmarkEstimates = Values;
-      lastPose = lin.at(i);
+      for jj=1:size(TL,1)
+        j=TL(jj,1);
+        key = symbol('L',j);
+        landmarkEstimates.insert(key,result.at(key));
+      end
     end
     newFactors = NonlinearFactorGraph;
     initial = Values;
@@ -146,7 +154,7 @@ for i=1:M % M
   end
   
   % visualize
-  if mod(i,50)==0 & k>minK
+  if mod(i,50)==0 && k>minK
     figure(1);clf;hold on
     
     % odometry
@@ -160,14 +168,20 @@ for i=1:M % M
     XY = utilities.extractPoint2(lin);
     plot(XY(:,1),XY(:,2),'r*');
     
-    % result 
+    % result
     result = isam.calculateEstimate();
     XYT = utilities.extractPose2(result);
     plot(XYT(:,1),XYT(:,2),'k-');
     XY = utilities.extractPoint2(result);
     plot(XY(:,1),XY(:,2),'k*');
     axis equal
-%     pause
+    %     pause
   end
 end
 toc
+
+%% Plot ground truth as well
+plot(GT(:,2),GT(:,3),'g-');
+plot(TL(:,2),TL(:,3),'g*');
+
+

matlab/gtsam_examples/RangeSLAMExample_plaza.m

@@ -0,0 +1,109 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% GTSAM Copyright 2010, Georgia Tech Research Corporation,
+% Atlanta, Georgia 30332-0415
+% All Rights Reserved
+% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+%
+% See LICENSE for the license information
+%
+% @brief Read Robotics Institute range-only Plaza2 dataset and do SAM
+% @author Frank Dellaert
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% preliminaries
+clear
+import gtsam.*
+
+%% Find and load data file
+% data available at http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/
+% Datafile format (from http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html)
+% GT: Groundtruth path from GPS
+%    Time (sec)	X_pose (m)	Y_pose (m)	Heading (rad)
+% DR: Odometry Input (delta distance traveled and delta heading change)
+%    Time (sec)	Delta Dist. Trav. (m)	Delta Heading (rad)
+% DRp: Dead Reckoned Path from Odometry
+%    Time (sec)	X_pose (m)	Y_pose (m)	Heading (rad)
+% TL: Surveyed Node Locations
+%    Time (sec)	X_pose (m)	Y_pose (m)
+% TD
+%    Time (sec)	Sender / Antenna ID	Receiver Node ID	Range (m)
+datafile = findExampleDataFile('Plaza1_.mat');
+load(datafile)
+M=size(DR,1);
+K=size(TD,1);
+sigmaR = 50; % range standard deviation
+sigmaInitial = 1; % draw initial landmark guess from Gaussian
+
+%% Set Noise parameters
+noiseModels.prior = noiseModel.Diagonal.Sigmas([1 1 pi]');
+noiseModels.pointPrior = noiseModel.Diagonal.Sigmas([1 1]');
+noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.01 0.2]');
+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));
+graph = NonlinearFactorGraph;
+graph.add(PriorFactorPose2(0,pose0,noiseModels.prior));
+initial = Values;
+initial.insert(0,pose0);
+
+for i=1:size(TL,1)
+  j=TL(i,1);
+  initial.insert(symbol('L',j),Point2(sigmaInitial*randn,sigmaInitial*randn));
+end
+
+%% Loop over odometry
+tic
+k = 1; % range measurement counter
+lastPose = pose0;
+for i=1:M
+  
+  % get odometry measurement
+  t = DR(i,1);
+  distance_traveled = DR(i,2);
+  delta_heading = DR(i,3);
+  
+  % add odometry factor
+  odometry = Pose2(distance_traveled,0,delta_heading);
+  graph.add(BetweenFactorPose2(i-1, i, odometry, noiseModels.odometry));
+  
+  % predict pose and add as initial estimate
+  predictedPose = lastPose.compose(odometry);
+  lastPose = predictedPose;
+  initial.insert(i,predictedPose);
+  
+  while k<=K && t>=TD(k,1)
+    j = TD(k,3);
+    range = TD(k,4);
+    factor = RangeFactorPosePoint2(i, symbol('L',j), range, noiseModels.range);
+    graph.add(factor);
+    k=k+1;
+  end
+  
+end
+toc
+
+%% GRaph was built, optimize !
+tic
+batchOptimizer = LevenbergMarquardtOptimizer(graph, initial);
+result = batchOptimizer.optimize();
+toc
+
+%% visualize
+figure(1);clf;hold on
+
+% odometry
+XYT = utilities.extractPose2(initial);
+plot(XYT(:,1),XYT(:,2),'y-');
+
+% GT
+plot(GT(:,2),GT(:,3),'g-');
+plot(TL(:,2),TL(:,3),'g*');
+
+% result
+XYT = utilities.extractPose2(result);
+plot(XYT(:,1),XYT(:,2),'k-');
+XY = utilities.extractPoint2(result);
+plot(XY(:,1),XY(:,2),'k*');
+axis equal