Stereo camera visibility tests passed

matlab/+gtsam/cylinderSampleProjectionStereo.m

@@ -33,10 +33,10 @@ for i = 1:cylinderNum
         end
         
         % measurements 
-        Z.du = K.fx() * K.baseline() / samplePoint3.z;  
+        Z.du = K.fx() * K.baseline() / sampledPoint3local.z;  
-        Z.uL = K.fx() * samplePoint3.x / samplePoint3.z + K.px();
+        Z.uL = K.fx() * sampledPoint3local.x / sampledPoint3local.z + K.px();
-        Z.uR = uL + du;
+        Z.uR = Z.uL + Z.du;
-        Z.v = K.fy() / samplePoint3.z + K.py();
+        Z.v = K.fy() / sampledPoint3local.z + K.py();
 
         % ignore points not visible in the scene
         if Z.uL < 0 || Z.uL >= imageSize.x || ...
@@ -49,35 +49,40 @@ for i = 1:cylinderNum
         % 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
+        visible = true;
         for k = 1:cylinderNum
 
             rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
-            rayCameraToCylinder = pose.translation().between(cylinders{i}.centroid).vector();
+            rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
-            rayCylinderToPoint = cylinders{i}.centroid.between(sampledPoint3).vector();
+            rayCylinderToPoint = cylinders{k}.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;
+            else 
+                projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder);
+                if projectedRay > 0
+                    %rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
+                    if rayCylinderToPoint(1) > cylinders{i}.radius && ...
+                            rayCylinderToPoint(2) > cylinders{i}.radius
+                       continue;
+                    else
+                        visible = false;
+                        break;
+                    end
+                end
             end
             
-            % Condition 2
+        end
-            projectedRay = dot(rayCameraToCylinder, rayCameraToPoint);
+        
-            if projectedRay > 0
+        if visible
-                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
 

matlab/+gtsam/points2DTrackStereo.m

@@ -10,12 +10,8 @@ import gtsam.*
 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;
 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(cameraPoses);
@@ -31,15 +27,15 @@ pts3d = cell(cameraPosesNum, 1);
 initialEstimate = Values;
 initialized = false;
 for i = 1:cameraPosesNum 
-    pts3d{i} = cylinderSampleProjectionStereo(K, cameraPose, imageSize, cylinders);
+    pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, imageSize, cylinders);
     
     if ~initialized
-        graph.add(PriorFactorPose3(symbol('x', 1), camera.pose, posePriorNoise));
+        graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{i}, posePriorNoise));
         initialized = true;
     end
     
-    for j = 1:length(pts3d.pts{i}.Z)
+    for j = 1:length(pts3d{i}.Z)
-        if isempty(pts3d.pts{i}.Z{j})
+        if isempty(pts3d{i}.Z{j})
             continue;
         end
         graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ...
@@ -49,7 +45,7 @@ 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;
@@ -69,18 +65,20 @@ marginals = Marginals(graph, initialEstimate);
 %% get all the 2d points track information
 % currently throws the Indeterminant linear system exception
 ptx = 1;
-for i = 1:length(cylinders)
+for k = 1:cameraPosesNum
+    for i = 1:length(cylinders)
         for j = 1:length(cylinders{i}.Points)
             if isempty(pts3d{k}.index{i}{j})
                 continue;
             end
             idx = pts3d{k}.index{i}{j};
-        pts2dTracksMono.pt3d{ptx} = pts3d{k}.data{idx};
+            pts2dTracksStereo.pt3d{ptx} = pts3d{k}.data{idx};
-        pts2dTracksMono.Z{ptx} = pts3d{k}.Z{idx};
+            pts2dTracksStereo.Z{ptx} = pts3d{k}.Z{idx};
-        pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',idx));
+            pts2dTracksStereo.cov{ptx} = marginals.marginalCovariance(symbol('p',idx));
 
             ptx = ptx + 1;
         end
+    end
 end
 
 end

matlab/gtsam_examples/CameraFlyingExample.m

@@ -31,7 +31,7 @@ options.imageSize = Point2([640, 480]');
 % use Monocular camera or Stereo camera
 options.Mono = false;
 
-%% test1: visibility
+%% test1: visibility test in monocular camera 
 cylinders{1}.centroid = Point3(30, 50, 5);
 cylinders{2}.centroid = Point3(50, 50, 5);
 cylinders{3}.centroid = Point3(70, 50, 5);
@@ -44,16 +44,18 @@ for i = 1:3
     cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0));
 end
 
-camera = SimpleCamera.Lookat(Point3(40, 50, 10), ... 
+camera = SimpleCamera.Lookat(Point3(10, 50, 10), ... 
     Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
     Point3([0,0,1]'), options.monoK); 
 
 pose = camera.pose;
-pts3d = cylinderSampleProjection(options.monoK, pose, options.imageSize, cylinders);
+prjMonoResult = cylinderSampleProjection(options.monoK, pose, options.imageSize, cylinders);
+
+%% test2: visibility test in stereo camera  
+prjStereoResult = cylinderSampleProjectionStereo(options.stereoK, pose, options.imageSize, cylinders);
+
+
 
-figID = 1;
-figure(figID);
-plotCylinderSamples(cylinders, options.fieldSize, figID);
 
 %% generate a set of cylinders and Samples
 cylinderNum = options.cylinderNum;