2D monocular track. Testing with random data now throws indeterminant linear system exception

matlab/+gtsam/cylinderSampleProjection.m

@@ -15,14 +15,15 @@ import gtsam.*
 
 %% memory allocation
 cylinderNum = length(cylinders);
-visiblePoints.index = cell(cylinderNum);
+visiblePoints.index = cell(cylinderNum,1);
 
 pointCloudNum = 0;
 for i = 1:cylinderNum
     pointCloudNum = pointCloudNum + length(cylinders{i}.Points);
-    visiblePoints.index{i} = cell(pointCloudNum);
+    visiblePoints.index{i} = cell(pointCloudNum,1);
 end
-visiblePoints.data = cell(pointCloudNum);
+visiblePoints.data = cell(pointCloudNum,1);
+visiblePoints.Z = cell(pointCloudNum, 1);
 
 %% check visiblity of points on each cylinder
 pointCloudIndex = 0;
@@ -34,8 +35,12 @@ for i = 1:cylinderNum
     for j = 1:pointNum
 
         pointCloudIndex  = pointCloudIndex + 1;
-        
+                
         sampledPoint3 = cylinders{i}.Points{j};
+        sampledPoint3local = camera.pose.transform_to(sampledPoint3);
+        if sampledPoint3local.z < 0
+            continue; % Cheirality Exception
+        end
         Z2d = camera.project(sampledPoint3);
 
         % ignore points not visible in the scene
@@ -50,8 +55,8 @@ for i = 1:cylinderNum
         %   2. For points behind the cylinders' surfaces, the cylinder
         for k = 1:cylinderNum
 
-            rayCameraToPoint = cameraPose.translation().between(sampledPoint3).vector();
+            rayCameraToPoint = camera.pose.translation().between(sampledPoint3).vector();
-            rayCameraToCylinder = cameraPose.translation().between(cylinders{i}.centroid).vector();
+            rayCameraToCylinder = camera.pose.translation().between(cylinders{i}.centroid).vector();
             rayCylinderToPoint = cylinders{i}.centroid.between(sampledPoint3).vector();
 
             % Condition 1: all points in front of the cylinders'
@@ -69,7 +74,7 @@ for i = 1:cylinderNum
                 rayCylinderToProjected = norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
                 if rayCylinderToProjected(1) > cylinders{i}.radius && ...
                         rayCylinderToProjected(2) > cylinders{i}.radius
-                    visiblePoints.data{pointCloudIndex} = sampledPoints3;
+                    visiblePoints.data{pointCloudIndex} = sampledPoint3;
                     visiblePoints.Z{pointCloudIndex} = Z2d;
                     visiblePoints.index{i}{j} = pointCloudIndex;
                 end

matlab/+gtsam/points2DTrackMonocular.m

@@ -1,4 +1,4 @@
-function pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders)
+function pts2dTracksMono = points2DTrackMonocular(cameras, 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
@@ -9,15 +9,15 @@ import gtsam.*
 %% create graph
 graph = NonlinearFactorGraph;
 
+%% create the noise factors
 pointNoiseSigma = 0.1;
 poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
-
+measurementNoiseSigma = 1.0;
-%% add a constraint on the starting pose
 posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
-firstPose = cameraPoses{1};
+pointPriorNoise = noiseModel.Isotropic.Sigma(3, pointNoiseSigma);
-graph.add(PriorFactorPose3(symbol('x', l), firstPose, posePriorNoise));
+measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma);
 
-cameraPosesNum = length(cameraPoses);
+cameraPosesNum = length(cameras);
 
 %% add measurements and initial camera & points values
 pointsNum = 0;
@@ -26,27 +26,50 @@ for i = 1:cylinderNum
     pointsNum = pointsNum + length(cylinders{i}.Points);
 end
 
-measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma);
-
 pts3d = {};
 initialEstimate = Values;
+initialized = false;
 for i = 1:cameraPosesNum
-    camera = SimpleCamera(K, cameraPoses{i});
+    % add a constraint on the starting pose    
+    camera = cameras{i};
     
     pts3d.pts{i} = cylinderSampleProjection(camera, imageSize, cylinders);
     pts3d.camera{i} = camera;
    
-    for j = 1:length(pts3d.pts{i}.Z)
+    if ~initialized
-        graph.add(GenericProjectionFactorCal3_S2(pts3d.pts{i}.Z{j}, ...
+        graph.add(PriorFactorPose3(symbol('x', 1), camera.pose, posePriorNoise));
-            measurementNoise, symbol('x', i), symbol('p', j), camera.K) );
+        k = 0;
+        if ~isempty(pts3d.pts{i}.data{1+k})
+            graph.add(PriorFactorPoint3(symbol('p', 1), ...
+                pts3d.pts{i}.data{1+k}, pointPriorNoise));
+        else
+            k = k+1;
+        end
+        initialized = true;
+    end
     
-        point_j = pts3d.pts{i}.data{j}.retract(0.1*randn(3,1));
+    for j = 1:length(pts3d.pts{i}.Z)
-        initialEstimate.insert(symbol('p', j), point_j);
+        if isempty(pts3d.pts{i}.Z{j})
+            continue;
+        end
+        graph.add(GenericProjectionFactorCal3_S2(pts3d.pts{i}.Z{j}, ...
+            measurementNoise, symbol('x', i), symbol('p', j), camera.calibration) );    
     end
 
+end
+
+%% initialize cameras and points close to ground truth 
+for i = 1:cameraPosesNum
     pose_i = camera.pose.retract(0.1*randn(6,1));
-    initialEstimate.insert(symbole('x', i), pose_i);
+    initialEstimate.insert(symbol('x', i), pose_i);    
-    
+end
+ptsIdx = 0;
+for i = 1:length(cylinders)
+    for j = 1:length(cylinders{i}.Points)
+        ptsIdx = ptsIdx + 1;
+        point_j = cylinders{i}.Points{j}.retract(0.1*randn(3,1));
+        initialEstimate.insert(symbol('p', ptsIdx), point_j);
+    end
 end
 
 %% Print the graph
@@ -55,12 +78,12 @@ graph.print(sprintf('\nFactor graph:\n'));
 marginals = Marginals(graph, initialEstimate);
 
 %% get all the 2d points track information
+% currently throws the Indeterminant linear system exception
 ptIdx = 0;
 for i = 1:pointsNum
    if isempty(pts3d.pts{i})
        continue;
    end
-   %pts2dTrackMono.pts2d = pts3d.pts{i}
    pts2dTracksMono.cov{ptIdx} = marginals.marginalCovariance(symbol('p',i));
 end