diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m index eb8369e42..890f174b7 100644 --- a/matlab/+gtsam/cylinderSampleProjection.m +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -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 diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 0001261ef..0f209bb43 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -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 diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index f899eb3b3..4b379d7a4 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -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