update the stereo model and occlusion detection

matlab/+gtsam/cylinderSampleProjectionStereo.m

@@ -0,0 +1,84 @@
+function [visiblePoints] = cylinderSampleProjectionStereo(K, pose, imageSize, cylinders)
+
+import gtsam.*
+
+%% memory allocation
+cylinderNum = length(cylinders);
+visiblePoints.index = cell(cylinderNum,1);
+
+pointCloudNum = 0;
+for i = 1:cylinderNum
+    pointCloudNum = pointCloudNum + length(cylinders{i}.Points);
+    visiblePoints.index{i} = cell(pointCloudNum,1);
+end
+visiblePoints.data = cell(pointCloudNum,1);
+visiblePoints.Z = cell(pointCloudNum, 1);
+
+%% check visiblity of points on each cylinder
+pointCloudIndex = 0;
+for i = 1:cylinderNum
+    
+    pointNum = length(cylinders{i}.Points);
+
+    % to check point visibility        
+    for j = 1:pointNum
+
+        pointCloudIndex  = pointCloudIndex + 1;
+                
+        % For Cheirality Exception
+        sampledPoint3 = cylinders{i}.Points{j};
+        sampledPoint3local = pose.transform_to(sampledPoint3);
+        if sampledPoint3local.z < 0
+            continue; 
+        end
+        
+        % measurements 
+        Z.du = K.fx() * K.baseline() / samplePoint3.z;  
+        Z.uL = K.fx() * samplePoint3.x / samplePoint3.z + K.px();
+        Z.uR = uL + du;
+        Z.v = K.fy() / samplePoint3.z + K.py();
+
+        % ignore points not visible in the scene
+        if Z.uL < 0 || Z.uL >= imageSize.x || ...
+                Z.uR < 0 || Z.uR >= imageSize.x || ...
+                Z.v < 0 || Z.v >= imageSize.y 
+            continue;       
+        end            
+
+        % ignore points occluded
+        % use a simple math hack to check occlusion:
+        %   1. All points in front of cylinders' surfaces are visible
+        %   2. For points behind the cylinders' surfaces, the cylinder
+        for k = 1:cylinderNum
+
+            rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
+            rayCameraToCylinder = pose.translation().between(cylinders{i}.centroid).vector();
+            rayCylinderToPoint = cylinders{i}.centroid.between(sampledPoint3).vector();
+
+            % Condition 1: all points in front of the cylinders'
+            % surfaces are visible
+            if dot(rayCylinderToPoint, rayCameraToCylinder) < 0
+                visiblePoints.data{pointCloudIndex} = sampledPoint3;
+                visiblePoints.Z{pointCloudIndex} = Z;
+                visiblePoints.index{i}{j} = pointCloudIndex; 
+                continue;
+            end
+
+            % Condition 2
+            projectedRay = dot(rayCameraToCylinder, rayCameraToPoint);
+            if projectedRay > 0
+                rayCylinderToProjected = norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
+                if rayCylinderToProjected(1) > cylinders{i}.radius && ...
+                        rayCylinderToProjected(2) > cylinders{i}.radius
+                    visiblePoints.data{pointCloudIndex} = sampledPoint3;
+                    visiblePoints.Z{pointCloudIndex} = Z;
+                    visiblePoints.index{i}{j} = pointCloudIndex;
+                end
+            end
+
+        end
+    end
+
+end
+
+end

matlab/+gtsam/points2DTrackStereo.m

@@ -1,4 +1,4 @@
-function pts2dTracksStereo = points2DTrackStereo(cameras, imageSize, cylinders)
+function pts2dTracksStereo = points2DTrackStereo(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
@@ -16,8 +16,9 @@ measurementNoiseSigma = 1.0;
 posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
 pointPriorNoise = noiseModel.Isotropic.Sigma(3, pointNoiseSigma);
 measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma);
+stereoNoise = noiseModel.Isotropic.Sigma(3,1);
 
-cameraPosesNum = length(cameras);
+cameraPosesNum = length(cameraPoses);
 
 %% add measurements and initial camera & points values
 pointsNum = 0;
@@ -26,25 +27,14 @@ 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    
+    pts3d{i} = cylinderSampleProjectionStereo(K, cameraPose, imageSize, cylinders);
-    camera = cameras{i};
-    
-    pts3d.pts{i} = cylinderSampleProjection(camera, imageSize, cylinders);
-    pts3d.camera{i} = camera;
     
     if ~initialized
         graph.add(PriorFactorPose3(symbol('x', 1), camera.pose, posePriorNoise));
-        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
     
@@ -52,10 +42,9 @@ for i = 1:cameraPosesNum
         if isempty(pts3d.pts{i}.Z{j})
             continue;
         end
-        graph.add(GenericProjectionFactorCal3_S2(pts3d.pts{i}.Z{j}, ...
+        graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ...
-            measurementNoise, symbol('x', i), symbol('p', j), camera.calibration) );    
+            stereoNoise, symbol('x', i), symbol('p', j), K));    
     end
-
 end
 
 %% initialize cameras and points close to ground truth 
@@ -79,12 +68,19 @@ marginals = Marginals(graph, initialEstimate);
 
 %% get all the 2d points track information
 % currently throws the Indeterminant linear system exception
-ptIdx = 0;
+ptx = 1;
-for i = 1:pointsNum
+for i = 1:length(cylinders)
-   if isempty(pts3d.pts{i})
+    for j = 1:length(cylinders{i}.Points)
+        if isempty(pts3d{k}.index{i}{j})
             continue;
         end
-   pts2dTracksMono.cov{ptIdx} = marginals.marginalCovariance(symbol('p',i));
+        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;
+    end
 end
 
 end