From 5564aea3328e37e490b0f207d8d2551758d1740c Mon Sep 17 00:00:00 2001
From: lvzhaoyang <lvzhaoyang1990@gmail.com>
Date: Sun, 11 Jan 2015 23:20:37 -0500
Subject: [PATCH] calculate all the visible points from a camera view

---
 matlab/+gtsam/cylinderSampleProjection.m | 64 ++++++++++++++++++++++++
 matlab/+gtsam/plotCylinderSamples.m      | 13 +++--
 2 files changed, 73 insertions(+), 4 deletions(-)
 create mode 100644 matlab/+gtsam/cylinderSampleProjection.m

diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m
new file mode 100644
index 000000000..d74726956
--- /dev/null
+++ b/matlab/+gtsam/cylinderSampleProjection.m
@@ -0,0 +1,64 @@
+function [visiblePoints, visiblePointsCylinderIdx] = cylinderSampleProjection(K, cameraPose, imageSize, cylinders)
+% Project sampled points on cylinder to camera frame
+% Authors: Zhaoyang Lv
+
+    import gtsam.*
+
+    cylinder_num = size(cylinders, 1);
+   
+    %camera = SimpleCamera(cameraPose, K);
+    camera = SimpleCamera.Lookat(cameraPose.translation(), cylinders{10}.centroid, Point3([0,0,1]'), K);
+
+    visiblePoints = {};
+    visiblePointsCylinderIdx = [];
+    
+    for i = 1:cylinder_num
+
+        point_num = size( cylinders{i}.Points, 1);
+        
+        % to check point visibility        
+        for j = 1:point_num
+            sampledPoint3 = cylinders{i}.Points{j};
+            measurements2d = camera.project(sampledPoint3);
+                     
+            % ignore points not visible in the scene
+            if measurements2d.x < 0 || measurements2d.x >= imageSize.x ...
+                    || measurements2d.y < 0 || measurements2d.y >= 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:cylinder_num
+                
+                rayCameraToPoint = cameraPose.translation().between(sampledPoint3).vector();
+                rayCameraToCylinder = cameraPose.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{end+1} = sampledPoint3;
+                    visiblePointsCylinderIdx = [visiblePointsCylinderIdx, i];
+                    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{end+1} = sampledPoint3;
+                        visiblePointsCylinderIdx = [visiblePointsCylinderIdx, i];
+                    end
+                end
+                
+            end
+        end
+        
+    end
+    
+end
diff --git a/matlab/+gtsam/plotCylinderSamples.m b/matlab/+gtsam/plotCylinderSamples.m
index d1fe981db..6c8a2249e 100644
--- a/matlab/+gtsam/plotCylinderSamples.m
+++ b/matlab/+gtsam/plotCylinderSamples.m
@@ -1,4 +1,8 @@
-function plotCylinderSamples(cylinders, fieldSize)
+function plotCylinderSamples(cylinders, fieldSize, figID)
+% plot the cylinders on the given field
+% @author: Zhaoyang Lv
+
+    figure(figID);
 
     holdstate = ishold;
     hold on
@@ -6,10 +10,9 @@ function plotCylinderSamples(cylinders, fieldSize)
     num = size(cylinders, 1);
 
     sampleDensity = 120;
-    figure
+    
 
-    for i = 1:num        
-        %base.z = cylinders{i}.centroid.z - cylinders{i}.height/2;        
+    for i = 1:num                
         [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height);
         
         X = X + cylinders{i}.centroid.x;
@@ -24,6 +27,8 @@ function plotCylinderSamples(cylinders, fieldSize)
     axis equal
     axis([0, fieldSize.x, 0, fieldSize.y, 0, 20]);
         
+    grid on
+    
     if ~holdstate
         hold off
     end