From 10dc767eda622637d565b6df010b4000b671f31d Mon Sep 17 00:00:00 2001
From: lvzhaoyang <lvzhaoyang1990@gmail.com>
Date: Mon, 12 Jan 2015 23:27:50 -0500
Subject: [PATCH] change monocular set up and add stereo test. Still under test

---
 matlab/gtsam_examples/CameraFlyingExample.m | 34 ++++++++++++++++-----
 1 file changed, 27 insertions(+), 7 deletions(-)

diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m
index 13dcbfbe4..258bd968b 100644
--- a/matlab/gtsam_examples/CameraFlyingExample.m
+++ b/matlab/gtsam_examples/CameraFlyingExample.m
@@ -1,12 +1,15 @@
 clear all;
 clc;
+clf;
 import gtsam.*
 
 %% generate a set of cylinders and Samples
 fieldSize = Point2([100, 100]');
-cylinder_num = 10;
-cylinders = cell(cylinder_num, 1);
+cylinderNum = 10;
+cylinders = cell(cylinderNum, 1);
 
+% ToDo: it seems random generated cylinders doesn't work that well
+% use fixed parameters instead
 for i = 1:cylinderNum
     baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]');
     cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1);
@@ -18,7 +21,6 @@ figID = 1;
 figure(figID);
 plotCylinderSamples(cylinders, fieldSize, figID);
 
-
 %% generate camera trajectories
 K = Cal3_S2(525,525,0,320,240);
 imageSize = Point2([640, 480]');
@@ -28,7 +30,8 @@ trans = Point3();
 % 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, ...
+    cylinderIdx = max(min(round(cylinderNum*rand), 10), 1);
+    cameras{i} = SimpleCamera.Lookat(trans, cylinders{cylinderIdx}.centroid, ...
         Point3([0,0,1]'), K);
     
     incT = Point3(5*rand, 5*rand, 5*rand);
@@ -36,17 +39,34 @@ for i = 1:poseNum
 end
 
 %% visibility validation
-visiblePoints3 = cylinderSampleProjection(camera, imageSize, cylinders);
+% for a simple test, it will be removed later
+visiblePoints3 = cylinderSampleProjection(cameras{1}, imageSize, cylinders);
 
 %% plot all the projected points
 %plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID);
 
 %% setp up monocular camera and get measurements
-pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders);
+%pts2dTracksMono = points2DTrackMonocular(cameras, imageSize, cylinders);
 
 %% set up stereo camera and get measurements
-%pts2dTracksStereo = points2DTrackStereo(K, cameraPoses, imageSize, cylinders);
+% load stereo calibration
+calib = dlmread(findExampleDataFile('VO_calibration.txt'));
+KStereo = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6));
+poseNum = 10;
+camerasStereo = cell(poseNum, 1);
+trans = Point3();
+for i = 1:poseNum
+    cylinderIdx = max(min(round(cylinderNum*rand), 10), 1);
+    camerasStereo{i} = SimpleCamera.Lookat(trans, cylinders{cylinderIdx}.centroid, ...
+        Point3([0,0,1]'), KStereo);
+    
+    incT = Point3(5*rand, 5*rand, 5*rand);
+    trans = trans.compose(incT);    
+end
 
+pts2dTracksStereo = points2DTrackStereo(camerasStereo, imageSize, cylinders);
+
+% plot the 2D tracks
 
 % ToDo: plot the trajectories
 %plot3DTrajectory();