function3 add graph measurement and initial estimate

matlab/+gtsam/cylinderSampleProjection.m

@@ -1,64 +1,83 @@
-function [visiblePoints, visiblePointsCylinderIdx] = cylinderSampleProjection(K, cameraPose, imageSize, cylinders)
+function [visiblePoints] = cylinderSampleProjection(camera, imageSize, cylinders)
-% Project sampled points on cylinder to camera frame
-% Authors: Zhaoyang Lv
 
-    import gtsam.*
+% Input: 
+% Output:
+%   visiblePoints:  data{k} 3D Point in overal point clouds with index k 
+%                   Z{k}    2D measurements in overal point clouds with index k   
+%                   index {i}{j}
+%                   i: the cylinder index;
+%                   j: the point index on the cylinder;
+%                   
+% @Description: Project sampled points on cylinder to camera frame
+% @Authors: Zhaoyang Lv
 
-    cylinder_num = size(cylinders, 1);
+import gtsam.*
-   
-    %camera = SimpleCamera(cameraPose, K);
-    camera = SimpleCamera.Lookat(cameraPose.translation(), cylinders{10}.centroid, Point3([0,0,1]'), K);
 
-    visiblePoints = {};
+%% memory allocation
-    visiblePointsCylinderIdx = [];
+cylinderNum = length(cylinders);
+visiblePoints.index = cell(cylinderNum);
+
+pointCloudNum = 0;
+for i = 1:cylinderNum
+    pointCloudNum = pointCloudNum + length(cylinders{i}.Points);
+    visiblePoints.index{i} = cell(pointCloudNum);
+end
+visiblePoints.data = cell(pointCloudNum);
+
+%% check visiblity of points on each cylinder
+pointCloudIndex = 0;
+for i = 1:cylinderNum
     
-    for i = 1:cylinder_num
+    pointNum = length(cylinders{i}.Points);
 
-        point_num = size( cylinders{i}.Points, 1);
+    % to check point visibility        
+    for j = 1:pointNum
+
+        pointCloudIndex  = pointCloudIndex + 1;
         
-        % to check point visibility        
+        sampledPoint3 = cylinders{i}.Points{j};
-        for j = 1:point_num
+        Z2d = camera.project(sampledPoint3);
-            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
+        % ignore points not visible in the scene
-                projectedRay = dot(rayCameraToCylinder, rayCameraToPoint);
+        if Z2d.x < 0 || Z2d.x >= imageSize.x ...
-                if projectedRay > 0
+                || Z2d.y < 0 || Z2d.y >= imageSize.y 
-                    rayCylinderToProjected = norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
+            continue;       
-                    if rayCylinderToProjected(1) > cylinders{i}.radius && ...
+        end            
-                            rayCylinderToProjected(2) > cylinders{i}.radius
+
-                        visiblePoints{end+1} = sampledPoint3;
+        % ignore points occluded
-                        visiblePointsCylinderIdx = [visiblePointsCylinderIdx, i];
+        % use a simple math hack to check occlusion:
-                    end
+        %   1. All points in front of cylinders' surfaces are visible
-                end
+        %   2. For points behind the cylinders' surfaces, the cylinder
-                
+        for k = 1:cylinderNum
+
+            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.data{pointCloudIndex} = sampledPoint3;
+                visiblePoints.Z{pointCloudIndex} = Z2d;
+                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} = sampledPoints3;
+                    visiblePoints.Z{pointCloudIndex} = Z2d;
+                    visiblePoints.index{i}{j} = pointCloudIndex;
+                end
+            end
+
         end
-        
     end
+
+end
     
 end

matlab/+gtsam/points2DTrackMonocular.m

@@ -1,4 +1,4 @@
-function pts2dTracksmon = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders)
+function pts2dTracksMono = points2DTrackMonocular(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
@@ -9,32 +9,59 @@ import gtsam.*
 %% create graph
 graph = NonlinearFactorGraph;
 
-%% add a constraint on the starting pose
+pointNoiseSigma = 0.1;
 poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
+
+%% add a constraint on the starting pose
 posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
 firstPose = cameraPoses{1};
 graph.add(PriorFactorPose3(symbol('x', l), firstPose, posePriorNoise));
 
-cameraPosesNum = size(cameraPoses, 1);
+cameraPosesNum = length(cameraPoses);
 
-%% add measurements
+%% add measurements and initial camera & points values
+pointsNum = 0;
+cylinderNum = length(cylinders);
+for i = 1:cylinderNum
+    pointsNum = pointsNum + length(cylinders{i}.Points);
+end
+
+measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma);
+
+pts3d = {};
 initialEstimate = Values;
 for i = 1:cameraPosesNum
-    [visiblePoints3, visiblePointsCylinderIdx] = cylinderSampleProjection(K, cameraPoses{i}, imageSize, cylinders);
+    camera = SimpleCamera(K, cameraPoses{i});
     
-    pointsNum = size(visiblePoints, 1);
+    pts3d.pts{i} = cylinderSampleProjection(camera, imageSize, cylinders);
+    pts3d.camera{i} = camera;
+   
+    for j = 1:length(pts3d.pts{i}.Z)
+        graph.add(GenericProjectionFactorCal3_S2(pts3d.pts{i}.Z{j}, ...
+            measurementNoise, symbol('x', i), symbol('p', j), camera.K) );
+    
+        point_j = pts3d.pts{i}.data{j}.retract(0.1*randn(3,1));
+        initialEstimate.insert(symbol('p', j), point_j);
+    end
+
+    pose_i = camera.pose.retract(0.1*randn(6,1));
+    initialEstimate.insert(symbole('x', i), pose_i);
     
-    %% not finished
-    %for j = 1:pointsNum
-    %    graph.add();
-    %end   
 end
 
+%% Print the graph
+graph.print(sprintf('\nFactor graph:\n'));
+
 marginals = Marginals(graph, initialEstimate);
 
-% should use all the points num to replace the num 100
+%% get all the 2d points track information
-for i = 1:100 
+ptIdx = 0;
-   marginals.marginalCovariance(symbol('p',i));
+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
 
 end

matlab/gtsam_examples/CameraFlyingExample.m

@@ -2,47 +2,49 @@ clear all;
 clc;
 import gtsam.*
 
+%% generate a set of cylinders and Samples
+fieldSize = Point2([100, 100]');
 cylinder_num = 10;
 cylinders = cell(cylinder_num, 1);
 
-% generate a set of cylinders
+for i = 1:cylinderNum
-fieldSize = Point2([100, 100]');
-
-% random generate cylinders on the fields
-for i = 1:cylinder_num
     baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]');
     cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1);
 end
 
-% plot all the cylinders and sampled points
+%% plot all the cylinders and sampled points
 % now is plotting on a 100 * 100 field
 figID = 1;
 figure(figID);
 plotCylinderSamples(cylinders, fieldSize, figID);
 
-% visibility validation
 
-% generate camera trajectories
+%% generate camera trajectories
 K = Cal3_S2(525,525,0,320,240);
 imageSize = Point2([640, 480]');
 poseNum = 10;
-cameraPoses = cell(poseNum, 1);
-cameraPoses{1} = Pose3();
 cameras = cell(poseNum, 1);
-for i = 2:poseNum
+trans = Point3();
-    incRot = Rot3.RzRyRx(0,0,pi/4);
+% To ensure there are landmarks in view, look at one randomly chosen cylinder 
+% each time.
+for i = 1:poseNum
+    camera = SimpleCamera.Lookat(trans, cylinders{round(cylinderNum*rand)}.centroid, ...
+        Point3([0,0,1]'), K);
+    
     incT = Point3(5*rand, 5*rand, 5*rand);
-    cameraPoses{i} = cameraPoses{i-1}.compose(Pose3(incRot, incT));    
+    trans = trans.compose(incT);    
 end
 
-[visiblePoints3, ~] = cylinderSampleProjection(K, cameraPoses{1}, imageSize, cylinders);
+%% visibility validation
+visiblePoints3 = cylinderSampleProjection(camera, imageSize, cylinders);
 
-plotPose3(cameraPoses{1}, 5 )
+%% plot all the projected points
-% plot all the projected points
+%plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID);
-plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID);
 
+%% setp up monocular camera and get measurements
 pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders);
 
+%% set up stereo camera and get measurements
 %pts2dTracksStereo = points2DTrackStereo(K, cameraPoses, imageSize, cylinders);
 
 

matlab/gtsam_examples/cylinderSampleProjection.m

@@ -1,41 +0,0 @@
-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
-
-        point_num = size( cylinders{i}.Points, 1);
-        
-        % 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
-    
-end

matlab/gtsam_examples/cylinderSampling.m

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

matlab/unstable_examples/.gitignore

@@ -1 +1,2 @@
 *.m~
+*.avi