create and use cameras, not poses

examples/matlab/VisualISAMExample.m

@@ -19,19 +19,18 @@ for j=1:nPoints
     points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]');
 end
 
-%% Create camera poses on a circle around the triangle
+%% Create camera cameras on a circle around the triangle
 nCameras = 10;
 height = 10;
 r = 30;
-poses = {};
+cameras = {};
 K = gtsamCal3_S2(500,500,0,640/2,480/2);
 for i=1:nCameras
     theta = (i-1)*2*pi/nCameras;
     t = gtsamPoint3([r*cos(theta), r*sin(theta), height]');
-    camera = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K)
+    cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K);
-    poses{i} = camera.pose();
 end
-odometry = poses{1}.between(poses{2});
+odometry = cameras{1}.pose.between(cameras{2}.pose);
 
 poseNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
 pointNoise = gtsamSharedNoiseModel_Sigma(3, 0.1);
@@ -45,9 +44,9 @@ newFactors = visualSLAMGraph;
 initialEstimates = visualSLAMValues;
 for i=1:nCameras
     
-    % Prior for the first pose or odometry for subsequent poses
+    % Prior for the first pose or odometry for subsequent cameras
     if (i==1)
-        newFactors.addPosePrior(symbol('x',1), poses{1}, poseNoise);
+        newFactors.addPosePrior(symbol('x',1), cameras{1}.pose, poseNoise);
         for j=1:nPoints
             newFactors.addPointPrior(symbol('l',j), points{j}, pointNoise);
         end
@@ -57,22 +56,21 @@ for i=1:nCameras
 
     % Visual measurement factors
     for j=1:nPoints
-        camera = gtsamSimpleCamera(K,poses{i});
+        zij = cameras{i}.project(points{j});
-        zij = camera.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 
     % the first frame.
     if (i==1)
-        initialEstimates.insertPose(symbol('x',i), poses{i});
+        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 = poses{1};
+        if (i==2), prevPose = cameras{1}.pose;
         else, prevPose = result.pose(symbol('x',i-1)); end
         initialEstimates.insertPose(symbol('x',i), prevPose.compose(odometry));
     end

examples/matlab/VisualSLAMExample.m

@@ -27,17 +27,16 @@ points = {gtsamPoint3([10 10 10]'),...
     gtsamPoint3([-10 -10 -10]'),...
     gtsamPoint3([10 -10 -10]')};
 
-% Camera poses on a circle around the cube, pointing at the world origin
+% Camera cameras on a circle around the cube, pointing at the world origin
 nCameras = 6;
 height = 0;
 r = 30;
-poses = {};
+cameras = {};
 K = gtsamCal3_S2(500,500,0,640/2,480/2);
 for i=1:nCameras
     theta = (i-1)*2*pi/nCameras;
     t = gtsamPoint3([r*cos(theta), r*sin(theta), height]');
-    camera = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K)
+    cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K);
-    poses{i} = camera.pose();
 end
 
 measurementNoiseSigma = 1.0;
@@ -50,26 +49,25 @@ graph = visualSLAMGraph;
 %% Add factors for all measurements
 measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma);
 for i=1:nCameras
-    camera = gtsamSimpleCamera(K,poses{i});
     for j=1:size(points,2)
-        zij = camera.project(points{j}); % you can add noise here if desired
+        zij = cameras{i}.project(points{j}); % you can add noise here if desired
         graph.addMeasurement(zij, measurementNoise, symbol('x',i), symbol('l',j), K);
     end
 end
 
 %% Add Gaussian priors for a pose and a landmark to constrain the system
 posePriorNoise  = gtsamSharedNoiseModel_Sigmas(poseNoiseSigmas);
-graph.addPosePrior(symbol('x',1), poses{1}, posePriorNoise);
+graph.addPosePrior(symbol('x',1), cameras{1}.pose, posePriorNoise);
 pointPriorNoise  = gtsamSharedNoiseModel_Sigma(3,pointNoiseSigma);
 graph.addPointPrior(symbol('l',1), points{1}, pointPriorNoise);
 
 %% Print the graph
 graph.print(sprintf('\nFactor graph:\n'));
 
-%% Initialize to noisy poses and points
+%% Initialize to noisy cameras and points
 initialEstimate = visualSLAMValues;
-for i=1:size(poses,2)
+for i=1:size(cameras,2)
-    initialEstimate.insertPose(symbol('x',i), poses{i});
+    initialEstimate.insertPose(symbol('x',i), cameras{i}.pose);
 end
 for j=1:size(points,2)
     initialEstimate.insertPoint(symbol('l',j), points{j});
@@ -91,10 +89,11 @@ for j=1:size(points,2)
     covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P);
 end
 
-for i=1:size(poses,2)
+for i=1:size(cameras,2)
     P = marginals.marginalCovariance(symbol('x',i))
     pose_i = result.pose(symbol('x',i))
     plotPose3(pose_i,P,10);
 end
+axis([-35 35 -35 35 -15 15]);
 axis equal
-
+view(-37,40)

examples/matlab/VisualSLAMExample_triangle.m

@@ -18,17 +18,16 @@ for j=1:3
     points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]');
 end
 
-%% Create camera poses on a circle around the triangle
+%% Create camera cameras on a circle around the triangle
 nCameras = 6;
 height = 10;
 r = 30;
-poses = {};
+cameras = {};
 K = gtsamCal3_S2(500,500,0,640/2,480/2);
 for i=1:nCameras
     theta = (i-1)*2*pi/nCameras;
     t = gtsamPoint3([r*cos(theta), r*sin(theta), height]');
-    camera = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K)
+    cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K)
-    poses{i} = camera.pose();
 end
 
 %% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph)
@@ -38,9 +37,8 @@ graph = visualSLAMGraph;
 measurementNoiseSigma=1; % in pixels
 measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma);
 for i=1:nCameras
-    camera = gtsamSimpleCamera(K,poses{i});
     for j=1:3
-        zij = camera.project(points{j}); % you can add noise here if desired
+        zij = cameras{i}.project(points{j}); % you can add noise here if desired
         graph.addMeasurement(zij, measurementNoise, symbol('x',i), symbol('l',j), K);
     end
 end
@@ -54,10 +52,10 @@ end
 %% Print the graph
 graph.print(sprintf('\nFactor graph:\n'));
 
-%% Initialize to noisy poses and points
+%% Initialize to noisy cameras and points
 initialEstimate = visualSLAMValues;
-for i=1:size(poses,2)
+for i=1:size(cameras,2)
-    initialEstimate.insertPose(symbol('x',i), poses{i});
+    initialEstimate.insertPose(symbol('x',i), cameras{i}.pose);
 end
 for j=1:size(points,2)
     initialEstimate.insertPoint(symbol('l',j), points{j});
@@ -79,10 +77,11 @@ for j=1:size(points,2)
     covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P);
 end
 
-for i=1:size(poses,2)
+for i=1:size(cameras,2)
     P = marginals.marginalCovariance(symbol('x',i))
     pose_i = result.pose(symbol('x',i))
     plotPose3(pose_i,P,10);
 end
+axis([-20 20 -20 20 -1 15]);
 axis equal
-
+view(-37,40)