12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

to make a straight line trajectory

release/4.3a0
lvzhaoyang 2015-01-14 23:36:15 -05:00
parent 4b0075a2f4
commit 3cb1f96371
1 changed files with 53 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
matlab/gtsam_examples/CameraFlyingExample.m
View File

@ -19,11 +19,17 @@ import gtsam.*
% the testing field size % the testing field size
options.fieldSize = Point2([100, 100]'); options.fieldSize = Point2([100, 100]');
% the number of cylinders % the number of cylinders
options.cylinderNum = 10; options.cylinderNum = 20;
% point density on cylinder % point density on cylinder
options.density = 1; options.density = 1;
% The number of camera poses % The number of camera poses
options.poseNum = 20; options.poseNum = 20;
% covariance scaling factor
options.scale = 1;
%% Camera Setup
% Monocular Camera Calibration % Monocular Camera Calibration
options.monoK = Cal3_S2(525,525,0,320,240); options.monoK = Cal3_S2(525,525,0,320,240);
% Stereo Camera Calibration % Stereo Camera Calibration
@ -31,7 +37,13 @@ options.stereoK = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
% the image size of camera % the image size of camera
options.imageSize = Point2([640, 480]'); options.imageSize = Point2([640, 480]');
% use Monocular camera or Stereo camera % use Monocular camera or Stereo camera
options.Mono = false; options.Mono = true;
% fps for image
options.fps = 20;
% camera flying speed
options.speed = 20;
%% test1: visibility test in monocular camera %% test1: visibility test in monocular camera
@ -57,7 +69,7 @@ prjMonoResult = cylinderSampleProjection(options.monoK, pose, options.imageSize,
%% test2: visibility test in stereo camera %% test2: visibility test in stereo camera
prjStereoResult = cylinderSampleProjectionStereo(options.stereoK, pose, options.imageSize, cylinders); prjStereoResult = cylinderSampleProjectionStereo(options.stereoK, pose, options.imageSize, cylinders);
%% generate a set of cylinders and Samples %% generate a set of cylinders and samples
cylinderNum = options.cylinderNum; cylinderNum = options.cylinderNum;
cylinders = cell(cylinderNum, 1); cylinders = cell(cylinderNum, 1);
@ -65,34 +77,49 @@ cylinders = cell(cylinderNum, 1);
% Now it set up a circle of cylinders % Now it set up a circle of cylinders
theta = 0; theta = 0;
for i = 1:cylinderNum for i = 1:cylinderNum
theta = theta + 2*pi / 10; theta = theta + 2*pi/10;
x = 10 * cos(theta) + options.fieldSize.x/2; x = 30 * rand * cos(theta) + options.fieldSize.x/2;
y = 10 * sin(theta) + options.fieldSize.y/2; y = 20 * rand * sin(theta) + options.fieldSize.y/2;
baseCentroid = Point2([x, y]'); baseCentroid = Point2([x, y]');
cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, options.density); cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, options.density);
end end
%% plot all the cylinders and sampled points %% plot all the cylinders and sampled points
% now is plotting on a 100 * 100 field % now is plotting on a 100 * 100 field
figID = 1; figID = 1;
figure(figID); figure(figID);
plotCylinderSamples(cylinders, options.fieldSize, figID); plotCylinderSamples(cylinders, options.fieldSize, figID);
%% generate ground truth camera trajectories: a circle % %% generate ground truth camera trajectories: a circle
% KMono = Cal3_S2(525,525,0,320,240);
% cameraPoses = cell(options.poseNum, 1);
% theta = 0;
% r = 40;
% for i = 1:options.poseNum
% theta = (i-1)*2*pi/options.poseNum;
% t = Point3([r*cos(theta) + options.fieldSize.x/2, ...
% r*sin(theta) + options.fieldSize.y/2, 10]');
% camera = SimpleCamera.Lookat(t, ...
% Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
% Point3([0,0,1]'), options.monoK);
% cameraPoses{i} = camera.pose;
% end
%% generate ground truth camera trajectories: a line
KMono = Cal3_S2(525,525,0,320,240); KMono = Cal3_S2(525,525,0,320,240);
cameraPoses = cell(options.poseNum, 1); cameraPoses = cell(options.poseNum, 1);
theta = 0; theta = 0;
r = 40;
for i = 1:options.poseNum for i = 1:options.poseNum
theta = (i-1)*2*pi/options.poseNum; t = Point3([(i-1)*(options.fieldSize.x - 20)/options.poseNum + 20, ...
t = Point3([r*cos(theta) + options.fieldSize.x/2, ... 15, 10]');
r*sin(theta) + options.fieldSize.y/2, 10]');
camera = SimpleCamera.Lookat(t, ... camera = SimpleCamera.Lookat(t, ...
Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
Point3([0,0,1]'), options.monoK); Point3([0,0,1]'), options.monoK);
cameraPoses{i} = camera.pose; cameraPoses{i} = camera.pose;
end end
%% set up camera and get measurements %% set up camera and get measurements
if options.Mono if options.Mono
% use Monocular Camera % use Monocular Camera
@ -102,9 +129,24 @@ else
% use Stereo Camera % use Stereo Camera
pts2dTracksStereo = points2DTrackStereo(options.stereoK, cameraPoses, ... pts2dTracksStereo = points2DTrackStereo(options.stereoK, cameraPoses, ...
options.imageSize, cylinders); options.imageSize, cylinders);
figID = 2;
figure(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 end
%% plot all the projected points %% plot all the projected points
%plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); %plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID);
% plot the 2D tracks % plot the 2D tracks