From 27b3b5ebedbc5c73f3efd9e70e3707763db0ccab Mon Sep 17 00:00:00 2001
From: lvzhaoyang <lvzhaoyang1990@gmail.com>
Date: Thu, 15 Jan 2015 00:20:06 -0500
Subject: [PATCH] plot the points covariance

---
 .../+gtsam/cylinderSampleProjectionStereo.m   |  5 +++
 matlab/+gtsam/plotProjectedCylinderSamples.m  | 45 ++++++++++---------
 matlab/+gtsam/points2DTrackStereo.m           |  8 +++-
 matlab/gtsam_examples/CameraFlyingExample.m   | 17 +------
 4 files changed, 39 insertions(+), 36 deletions(-)

diff --git a/matlab/+gtsam/cylinderSampleProjectionStereo.m b/matlab/+gtsam/cylinderSampleProjectionStereo.m
index ae02c879c..51cda12ac 100644
--- a/matlab/+gtsam/cylinderSampleProjectionStereo.m
+++ b/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;
diff --git a/matlab/+gtsam/plotProjectedCylinderSamples.m b/matlab/+gtsam/plotProjectedCylinderSamples.m
index 5d9a06713..a58526632 100644
--- a/matlab/+gtsam/plotProjectedCylinderSamples.m
+++ b/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,27 +9,32 @@ function plotProjectedCylinderSamples(visiblePoints3, cameraPose, figID)
     holdstate = ishold;
     hold on
 
-    %plotCamera(cameraPose, 5);
-    
-    pointsNum = size(visiblePoints3, 1)
-    
-    for i=1:pointsNum
-        ray = visiblePoints3{i}.between(cameraPose.translation()).vector();
-        dist = norm(ray);
-        
-        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
-        
+    pointsNum = length(pts3d);
+    for i = 1:pointsNum
+        plotPoint3(pts3d{i}, 'green', covariance{i}); 
+        hold on
     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
     end
diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m
index a65276e38..141596cea 100644
--- a/matlab/+gtsam/points2DTrackStereo.m
+++ b/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};
diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m
index 727374288..eb047b50a 100644
--- a/matlab/gtsam_examples/CameraFlyingExample.m
+++ b/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