random sample cylinders and plot them on the fields

matlab/+gtsam/plotCylinderSamples.m

@@ -0,0 +1,31 @@
+function plotCylinderSamples(cylinders, fieldSize)
+
+    holdstate = ishold;
+    hold on
+    
+    num = size(cylinders, 1);
+
+    sampleDensity = 120;
+    figure
+
+    for i = 1:num        
+        %base.z = cylinders{i}.centroid.z - cylinders{i}.height/2;        
+        [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height);
+        
+        X = X + cylinders{i}.centroid.x;
+        Y = Y + cylinders{i}.centroid.y;
+        Z = Z * cylinders{i}.height;
+        
+        cylinderHandle = surf(X,Y,Z);
+        set(cylinderHandle, 'FaceAlpha', 0.5);
+        hold on
+    end
+    
+    axis equal
+    axis([0, fieldSize.x, 0, fieldSize.y, 0, 20]);
+        
+    if ~holdstate
+        hold off
+    end
+    
+end

matlab/gtsam_examples/CameraFlyingExample.m

@@ -2,27 +2,33 @@ clear all;
 clc;
 import gtsam.*
 
-cylinder_num = 10;
+cylinder_num = 20;
 cylinders = cell(cylinder_num, 1);
 
 % generate a set of cylinders
+fieldSize = Point2([100, 100]');
+
+% random generate cylinders on the fields
 for i = 1:cylinder_num
-    cylinder_center = Point2([10, 5 * i]');
-    cylinders{i,1} = cylinderSampling(cylinder_center, 1, 5, 30);
+    baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]');
+    cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 30);
 end
 
+% plot all the cylinders and sampled points
+% now is plotting on a 100 * 100 field
+plotCylinderSamples(cylinders, fieldSize);
+
 % visibility validation
-%camera_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),y_shift);
 
 K = Cal3_S2(525,525,0,320,240);
-cam_pose = Pose3();
-cam = SimpleCamera(cam_pose, K);
+cameraPose = Pose3();             % now set to be default
 
 % the projections of all visible 3D points
-visiblePoints3 = cylinderSampleProjection(cam, cam_pose, cylinders);
-
-% 
-
-% 
+% visiblePoints3 = cylinderSampleProjection(K, cameraPose, cylinders);
+
+
+
+
+ 
 
 

matlab/gtsam_examples/cylinderSampleProjection.m

@@ -1,17 +1,40 @@
-function [] = cylinderSampleProjection(Cam, Pose3, cylinders)
+function [] = cylinderSampleProjection(K, cameraPose, imageSize, cylinders)
+% Project sampled points on cylinder to camera frame
+% Authors: Zhaoyang Lv
 
     cylinder_num = size(cylinders, 1);
+   
+    camera = SimpleCamera(cameraPose, K);
+        
     for i = 1:cylinder_num
-        cylinder = cylinders{i};
 
-        point_num = size(cylinder.Points, 1);
-        % to be finished
+        point_num = size( cylinders{i}.Points, 1);
         
-        %         for j = 1:point_num
-%             
-%             cylinderPoints = 
-%             
-%         end
+        % to check point visibility        
+        for j = 1:point_num
+            sampledPoint3 = cylinders{i}.Poinsts{j};
+            measurements2d = camera.project(sampledPoint3);
+            
+            % ignore points not visible in the scene
+            if measurements2d.x < 0 || measurements.x >= imageSize.x ...
+                    || measurements2d.y < 0 || measurements.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 = sampledPoint3 - cameraPose.t;
+                rayCameraToCylinder = cylinders{i} - cameraPose.t;
+                
+                projectedRay = dot(rayCameraToPoint, rayCameraToCylinder);
+                distCameraToCylinder = norm(rayCameraToCylinder);
+                
+                
+            end
+        end
         
     end
     

matlab/gtsam_examples/cylinderSampling.m

@@ -1,22 +1,25 @@
-function [cylinder] = cylinderSampling(Point2, radius, height, density)
+function [cylinder] = cylinderSampling(baseCentroid, radius, height, density)
 %
     import gtsam.*
     % calculate the cylinder area
-    A = 2 * pi * radius * height;
+    area = 2 * pi * radius * height;
     
-    PointsNum = round(A * density);
+    pointsNum = round(area * density);
     
-    Points3 = cell(PointsNum, 1);
+    points3 = cell(pointsNum, 1);
     
     % sample the points
-    for i = 1:PointsNum
+    for i = 1:pointsNum
         theta = 2 * pi * rand;
-        x = radius * cos(theta) + Point2.x;
-        y = radius * sin(theta) + Point2.y;
+        x = radius * cos(theta) + baseCentroid.x;
+        y = radius * sin(theta) + baseCentroid.y;
         z = height * rand;
-        Points3{i,1} = Point3([x,y,z]');
+        points3{i,1} = Point3([x,y,z]');
     end
    
-    cylinder.area = A;
-    cylinder.Points = Points3;
+    cylinder.area = area;
+    cylinder.radius = radius;
+    cylinder.height = height;
+    cylinder.Points = points3;
+    cylinder.centroid = Point3(baseCentroid.x, baseCentroid.y, height/2);
 end