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

test visibility. monocular camera visibility tests passed

release/4.3a0
lvzhaoyang 2015-01-13 23:25:44 -05:00
parent da06689677
commit 4a5d94ea59
3 changed files with 90 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
matlab/+gtsam/cylinderSampleProjection.m
View File

@ -38,16 +38,16 @@ for i = 1:cylinderNum
pointCloudIndex = pointCloudIndex + 1;
% Cheirality Exception
sampledPoint3 = cylinders{i}.Points{j};
sampledPoint3local = camera.pose.transform_to(sampledPoint3);
if sampledPoint3local.z < 0
continue; % Cheirality Exception
sampledPoint3local = pose.transform_to(sampledPoint3);
if sampledPoint3local.z <= 0
continue;
end
Z2d = camera.project(sampledPoint3);
% ignore points not visible in the scene
if Z2d.x < 0 || Z2d.x >= imageSize.x ...
|| Z2d.y < 0 || Z2d.y >= imageSize.y
if Z2d.x < 0 || Z2d.x >= imageSize.x || Z2d.y < 0 || Z2d.y >= imageSize.y
continue;
end
@ -55,36 +55,40 @@ for i = 1:cylinderNum
% use a simple math hack to check occlusion:
% 1. All points in front of cylinders' surfaces are visible
% 2. For points behind the cylinders' surfaces, the cylinder
visible = true;
for k = 1:cylinderNum
rayCameraToPoint = camera.pose.translation().between(sampledPoint3).vector();
rayCameraToCylinder = camera.pose.translation().between(cylinders{i}.centroid).vector();
rayCylinderToPoint = cylinders{i}.centroid.between(sampledPoint3).vector();
rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
% Condition 1: all points in front of the cylinders'
% surfaces are visible
if dot(rayCylinderToPoint, rayCameraToCylinder) < 0
visiblePoints.data{pointCloudIndex} = sampledPoint3;
visiblePoints.Z{pointCloudIndex} = Z2d;
visiblePoints.index{i}{j} = pointCloudIndex;
continue;
end
% Condition 2
projectedRay = dot(rayCameraToCylinder, rayCameraToPoint);
if projectedRay > 0
rayCylinderToProjected = norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
if rayCylinderToProjected(1) > cylinders{i}.radius && ...
rayCylinderToProjected(2) > cylinders{i}.radius
visiblePoints.data{pointCloudIndex} = sampledPoint3;
visiblePoints.Z{pointCloudIndex} = Z2d;
visiblePoints.index{i}{j} = pointCloudIndex;
continue;
else
projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder);
if projectedRay > 0
%rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
if rayCylinderToPoint(1) > cylinders{i}.radius && ...
rayCylinderToPoint(2) > cylinders{i}.radius
continue;
else
visible = false;
break;
end
end
end
end
if visible
visiblePoints.data{pointCloudIndex} = sampledPoint3;
visiblePoints.Z{pointCloudIndex} = Z2d;
visiblePoints.index{i}{j} = pointCloudIndex;
end
end
end
end

16
matlab/+gtsam/points2DTrackMonocular.m
View File

@ -10,11 +10,10 @@ import gtsam.*
graph = NonlinearFactorGraph;
%% create the noise factors
pointNoiseSigma = 0.1;
poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
measurementNoiseSigma = 1.0;
posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
pointPriorNoise = noiseModel.Isotropic.Sigma(3, pointNoiseSigma);
measurementNoiseSigma = 1.0;
measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma);
cameraPosesNum = length(cameraPoses);
@ -37,13 +36,6 @@ for i = 1:cameraPosesNum
if ~initialized
graph.add(PriorFactorPose3(symbol('x', 1), cameraPose, posePriorNoise));
k = 0;
if ~isempty(pts3d{i}.data{1+k})
graph.add(PriorFactorPoint3(symbol('p', 1), ...
pts3d{i}.data{1+k}, pointPriorNoise));
else
k = k+1;
end
initialized = true;
end
@ -74,7 +66,7 @@ end
%% Print the graph
graph.print(sprintf('\nFactor graph:\n'));
%marginals = Marginals(graph, initialEstimate);
marginals = Marginals(graph, initialEstimate);
%% get all the points track information
% currently throws the Indeterminant linear system exception
@ -89,7 +81,7 @@ for k = 1:cameraPosesNum
idx = pts3d{k}.index{i}{j};
pts2dTracksMono.pt3d{ptx} = pts3d{k}.data{idx};
pts2dTracksMono.Z{ptx} = pts3d{k}.Z{idx};
%pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',idx));
pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',idx));
ptx = ptx + 1;
end

76
matlab/gtsam_examples/CameraFlyingExample.m
View File

@ -1,15 +1,59 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
% Atlanta, Georgia 30332-0415
% All Rights Reserved
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
%
% See LICENSE for the license information
%
% @brief A camera flying example through a field of cylinder landmarks
% @author Zhaoyang Lv
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all;
clc;
clf;
import gtsam.*
%% define the options
% the testing field size
options.fieldSize = Point2([100, 100]');
% the number of cylinders
options.cylinderNum = 10;
% The number of camera poses
options.poseNum = 20;
% Monocular Camera Calibration
options.monoK = Cal3_S2(525,525,0,320,240);
options.stereoK = Cal3_S2Stereo(721,721,0.0,609,172,0.53715); % read from the VO calibration file
% Stereo Camera Calibration
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;
%% test1: visibility
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
cylinders{i}.radius = 5;
cylinders{i}.height = 10;
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));
end
camera = SimpleCamera.Lookat(Point3(40, 50, 10), ...
Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
Point3([0,0,1]'), options.monoK);
pose = camera.pose;
pts3d = cylinderSampleProjection(options.monoK, pose, options.imageSize, cylinders);
figID = 1;
figure(figID);
plotCylinderSamples(cylinders, options.fieldSize, figID);
%% generate a set of cylinders and Samples
cylinderNum = options.cylinderNum;
@ -32,11 +76,9 @@ figID = 1;
figure(figID);
plotCylinderSamples(cylinders, options.fieldSize, figID);
%% generate camera trajectories: a circle
%% generate ground truth camera trajectories: a circle
KMono = Cal3_S2(525,525,0,320,240);
imageSize = Point2([640, 480]');
cameraPoses = cell(options.poseNum, 1);
% Generate ground truth trajectory r.w.t. the field center
theta = 0;
r = 40;
for i = 1:options.poseNum
@ -45,27 +87,23 @@ for i = 1:options.poseNum
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]'), KMono);
Point3([0,0,1]'), options.monoK);
cameraPoses{i} = camera.pose;
end
%% visibility validation
% 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(KMono, cameraPoses, imageSize, cylinders);
%% set up stereo camera and get measurements
% load stereo calibration
calib = dlmread(findExampleDataFile('VO_calibration.txt'));
KStereo = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6));
camerasStereo = cell(options.poseNum, 1);
%pts2dTracksStereo = points2DTrackStereo(KStereo, cameraPoses, imageSize, cylinders);
%% set up camera and get measurements
if options.Mono
% use Monocular Camera
pts2dTracksMono = points2DTrackMonocular(options.monoK, cameraPoses, ...
options.imageSize, cylinders);
else
% use Stereo Camera
pts2dTracksStereo = points2DTrackStereo(options.stereoK, cameraPoses, ...
options.imageSize, cylinders);
end
% plot the 2D tracks