interface update

matlab/+gtsam/cylinderSampleProjection.m

@@ -1,4 +1,4 @@
-function [visiblePoints] = cylinderSampleProjection(camera, imageSize, cylinders)
+function [visiblePoints] = cylinderSampleProjection(K, pose, imageSize, cylinders)
 
 % Input: 
 % Output:
@@ -13,6 +13,8 @@ function [visiblePoints] = cylinderSampleProjection(camera, imageSize, cylinders
 
 import gtsam.*
 
+camera = SimpleCamera(pose, K);
+
 %% memory allocation
 cylinderNum = length(cylinders);
 visiblePoints.index = cell(cylinderNum,1);

matlab/+gtsam/plotCylinderSamples.m

@@ -11,7 +11,6 @@ function plotCylinderSamples(cylinders, fieldSize, figID)
 
     sampleDensity = 120;
     
-
     for i = 1:num                
         [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height);
         

matlab/+gtsam/points2DTrackMonocular.m

@@ -1,4 +1,4 @@
-function pts2dTracksMono = points2DTrackMonocular(cameras, imageSize, cylinders)
+function pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders)
 % Assess how accurately we can reconstruct points from a particular monocular camera setup. 
 % After creation of the factor graph for each track, linearize it around ground truth. 
 % There is no optimization
@@ -17,7 +17,7 @@ posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
 pointPriorNoise = noiseModel.Isotropic.Sigma(3, pointNoiseSigma);
 measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma);
 
-cameraPosesNum = length(cameras);
+cameraPosesNum = length(cameraPoses);
 
 %% add measurements and initial camera & points values
 pointsNum = 0;
@@ -26,41 +26,40 @@ for i = 1:cylinderNum
     pointsNum = pointsNum + length(cylinders{i}.Points);
 end
 
-pts3d = {};
+pts3d = cell(cameraPosesNum, 1);
 initialEstimate = Values;
 initialized = false;
 for i = 1:cameraPosesNum
     % add a constraint on the starting pose    
-    camera = cameras{i};
+    cameraPose = cameraPoses{i};
     
-    pts3d.pts{i} = cylinderSampleProjection(camera, imageSize, cylinders);
-    pts3d.camera{i} = camera;
+    pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders);
    
     if ~initialized
-        graph.add(PriorFactorPose3(symbol('x', 1), camera.pose, posePriorNoise));
+        graph.add(PriorFactorPose3(symbol('x', 1), cameraPose, posePriorNoise));
         k = 0;
-        if ~isempty(pts3d.pts{i}.data{1+k})
+        if ~isempty(pts3d{i}.data{1+k})
             graph.add(PriorFactorPoint3(symbol('p', 1), ...
-                pts3d.pts{i}.data{1+k}, pointPriorNoise));
+                pts3d{i}.data{1+k}, pointPriorNoise));
         else
             k = k+1;
         end
         initialized = true;
     end
     
-    for j = 1:length(pts3d.pts{i}.Z)
-        if isempty(pts3d.pts{i}.Z{j})
+    for j = 1:length(pts3d{i}.Z)
+        if isempty(pts3d{i}.Z{j})
             continue;
         end
-        graph.add(GenericProjectionFactorCal3_S2(pts3d.pts{i}.Z{j}, ...
-            measurementNoise, symbol('x', i), symbol('p', j), camera.calibration) );    
+        graph.add(GenericProjectionFactorCal3_S2(pts3d{i}.Z{j}, ...
+            measurementNoise, symbol('x', i), symbol('p', j), K) );    
     end
 
 end
 
 %% initialize cameras and points close to ground truth 
 for i = 1:cameraPosesNum
-    pose_i = camera.pose.retract(0.1*randn(6,1));
+    pose_i = cameraPoses{i}.retract(0.1*randn(6,1));
     initialEstimate.insert(symbol('x', i), pose_i);    
 end
 ptsIdx = 0;
@@ -75,24 +74,24 @@ end
 %% Print the graph
 graph.print(sprintf('\nFactor graph:\n'));
 
-marginals = Marginals(graph, initialEstimate);
+%marginals = Marginals(graph, initialEstimate);
 
 %% get all the points track information
 % currently throws the Indeterminant linear system exception
-ptx = 0;
+ptx = 1;
 for k = 1:cameraPosesNum
 
     for i = 1:length(cylinders)
         for j = 1:length(cylinders{i}.Points)
-            if isempty(pts3d.pts{k}.index{i}{j})
+            if isempty(pts3d{k}.index{i}{j})
                 continue;
             end
+            idx = pts3d{k}.index{i}{j};
+            pts2dTracksMono.pt3d{ptx} = pts3d{k}.data{idx};
+            pts2dTracksMono.Z{ptx} = pts3d{k}.Z{idx};
+            %pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',idx));
+            
             ptx = ptx + 1;
-   
-            idx = pts3d.pts{k}.index{i}{j};
-            pts2dTracksMono.pt3d{ptx} = pts3d.pts{k}.data{idx};
-            pts2dTracksMono.Z{ptx} = pts3d.pts{k}.Z{idx};
-            pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',idx));
         end
     end
    
@@ -100,7 +99,10 @@ end
 
 %% plot the result with covariance ellipses
 hold on;
-plot3DPoints(initialEstimate, [], marginals);
-plot3DTrajectory(initialEstimate, '*', 1, 8, marginals);
+%plot3DPoints(initialEstimate, [], marginals);
+%plot3DTrajectory(initialEstimate, '*', 1, 8, marginals);
+plot3DTrajectory(initialEstimate, '*', 1, 8);
+view(3);
+
 
 end

matlab/gtsam_examples/CameraFlyingExample.m

@@ -20,8 +20,8 @@ cylinders = cell(cylinderNum, 1);
 theta = 0;
 for i = 1:cylinderNum
     theta = theta + 2*pi / 10;
-    x = 20 * cos(theta) + options.fieldSize.x/2;
-    y = 20 * sin(theta) + options.fieldSize.y/2;
+    x = 10 * cos(theta) + options.fieldSize.x/2;
+    y = 10 * sin(theta) + options.fieldSize.y/2;
     baseCentroid = Point2([x, y]');
     cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1);
 end
@@ -32,46 +32,40 @@ figID = 1;
 figure(figID);
 plotCylinderSamples(cylinders, options.fieldSize, figID);
 
-%% generate camera trajectories
-K = Cal3_S2(525,525,0,320,240);
+%% generate camera trajectories: a circle 
+KMono = Cal3_S2(525,525,0,320,240);
 imageSize = Point2([640, 480]');
-cameras = cell(options.poseNum, 1);
+cameraPoses = cell(options.poseNum, 1);
 % Generate ground truth trajectory r.w.t. the field center
 theta = 0;
-r = 30;
+r = 40;
 for i = 1:options.poseNum    
     theta = (i-1)*2*pi/options.poseNum;
-    t = Point3([30*cos(theta), 30*sin(theta), 10]');
-    cameras{i} = SimpleCamera.Lookat(t, ... 
+    t = Point3([r*cos(theta) + options.fieldSize.x/2, ...
+        r*sin(theta) + options.fieldSize.y/2, 10]');
+    camera = SimpleCamera.Lookat(t, ... 
         Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
-        Point3([0,0,1]'), K);    
+        Point3([0,0,1]'), KMono);    
+    cameraPoses{i} = camera.pose;
 end
 
 %% visibility validation
 % for a simple test, it will be removed later
-visiblePoints3 = cylinderSampleProjection(cameras{1}, imageSize, cylinders);
+%visiblePoints3 = cylinderSampleProjection(cameras{1}, imageSize, cylinders);
 
 %% plot all the projected points
 %plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID);
 
 %% setp up monocular camera and get measurements
-pts2dTracksMono = points2DTrackMonocular(cameras, imageSize, cylinders);
+pts2dTracksMono = points2DTrackMonocular(KMono, cameraPoses, imageSize, cylinders);
 
 %% set up stereo camera and get measurements
 % load stereo calibration
 calib = dlmread(findExampleDataFile('VO_calibration.txt'));
 KStereo = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6));
 camerasStereo = cell(options.poseNum, 1);
-% for i = 1:options.poseNum
-%     cylinderIdx = max(min(round(cylinderNum*rand), 10), 1);
-%     camerasStereo{i} = SimpleCamera.Lookat(trans, cylinders{cylinderIdx}.centroid, ...
-%         Point3([0,0,1]'), KStereo);
-%     
-%     incT = Point3(5*rand, 5*rand, 5*rand);
-%     trans = trans.compose(incT);    
-% end
 
-%pts2dTracksStereo = points2DTrackStereo(camerasStereo, imageSize, cylinders);
+%pts2dTracksStereo = points2DTrackStereo(KStereo, cameraPoses, imageSize, cylinders);
 
 % plot the 2D tracks