Re-factored iSAM MATLAB example and wrapped more ISAM functions

examples/matlab/VisualISAMExample.m

@@ -10,18 +10,32 @@
 % @author Duy-Nguyen Ta
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-%% Create a triangle target, just 3 points on a plane
+if 0
-nPoints = 3;
+    %% Create a triangle target, just 3 points on a plane
-r = 10;
+    nPoints = 3;
-points = {};
+    r = 10;
-for j=1:nPoints
+    points = {};
+    for j=1:nPoints
         theta = (j-1)*2*pi/nPoints;
         points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]');
+    end
+else
+    %% Generate simulated data
+    % 3D landmarks as vertices of a cube
+    nPoints = 8;
+    points = {gtsamPoint3([10 10 10]'),...
+        gtsamPoint3([-10 10 10]'),...
+        gtsamPoint3([-10 -10 10]'),...
+        gtsamPoint3([10 -10 10]'),...
+        gtsamPoint3([10 10 -10]'),...
+        gtsamPoint3([-10 10 -10]'),...
+        gtsamPoint3([-10 -10 -10]'),...
+        gtsamPoint3([10 -10 -10]')};
 end
 
 %% Create camera cameras on a circle around the triangle
 nCameras = 10;
-height = 10;
+height = 0;
 r = 30;
 cameras = {};
 K = gtsamCal3_S2(500,500,0,640/2,480/2);
@@ -33,57 +47,58 @@ end
 odometry = cameras{1}.pose.between(cameras{2}.pose);
 
 poseNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
+odometryNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
 pointNoise = gtsamSharedNoiseModel_Sigma(3, 0.1);
 measurementNoise = gtsamSharedNoiseModel_Sigma(2, 1.0);
 
-%% Create an ISAM object for inference
+%% Initialize iSAM
-isam = visualSLAMISAM;
+isam = visualSLAMISAM(2);
-
-%% Update ISAM
 newFactors = visualSLAMGraph;
 initialEstimates = visualSLAMValues;
-for i=1:nCameras
+if 1 % add hard constraint
-    
+    newFactors.addPoseConstraint(symbol('x',1),cameras{1}.pose);
-    % Prior for the first pose or odometry for subsequent cameras
+else
-    if (i==1)
     newFactors.addPosePrior(symbol('x',1), cameras{1}.pose, poseNoise);
-        for j=1:nPoints
+end
+initialEstimates.insertPose(symbol('x',1), cameras{1}.pose);
+% Add visual measurement factors from first pose
+for j=1:nPoints
+    if 0 % add point priors
     	newFactors.addPointPrior(symbol('l',j), points{j}, pointNoise);
     end
-    else
+    zij = cameras{i}.project(points{j});
-        newFactors.addOdometry(symbol('x',i-1), symbol('x',i), odometry, poseNoise);
+    newFactors.addMeasurement(zij, measurementNoise, symbol('x',1), symbol('l',j), K);
-    end
+    initialEstimates.insertPoint(symbol('l',j), points{j});
+end
 
-    % Visual measurement factors
+%% Run iSAM Loop
+for i=2:nCameras
+    
+    %% Add odometry
+    newFactors.addOdometry(symbol('x',i-1), symbol('x',i), odometry, odometryNoise);
+    
+    %% Add visual measurement factors
     for j=1:nPoints
         zij = cameras{i}.project(points{j});
         newFactors.addMeasurement(zij, measurementNoise, symbol('x',i), symbol('l',j), K);
     end
     
-    % Initial estimates for the new pose. Also initialize points while in 
+    %% Initial estimates for the new pose. Also initialize points while in the first frame.
-    % the first frame.
+    %TODO: this might be suboptimal since "result" is not the fully optimized result
-    if (i==1)
-        initialEstimates.insertPose(symbol('x',i), cameras{i}.pose);
-        for j=1:size(points,2)
-            initialEstimates.insertPoint(symbol('l',j), points{j});
-        end
-    else
-        %TODO: this might be suboptimal since "result" is not the fully
-        %optimized result
     if (i==2), prevPose = cameras{1}.pose;
     else, prevPose = result.pose(symbol('x',i-1)); end
     initialEstimates.insertPose(symbol('x',i), prevPose.compose(odometry));
+    
+    %% Update ISAM
+    isam.update(newFactors, initialEstimates);
+    result = isam.estimate();
+    if 0 % re-linearize
+        isam.reorder_relinearize();
     end
     
-    % Update ISAM, only update for the second frame onward
+    %% Plot results
-    % Update the first frame will cause error, since it's under constrained
+    P1 = isam.marginalCovariance(symbol('x',1));
-    if (i>=2)
+    sqrt(diag(P1))
-        isam.update(newFactors, initialEstimates);
-        emptyFactors = visualSLAMGraph;
-        emptyEstimates = visualSLAMValues;
-        result = isam.estimate();
-
-        % Plot first result
     h=figure(1);clf
     hold on;
     for j=1:size(points,2)
@@ -92,19 +107,20 @@ for i=1:nCameras
         plot3(point_j.x, point_j.y, point_j.z,'marker','o');
         covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P);
     end
-
     for ii=1:i
         P = isam.marginalCovariance(symbol('x',ii));
         pose_ii = result.pose(symbol('x',ii));
         plotPose3(pose_ii,P,10);
+        if 1 % show ground truth
+            plotPose3(cameras{ii}.pose,0.001*eye(6),10);
         end
-        axis([-50 50 -50 50 -50 50])
+    end
+    axis([-40 40 -40 40 -10 20]);axis equal
+    view(2)
     colormap('hot')
     %print(h,'-dpng',sprintf('VisualISAM_%03d.png',i));
     
-        % Reset newFactors and initialEstimates to prepare for the next 
+    %% Reset newFactors and initialEstimates to prepare for the next update
-        % update
     newFactors = visualSLAMGraph;
     initialEstimates = visualSLAMValues;
-    end
 end

gtsam.h

@@ -600,6 +600,14 @@ class GaussianFactorGraph {
 	Matrix denseHessian() const;
 };
 
+class GaussianISAM {
+  GaussianISAM();
+  gtsam::GaussianFactor* marginalFactor(size_t j) const;
+  gtsam::GaussianBayesNet* marginalBayesNet(size_t key) const;
+  Matrix marginalCovariance(size_t key) const;
+  gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const;
+};
+
 class GaussianSequentialSolver {
 	GaussianSequentialSolver(const gtsam::GaussianFactorGraph& graph,
 			bool useQR);
@@ -925,10 +933,15 @@ class Graph {
 class ISAM {
 	ISAM();
 	ISAM(int reorderInterval);
-	void print(string s) const;
 	visualSLAM::Values estimate() const;
   Matrix marginalCovariance(size_t key) const;
+  int reorderInterval() const;
+  int reorderCounter() const;
+  void print(string s) const;
   void update(const visualSLAM::Graph& newFactors, const visualSLAM::Values& initialValues);
+  void reorder_relinearize();
+  void addKey(size_t key);
+  void setOrdering(const gtsam::Ordering& new_ordering);
 };
 
 }///\namespace visualSLAM