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

add in simulated camera options

release/4.3a0
lvzhaoyang 2015-01-19 16:18:18 -05:00
parent d42391d28d
commit b202bbd5f1
5 changed files with 160 additions and 150 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
matlab/+gtsam/plotFlyingResults.m Normal file
View File

@ -0,0 +1,41 @@
function plotFlyingResults(pts3d, covariance, values, marginals)
% plot the visible points on the cylinders and trajectories
% author: Zhaoyang Lv
import gtsam.*
haveMarginals = exist('marginals', 'var');
keys = KeyVector(values.keys);
holdstate = ishold;
hold on
keys = KeyVector(values.keys);
%% plot trajectories
for i = 0:keys.size - 1
if exist('h_result', 'var')
delete(h_result);
end
key = keys.at(i);
pose = keys.at(key);
P = marginals.marginalCovariance(key);
h_result = gtsam.plotPose3(pose, P, 1);
end
%% plot point covariance
if exist('h_result', 'var')
delete(h_result);
end
if ~holdstate
hold off
end
end

42
matlab/+gtsam/plotProjectedCylinderSamples.m
View File

@ -1,42 +0,0 @@
function plotProjectedCylinderSamples(pts3d, covariance, options, figID)
% plot the visible points on the cylinders
% author: Zhaoyang Lv
import gtsam.*
figure(figID);
holdstate = ishold;
hold on
pointsNum = length(pts3d);
for i = 1:pointsNum
%gtsam.plotPoint3(p, 'g', covariance{i});
plotPoint3(pts3d{i}, 'r', 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
end

12
matlab/+gtsam/points2DTrackMonocular.m
View File

@ -101,20 +101,8 @@ for i = 1:pointsNum
end end
end end
% for k = 1:cameraPosesNum
% num = length(pts3d{k}.data);
% for i = 1:num
% pts2dTracksMono.pt3d{i} = pts3d{k}.data{i};
% pts2dTracksMono.Z{i} = pts3d{k}.Z{i};
% pts2dTracksMono.cov{i} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{visiblePointIdx}));
% end
% end
%% plot the result with covariance ellipses
hold on;
%plot3DPoints(initialEstimate, [], marginals); %plot3DPoints(initialEstimate, [], marginals);
%plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); %plot3DTrajectory(initialEstimate, '*', 1, 8, marginals);
plot3DTrajectory(initialEstimate, '*', 1, 8);
view(3); view(3);

22
matlab/+gtsam/points2DTrackStereo.m
View File

@ -1,4 +1,4 @@
function pts2dTracksStereo = points2DTrackStereo(K, cameraPoses, imageSize, cylinders) function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(K, cameraPoses, imageSize, cylinders)
% Assess how accurately we can reconstruct points from a particular monocular camera setup. % Assess how accurately we can reconstruct points from a particular monocular camera setup.
% After creation of the factor graph for each track, linearize it around ground truth. % After creation of the factor graph for each track, linearize it around ground truth.
% There is no optimization % There is no optimization
@ -100,24 +100,10 @@ for i = 1:pointsNum
end end
% for k = 1:cameraPosesNum
% if isempty(pts3d{k}.data)
% continue;
% end
%
% for i = 1:length(pts3d{k}.data)
% pts2dTracksStereo.pt3d{end+1} = pts3d{k}.data{i};
% pts2dTracksStereo.Z{end+1} = pts3d{k}.Z{i};
% pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{i}));
% end
% end
%% plot the result with covariance ellipses %% plot the result with covariance ellipses
hold on; plotFlyingResults(pts2dTracksStereo.pts3d, pts2dTracksStereo.cov, initialiEstimate, marginals);
%plot3DPoints(initialEstimate, [], marginals);
%plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); %plot3DTrajectory(initialEstimate, '*', 1, 8, marginals);
plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); %view(3);
view(3);
end end

193
matlab/gtsam_examples/CameraFlyingExample.m
View File

@ -10,100 +10,135 @@
% @author Zhaoyang Lv % @author Zhaoyang Lv
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all; clear all;
clc; clc;
clf; clf;
import gtsam.* import gtsam.*
%% define the options % test or run
% the testing field size options.enableTests = false;
options.fieldSize = Point2([100, 100]');
% the number of cylinders
options.cylinderNum = 20;
% point density on cylinder
options.density = 1;
% The number of camera poses % the number of cylinders in the field
options.poseNum = 20; options.cylinder.cylinderNum = 15; % pls be smaller than 20
% covariance scaling factor % cylinder size
options.cylinder.radius = 3; % pls be smaller than 5
options.cylinder.height = 10;
% point density on cylinder
options.cylinder.pointDensity = 1;
%% set up the camera
% parameters set according to the stereo camera:
% http://www.matrix-vision.com/USB2.0-single-board-camera-mvbluefox-mlc.html
% set up monocular camera or stereo camera
options.camera.IS_MONO = false;
% the field of view of camera
options.camera.fov = 120;
% fps for image
options.camera.fps = 25;
% camera pixel resolution
options.camera.resolution = Point2(752, 480);
% camera horizon
options.camera.horizon = 60;
% camera baseline
options.camera.baseline = 0.05;
% camera focal length
options.camera.f = round(options.camera.resolution.x * options.camera.horizon / ...
options.camera.fov);
% camera focal baseline
options.camera.fB = options.camera.f * options.camera.baseline;
% camera disparity
options.camera.disparity = options.camera.fB / options.camera.horizon;
% Monocular Camera Calibration
options.camera.monoK = Cal3_S2(options.camera.f, options.camera.f, 0, ...
options.camera.resolution.x/2, options.camera.resolution.y/2);
% Stereo Camera Calibration
options.camera.stereoK = Cal3_S2Stereo(options.camera.f, options.camera.f, 0, ...
options.camera.resolution.x/2, options.camera.resolution.y/2, options.camera.disparity);
% write video output
options.writeVideo = true;
% the testing field size (unit: meter)
options.fieldSize = Point2([100, 100]');
% camera flying speed (unit: meter / second)
options.speed = 20;
% number of camera poses in the simulated trajectory
options.poseNum = options.fieldSize.x / (options.speed / options.camera.fps);
% display covariance scaling factor
options.scale = 1; options.scale = 1;
% if(options.writeVideo)
%% Camera Setup % videoObj = VideoWriter('Camera_Flying_Example.avi');
% Monocular Camera Calibration % videoObj.Quality = 100;
options.monoK = Cal3_S2(525,525,0,320,240); % videoObj.FrameRate = options.fps;
% Stereo Camera Calibration % end
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 = false;
% fps for image
options.fps = 20;
% camera flying speed
options.speed = 20;
%% test1: visibility test in monocular camera %% This is for tests
cylinders{1}.centroid = Point3(30, 50, 5); if options.enableTests
cylinders{2}.centroid = Point3(50, 50, 5); % test1: visibility test in monocular camera
cylinders{3}.centroid = Point3(70, 50, 5); cylinders{1}.centroid = Point3(30, 50, 5);
cylinders{2}.centroid = Point3(50, 50, 5);
cylinders{3}.centroid = Point3(70, 50, 5);
for i = 1:3 for i = 1:3
cylinders{i}.radius = 5; cylinders{i}.radius = 5;
cylinders{i}.height = 10; cylinders{i}.height = 10;
cylinders{i}.Points{1} = cylinders{i}.centroid.compose(Point3(-cylinders{i}.radius, 0, 0)); cylinders{i}.Points{1} = cylinders{i}.centroid.compose(Point3(-cylinders{i}.radius, 0, 0));
cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0)); cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0));
end
camera = SimpleCamera.Lookat(Point3(10, 50, 10), ...
Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
Point3([0,0,1]'), options.monoK);
pose = camera.pose;
prjMonoResult = cylinderSampleProjection(options.camera.monoK, pose, ...
options.camera.resolution, cylinders);
% test2: visibility test in stereo camera
prjStereoResult = cylinderSampleProjectionStereo(options.camera.stereoK, ...
pose, options.camera.resolution, cylinders);
end end
camera = SimpleCamera.Lookat(Point3(10, 50, 10), ... %% generate a set of cylinders and point samples on cylinders
Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... cylinderNum = options.cylinder.cylinderNum;
Point3([0,0,1]'), options.monoK);
pose = camera.pose;
prjMonoResult = cylinderSampleProjection(options.monoK, pose, options.imageSize, cylinders);
%% test2: visibility test in stereo camera
prjStereoResult = cylinderSampleProjectionStereo(options.stereoK, pose, options.imageSize, cylinders);
%% generate a set of cylinders and samples
cylinderNum = options.cylinderNum;
cylinders = cell(cylinderNum, 1); cylinders = cell(cylinderNum, 1);
baseCentroid = cell(cylinderNum, 1);
% It seems random generated cylinders doesn't work that well
% Now it set up a circle of cylinders
theta = 0; theta = 0;
for i = 1:cylinderNum i = 1;
while i <= cylinderNum
theta = theta + 2*pi/10; theta = theta + 2*pi/10;
x = 30 * rand * cos(theta) + options.fieldSize.x/2; x = 40 * rand * cos(theta) + options.fieldSize.x/2;
y = 20 * rand * sin(theta) + options.fieldSize.y/2; y = 20 * rand * sin(theta) + options.fieldSize.y/2;
baseCentroid = Point2([x, y]'); baseCentroid{i} = Point2([x, y]');
cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, options.density);
% prevent two cylinders interact with each other
regenerate = false;
for j = 1:i-1
if i > 1 && baseCentroid{i}.dist(baseCentroid{j}) < options.cylinder.radius * 2
regenerate = true;
break;
end
end
if regenerate
continue;
end
cylinders{i,1} = cylinderSampling(baseCentroid{i}, options.cylinder.radius, ...
options.cylinder.height, options.cylinder.pointDensity);
i = i+1;
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, figID); plotCylinderSamples(cylinders, options, figID);
% %% 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 %% 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);
@ -113,24 +148,22 @@ for i = 1:options.poseNum
15, 10]'); 15, 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.camera.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.camera.IS_MONO
% use Monocular Camera % use Monocular Camera
pts2dTracksMono = points2DTrackMonocular(options.monoK, cameraPoses, ... pts2dTracksMono = points2DTrackMonocular(options.camera.monoK, cameraPoses, ...
options.imageSize, cylinders); options.camera.resolution, cylinders);
else else
% use Stereo Camera % use Stereo Camera
pts2dTracksStereo = points2DTrackStereo(options.stereoK, cameraPoses, ... [pts2dTracksStereo, estimateValuesStereo] = points2DTrackStereo(options.camera.stereoK, ...
options.imageSize, cylinders); cameraPoses, options.camera.resolution, cylinders);
figID = 2; plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, estimateValuesStereo, options, figID);
plotProjectedCylinderSamples(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, options, figID);
end end
%% plot all the projected points %% plot all the projected points
@ -142,6 +175,10 @@ end
%plot3DTrajectory(); %plot3DTrajectory();
%%close video
% if(options.writeVideo)
% close(videoObj);
% end