From 86f580b9ae3cd6c560e077a62ef1ed737883fb92 Mon Sep 17 00:00:00 2001
From: lvzhaoyang <lvzhaoyang1990@gmail.com>
Date: Wed, 14 Jan 2015 00:19:17 -0500
Subject: [PATCH] stereo view works fine. but monocular camera still sufferes
 from the indeterminant system problem

---
 matlab/+gtsam/points2DTrackStereo.m         | 16 +++++++++++++---
 matlab/gtsam_examples/CameraFlyingExample.m |  7 +++++--
 2 files changed, 18 insertions(+), 5 deletions(-)

diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m
index 0a27cf90b..a65276e38 100644
--- a/matlab/+gtsam/points2DTrackStereo.m
+++ b/matlab/+gtsam/points2DTrackStereo.m
@@ -63,9 +63,19 @@ marginals = Marginals(graph, initialEstimate);
 %% get all the 2d points track information
 % currently throws the Indeterminant linear system exception
 for k = 1:cameraPosesNum
-    pts2dTracksStereo.pt3d{ptx} = pts3d{k}.data{idx};
-    pts2dTracksStereo.Z{ptx} = pts3d{k}.Z{idx};
-    pts2dTracksStereo.cov{ptx} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{visiblePointIdx}));
+    num = length(pts3d{k}.data);
+    for i = 1:num
+        pts2dTracksStereo.pt3d{i} = pts3d{k}.data{i};
+        pts2dTracksStereo.Z{i} = pts3d{k}.Z{i};
+        pts2dTracksStereo.cov{i} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{i}));
+    end
 end
 
+%% plot the result with covariance ellipses
+hold on;
+%plot3DPoints(initialEstimate, [], marginals);
+%plot3DTrajectory(initialEstimate, '*', 1, 8, marginals);
+plot3DTrajectory(initialEstimate, '*', 1, 8, marginals);
+view(3);
+
 end
diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m
index 1e5981614..c86c8cb83 100644
--- a/matlab/gtsam_examples/CameraFlyingExample.m
+++ b/matlab/gtsam_examples/CameraFlyingExample.m
@@ -20,6 +20,8 @@ import gtsam.*
 options.fieldSize = Point2([100, 100]');
 % the number of cylinders
 options.cylinderNum = 10;
+% point density on cylinder
+options.density = 1;
 % The number of camera poses
 options.poseNum = 20;
 % Monocular Camera Calibration
@@ -29,7 +31,8 @@ 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;
+
 
 %% test1: visibility test in monocular camera 
 cylinders{1}.centroid = Point3(30, 50, 5);
@@ -66,7 +69,7 @@ for i = 1:cylinderNum
     x = 10 * cos(theta) + options.fieldSize.x/2;
     y = 10 * sin(theta) + options.fieldSize.y/2;
     baseCentroid = Point2([x, y]');
-    cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1);
+    cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, options.density);
 end
 
 %% plot all the cylinders and sampled points