plot the points covariance

matlab/+gtsam/cylinderSampleProjectionStereo.m

@@ -5,6 +5,11 @@ import gtsam.*
 %% memory allocation
 cylinderNum = length(cylinders);
 
+visiblePoints.data = cell(1);
+visiblePoints.Z = cell(1);
+visiblePoints.cylinderIdx = cell(1);
+visiblePoints.overallIdx = cell(1);
+
 %% check visiblity of points on each cylinder
 pointCloudIndex = 0;
 visiblePointIdx = 1;

matlab/+gtsam/plotProjectedCylinderSamples.m

@@ -1,4 +1,4 @@
-function plotProjectedCylinderSamples(visiblePoints3, cameraPose, figID)
+function plotProjectedCylinderSamples(pts3d, covariance, figID)
 % plot the visible projected points on the cylinders
 % author: Zhaoyang Lv
 
@@ -9,26 +9,31 @@ function plotProjectedCylinderSamples(visiblePoints3, cameraPose, figID)
     holdstate = ishold;
     hold on
 
-    %plotCamera(cameraPose, 5);
+    pointsNum = length(pts3d);
-    
-    pointsNum = size(visiblePoints3, 1)
-    
     for i = 1:pointsNum
-        ray = visiblePoints3{i}.between(cameraPose.translation()).vector();
+        plotPoint3(pts3d{i}, 'green', covariance{i}); 
-        dist = norm(ray);
+        hold on
-        
-        p = plot3(visiblePoints3{i}.x, visiblePoints3{i}.y, visiblePoints3{i}.z, ...
-            'o', 'MarkerFaceColor', 'Green');
-        
-        for t=0:0.1:dist
-            marchingRay = ray * t;
-            p.XData = visiblePoints3{i}.x + marchingRay(1);
-            p.YData = visiblePoints3{i}.y + marchingRay(2);
-            p.ZData = visiblePoints3{i}.z + marchingRay(3);
-            drawnow update
     end
     
-    end
+%     for i=1:pointsNum
+%         ray = pts2dTracksStereo{i}.between(cameraPose.translation()).vector();
+%         dist = norm(ray);
+%         
+%         p = plot3(pts2dTracksStereo{i}.x, pts2dTracksStereo{i}.y, pts2dTracksStereo{i}.z, ...
+%             'o', 'MarkerFaceColor', 'Green');
+%         
+%         for t=0:0.1:dist
+%             marchingRay = ray * t;
+%             p.XData = pts2dTracksStereo{i}.x + marchingRay(1);
+%             p.YData = pts2dTracksStereo{i}.y + marchingRay(2);
+%             p.ZData = pts2dTracksStereo{i}.z + marchingRay(3);
+%             drawnow update
+%         end
+%         
+%     end
+
+    axis equal;
+    axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]);
     
     if ~holdstate
         hold off

matlab/+gtsam/points2DTrackStereo.m

@@ -12,7 +12,7 @@ graph = NonlinearFactorGraph;
 %% create the noise factors
 poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
 posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
-stereoNoise = noiseModel.Isotropic.Sigma(3,1);
+stereoNoise = noiseModel.Isotropic.Sigma(3,0.2);
 
 cameraPosesNum = length(cameraPoses);
 
@@ -35,6 +35,9 @@ for i = 1:cameraPosesNum
     end
     
     measurementNum = length(pts3d{i}.Z);
+    if measurementNum < 1
+        continue;
+    end
     for j = 1:measurementNum
         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));    
@@ -64,6 +67,9 @@ marginals = Marginals(graph, initialEstimate);
 % currently throws the Indeterminant linear system exception
 for k = 1:cameraPosesNum
     num = length(pts3d{k}.data);
+    if num < 1
+        continue;
+    end
     for i = 1:num
         pts2dTracksStereo.pt3d{i} = pts3d{k}.data{i};
         pts2dTracksStereo.Z{i} = pts3d{k}.Z{i};

matlab/gtsam_examples/CameraFlyingExample.m

@@ -37,15 +37,13 @@ options.stereoK = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
 % the image size of camera
 options.imageSize = Point2([640, 480]');
 % use Monocular camera or Stereo camera
-options.Mono = true;
+options.Mono = false;
 % fps for image
 options.fps = 20;
 % camera flying speed
 options.speed = 20;
 
 
-
-
 %% test1: visibility test in monocular camera 
 cylinders{1}.centroid = Point3(30, 50, 5);
 cylinders{2}.centroid = Point3(50, 50, 5);
@@ -131,23 +129,12 @@ else
         options.imageSize, cylinders);
     
     figID = 2;
-    figure(figID)
+    plotProjectedCylinderSamples(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, figID);
 
-    axis equal;
-    axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]);
-    
-    ptsSize = length(pts2dTracksStereo.pt3d{i});
-    for i = 1:ptsSize
-        plotPoint3(pts2dTracksStereo.pt3d{i}, 'red', pts2dTracksStereo.cov{i}); 
-        hold on
-    end
-    
-    hold off
 end
 
 %% plot all the projected points
 
-%plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID);
 
 % plot the 2D tracks