Initialize now initailizes first two cameras, makes so much more sense!

matlab/examples/VisualISAMDemo.m

@@ -8,7 +8,7 @@ global SAVE_GRAPH PRINT_STATS DRAW_INTERVAL CAMERA_INTERVAL DRAW_TRUE_POSES
 global SAVE_FIGURES SAVE_GRAPHS SHOW_TIMING
 global data
 global poseNoise pointNoise odometryNoise measurementNoise
-global frame_i isam newFactors initialEstimates result
+global frame_i isam result
 
 % Start GUI
 VisualISAM_gui

matlab/examples/VisualISAMExample.m

@@ -11,7 +11,8 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 %% Global variables used in VisualISAMExample
-global data
+global data isam frame_i result
+global poseNoise odometryNoise pointNoise measurementNoise
 global HARD_CONSTRAINT POINT_PRIORS BATCH_INIT REORDER_INTERVAL ALWAYS_RELINEARIZE 
 global SAVE_GRAPH PRINT_STATS DRAW_INTERVAL CAMERA_INTERVAL DRAW_TRUE_POSES 
 global SAVE_FIGURES SAVE_GRAPHS 
@@ -44,7 +45,7 @@ figure;
 VisualISAMPlot
 
 %% Main loop for iSAM: stepping through all poses
-for frame_i=2:options.nrCameras
+for frame_i=3:options.nrCameras
     VisualISAMStep
     if mod(frame_i,DRAW_INTERVAL)==0
         VisualISAMPlot

matlab/examples/VisualISAMExample_triangle.m

@@ -11,7 +11,8 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 %% Global variables used in VisualISAMExample
-global data
+global data isam frame_i result
+global poseNoise odometryNoise pointNoise measurementNoise
 global HARD_CONSTRAINT POINT_PRIORS BATCH_INIT REORDER_INTERVAL ALWAYS_RELINEARIZE 
 global SAVE_GRAPH PRINT_STATS DRAW_INTERVAL CAMERA_INTERVAL DRAW_TRUE_POSES 
 global SAVE_FIGURES SAVE_GRAPHS 
@@ -44,7 +45,7 @@ figure;
 VisualISAMPlot
 
 %% Main loop for iSAM: stepping through all poses
-for frame_i=2:options.nrCameras
+for frame_i=3:options.nrCameras
     VisualISAMStep
     if mod(frame_i,DRAW_INTERVAL)==0
         VisualISAMPlot

matlab/examples/VisualISAMInitialize.m

@@ -1,17 +1,17 @@
-function VisualISAMStep
-% VisualISAMStep: execute one update step of visualSLAM::iSAM object
+function VisualInitialize
+% VisualInitialize: initialize visualSLAM::iSAM object and noise parameters
 % Authors: Duy Nguyen Ta and Frank Dellaert
 
+% options
+global REORDER_INTERVAL HARD_CONSTRAINT POINT_PRIORS BATCH_INIT ALWAYS_RELINEARIZE
+
 % global variables, input
-global data pointNoise poseNoise measurementNoise
+global data
 
 % global variables, output
-global isam newFactors initialEstimates frame_i result
+global isam frame_i result
 global poseNoise odometryNoise pointNoise measurementNoise
 
-% options
-global REORDER_INTERVAL HARD_CONSTRAINT POINT_PRIORS
-
 %% Initialize iSAM
 isam = visualSLAMISAM(REORDER_INTERVAL);
 
@@ -24,27 +24,50 @@ measurementNoise = gtsamSharedNoiseModel_Sigma(2, 1.0);
 %% Add constraints/priors
 newFactors = visualSLAMGraph;
 initialEstimates = visualSLAMValues;
-i1 = symbol('x',1);
-camera1 = data.cameras{1};
-pose1 = camera1.pose;
-if HARD_CONSTRAINT % add hard constraint
-    newFactors.addPoseConstraint(i1,pose1);
-else
-    newFactors.addPosePrior(i1,pose1, poseNoise);
+for frame_i=1:2
+    ii = symbol('x',frame_i);
+    if frame_i==1 & HARD_CONSTRAINT % add hard constraint
+        newFactors.addPoseConstraint(ii,data.cameras{1}.pose);
+    else
+        newFactors.addPosePrior(ii,data.cameras{frame_i}.pose, poseNoise);
+    end
+    % TODO: init should not be from ground truth!
+    initialEstimates.insertPose(ii,data.cameras{frame_i}.pose);
 end
-initialEstimates.insertPose(i1,pose1); % TODO: should not be from ground truth!
 
-%% Add visual measurement factors from first pose
+%% Add visual measurement factors from two first poses
+for frame_i=1:2
+    ii = symbol('x',frame_i);
+    for j=1:size(data.points,2)
+        jj = symbol('l',j);
+        zij = data.cameras{frame_i}.project(data.points{j});
+        newFactors.addMeasurement(zij, measurementNoise, ii, jj, data.K);
+    end
+end
+
+%% Initialize points, possibly add priors on them
 for j=1:size(data.points,2)
     jj = symbol('l',j);
     if POINT_PRIORS % add point priors
         newFactors.addPointPrior(jj, data.points{j}, pointNoise);
     end
-    zij = camera1.project(data.points{j});
-    newFactors.addMeasurement(zij, measurementNoise, i1, jj, data.K);
     initialEstimates.insertPoint(jj, data.points{j});  % TODO: should not be from ground truth!
 end
 
-frame_i = 1;
-result = initialEstimates;
+%% Update ISAM
+if BATCH_INIT % Do a full optimize for first two poses
+    fullyOptimized = newFactors.optimize(initialEstimates);
+    isam.update(newFactors, fullyOptimized);
+else
+    isam.update(newFactors, initialEstimates);
+end
+% figure(1);tic;
+% t=toc; plot(frame_i,t,'r.'); tic
+result = isam.estimate();
+% t=toc; plot(frame_i,t,'g.');
+
+if ALWAYS_RELINEARIZE % re-linearize
+    isam.reorder_relinearize();
+end
+
 cla;

matlab/examples/VisualISAMPlot.m

@@ -6,6 +6,7 @@ global data frame_i isam result
 
 % options
 global CAMERA_INTERVAL DRAW_TRUE_POSES SAVE_FIGURES SAVE_GRAPHS
+global SAVE_GRAPH PRINT_STATS
 
 %% Plot results
 h=gca;
@@ -46,4 +47,12 @@ if SAVE_GRAPHS && (frame_i>1)
     isam.saveGraph(sprintf('VisualiSAM%03d.dot',frame_i));
 end
 
+if SAVE_GRAPH
+    isam.saveGraph(sprintf('VisualiSAM.dot',frame_i));
+end
+
+if PRINT_STATS
+    isam.printStats();
+end
+
 drawnow

matlab/examples/VisualISAMStep.m

@@ -3,7 +3,7 @@ function VisualISAMStep
 % Authors: Duy Nguyen Ta and Frank Dellaert
 
 % global variables, input
-global frame_i odometryNoise measurementNoise newFactors initialEstimates
+global frame_i odometryNoise measurementNoise
 global data
 
 % global variables, input/output
@@ -13,16 +13,12 @@ global isam
 global result
 
 % options
-global BATCH_INIT SHOW_TIMING ALWAYS_RELINEARIZE 
-global SAVE_GRAPH PRINT_STATS
+global SHOW_TIMING ALWAYS_RELINEARIZE 
 
 % iSAM expects us to give it a new set of factors 
 % along with initial estimates for any new variables introduced.
-% We do not clear in frame 2 so we add to the factors added in Initialize
-if frame_i > 2
-    newFactors = visualSLAMGraph;
-    initialEstimates = visualSLAMValues;
-end
+newFactors = visualSLAMGraph;
+initialEstimates = visualSLAMValues;
 
 %% Add odometry
 newFactors.addOdometry(symbol('x',frame_i-1), symbol('x',frame_i), data.odometry, odometryNoise);
@@ -33,29 +29,17 @@ for j=1:size(data.points,2)
     newFactors.addMeasurement(zij, measurementNoise, symbol('x',frame_i), symbol('l',j), data.K);
 end
 
-%% Initial estimates for the new pose. Also initialize data.points while in the first frame.
-if (frame_i==2), prevPose = data.cameras{1}.pose;
-else, prevPose = result.pose(symbol('x',frame_i-1)); end
+%% Initial estimates for the new pose.
+prevPose = result.pose(symbol('x',frame_i-1));
 initialEstimates.insertPose(symbol('x',frame_i), prevPose.compose(data.odometry));
 
 %% Update ISAM
-if BATCH_INIT & (frame_i==2) % Do a full optimize for first two poses
-    fullyOptimized = newFactors.optimize(initialEstimates);
-    initialEstimates = fullyOptimized;
-end
 % figure(1);tic;
 isam.update(newFactors, initialEstimates);
 % t=toc; plot(frame_i,t,'r.'); tic
 result = isam.estimate();
 % t=toc; plot(frame_i,t,'g.');
+
 if ALWAYS_RELINEARIZE % re-linearize
     isam.reorder_relinearize();
 end
-
-if SAVE_GRAPH
-    isam.saveGraph(sprintf('VisualiSAM.dot',frame_i));
-end
-
-if PRINT_STATS
-    isam.printStats();
-end