remove dulipcate points in stereo camera set up

matlab/+gtsam/plotCylinderSamples.m

@@ -1,4 +1,4 @@
-function plotCylinderSamples(cylinders, fieldSize, figID)
+function plotCylinderSamples(cylinders, options, figID)
 % plot the cylinders on the given field
 % @author: Zhaoyang Lv
 
@@ -24,7 +24,7 @@ function plotCylinderSamples(cylinders, fieldSize, figID)
     end
     
     axis equal
-    axis([0, fieldSize.x, 0, fieldSize.y, 0, 20]);
+    axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]);
         
     grid on
     

matlab/+gtsam/plotProjectedCylinderSamples.m

@@ -1,5 +1,5 @@
-function plotProjectedCylinderSamples(pts3d, covariance, figID)
+function plotProjectedCylinderSamples(pts3d, covariance, options, figID)
-% plot the visible projected points on the cylinders
+% plot the visible points on the cylinders
 % author: Zhaoyang Lv
 
     import gtsam.*
@@ -11,7 +11,8 @@ function plotProjectedCylinderSamples(pts3d, covariance, figID)
 
     pointsNum = length(pts3d);
     for i = 1:pointsNum
-        plotPoint3(pts3d{i}, 'green', covariance{i}); 
+        %gtsam.plotPoint3(p, 'g', covariance{i});
+        plotPoint3(pts3d{i}, 'r', covariance{i}); 
         hold on
     end
     

matlab/+gtsam/points2DTrackMonocular.m

@@ -59,6 +59,13 @@ for i = 1:cameraPosesNum
 end
 
 for i = 1:pointsNum
+    % add in values
+    point_j = points3d{i}.data.retract(0.1*randn(3,1));
+    initialEstimate.insert(symbol('p', i), point_j); 
+    
+    if ~points3d{i}.visiblity 
+        continue;
+    end
     % single measurement. not added to graph
     factorNum = length(points3d{i}.Z);
     if factorNum > 1
@@ -68,41 +75,40 @@ for i = 1:pointsNum
                 measurementNoise, symbol('x', cameraIdx), symbol('p', points3d{i}.cameraConstraint{j}), K) );    
         end
     end
-   
+  
-    % add in values
-    point_j = points3d{i}.data.retract(0.1*randn(3,1));
-    initialEstimate.insert(symbol('p', i), point_j);
-    
 end
 
 %% Print the graph
 graph.print(sprintf('\nFactor graph:\n'));
 
+%% linearize the graph
+% currently throws the Indeterminant linear system exception
 marginals = Marginals(graph, initialEstimate);
 
 %% get all the points track information
-% currently throws the Indeterminant linear system exception
 for i = 1:pointsNum
-    if points3d{i}.visiblity
+    if ~points3d{i}.visiblity
-        pts2dTracksMono.pt3d{i} = points3d{i}.data;
+        continue;
-        pts2dTracksMono.Z = points3d{i}.Z;
+    end
+    
+    pts2dTracksMono.pt3d{end+1} = points3d{i}.data;
+    pts2dTracksMono.Z{end+1} = points3d{i}.Z;
 
-        if length(points3d{i}.Z) == 1
+    if length(points3d{i}.Z) == 1
-            %pts2dTracksMono.cov{i} singular matrix 
+        %pts2dTracksMono.cov{i} singular matrix 
-        else 
+    else 
-            pts2dTracksMono.cov{i} = marginals.marginalCovariance(symbol('p', i));    
+        pts2dTracksMono.cov{end+1} = marginals.marginalCovariance(symbol('p', i));    
-        end
     end
 end
 
-for k = 1:cameraPosesNum
+% for k = 1:cameraPosesNum
-    num = length(pts3d{k}.data);
+%     num = length(pts3d{k}.data);
-    for i = 1:num
+%     for i = 1:num
-        pts2dTracksMono.pt3d{i} = pts3d{k}.data{i};
+%         pts2dTracksMono.pt3d{i} = pts3d{k}.data{i};
-        pts2dTracksMono.Z{i} = pts3d{k}.Z{i};
+%         pts2dTracksMono.Z{i} = pts3d{k}.Z{i};
-        pts2dTracksMono.cov{i} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{visiblePointIdx}));
+%         pts2dTracksMono.cov{i} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{visiblePointIdx}));
-    end
+%     end
-end
+% end
 
 %% plot the result with covariance ellipses
 hold on;

matlab/+gtsam/points2DTrackStereo.m

@@ -19,64 +19,100 @@ cameraPosesNum = length(cameraPoses);
 %% add measurements and initial camera & points values
 pointsNum = 0;
 cylinderNum = length(cylinders);
+points3d = cell(0);
 for i = 1:cylinderNum
+    cylinderPointsNum = length(cylinders{i}.Points);
     pointsNum = pointsNum + length(cylinders{i}.Points);
+    for j = 1:cylinderPointsNum
+        points3d{end+1}.data = cylinders{i}.Points{j};
+        points3d{end}.Z = cell(0);
+        points3d{end}.cameraConstraint = cell(0);
+        points3d{end}.visiblity = false;
+    end
 end
 
+graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
+
 pts3d = cell(cameraPosesNum, 1);
 initialEstimate = Values;
-initialized = false;
 for i = 1:cameraPosesNum 
     pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, imageSize, cylinders);
     
-    if ~initialized
+    if isempty(pts3d{i}.Z)
-        graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{i}, posePriorNoise));
+        continue;
-        initialized = true;
     end
     
     measurementNum = length(pts3d{i}.Z);
-    if measurementNum < 1
-        continue;
-    end
     for j = 1:measurementNum
+        index = pts3d{i}.overallIdx{j};
+        points3d{index}.Z{end+1} = pts3d{i}.Z{j};
+        points3d{index}.cameraConstraint{end+1} = i;
+        points3d{index}.visiblity = true;
+    end
+    
+    for j = 1:length(pts3d{i}.Z)
         graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ...
             stereoNoise, symbol('x', i), symbol('p', pts3d{i}.overallIdx{j}), K));    
     end
 end
 
-%% initialize cameras and points close to ground truth 
+%% initialize graph and values
 for i = 1:cameraPosesNum
     pose_i = cameraPoses{i}.retract(0.1*randn(6,1));
     initialEstimate.insert(symbol('x', i), pose_i);    
 end
-ptsIdx = 0;
+
-for i = 1:length(cylinders)
+for i = 1:pointsNum
-    for j = 1:length(cylinders{i}.Points)
+    point_j = points3d{i}.data.retract(0.1*randn(3,1));
-        ptsIdx = ptsIdx + 1;
+    initialEstimate.insert(symbol('p', i), point_j);
-        point_j = cylinders{i}.Points{j}.retract(0.1*randn(3,1));
+    
-        initialEstimate.insert(symbol('p', ptsIdx), point_j);
+    if ~points3d{i}.visiblity
+        continue;
     end
+    
+    factorNum = length(points3d{i}.Z);
+    for j = 1:factorNum
+        cameraIdx = points3d{i}.cameraConstraint{j};
+        graph.add(GenericStereoFactor3D(StereoPoint2(points3d{i}.Z{j}.uL, points3d{i}.Z{j}.uR, points3d{i}.Z{j}.v), ...
+            stereoNoise, symbol('x', cameraIdx), symbol('p', points3d{i}.cameraConstraint{j}), K));            
+    end    
 end
 
+
 %% Print the graph
 graph.print(sprintf('\nFactor graph:\n'));
 
+%% linearize the graph
 marginals = Marginals(graph, initialEstimate);
 
 %% get all the 2d points track information
-% currently throws the Indeterminant linear system exception
+pts2dTracksStereo.pt3d = cell(0);
-for k = 1:cameraPosesNum
+pts2dTracksStereo.Z = cell(0);
-    num = length(pts3d{k}.data);
+pts2dTracksStereo.cov = cell(0);
-    if num < 1
+for i = 1:pointsNum
+    if ~points3d{i}.visiblity
         continue;
     end
-    for i = 1:num
+    
-        pts2dTracksStereo.pt3d{i} = pts3d{k}.data{i};
+    pts2dTracksStereo.pt3d{end+1} = points3d{i}.data;
-        pts2dTracksStereo.Z{i} = pts3d{k}.Z{i};
+    pts2dTracksStereo.Z{end+1} = points3d{i}.Z;
-        pts2dTracksStereo.cov{i} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{i}));
+    pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i));    
-    end
+   
 end
 
+
+% for k = 1:cameraPosesNum
+%     if isempty(pts3d{k}.data)
+%         continue;
+%     end
+% 
+%     for i = 1:length(pts3d{k}.data)
+%         pts2dTracksStereo.pt3d{end+1} = pts3d{k}.data{i};
+%         pts2dTracksStereo.Z{end+1} = pts3d{k}.Z{i};
+%         pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{i}));
+%     end
+% end
+
 %% plot the result with covariance ellipses
 hold on;
 %plot3DPoints(initialEstimate, [], marginals);

matlab/gtsam_examples/CameraFlyingExample.m

@@ -87,7 +87,7 @@ end
 % now is plotting on a 100 * 100 field
 figID = 1;
 figure(figID);
-plotCylinderSamples(cylinders, options.fieldSize, figID);
+plotCylinderSamples(cylinders, options, figID);
 
 % %% generate ground truth camera trajectories: a circle 
 % KMono = Cal3_S2(525,525,0,320,240);
@@ -129,7 +129,7 @@ else
         options.imageSize, cylinders);
     
     figID = 2;
-    plotProjectedCylinderSamples(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, figID);
+    plotProjectedCylinderSamples(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, options, figID);
 
 end