diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m index 4a8c3b06a..eb8369e42 100644 --- a/matlab/+gtsam/cylinderSampleProjection.m +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -1,4 +1,4 @@ -function [visiblePoints] = cylinderSampleProjection(camera, imageSize, cylinders) +function [visiblePoints] = cylinderSampleProjection(K, pose, imageSize, cylinders) % Input: % Output: @@ -13,6 +13,8 @@ function [visiblePoints] = cylinderSampleProjection(camera, imageSize, cylinders import gtsam.* +camera = SimpleCamera(pose, K); + %% memory allocation cylinderNum = length(cylinders); visiblePoints.index = cell(cylinderNum,1); diff --git a/matlab/+gtsam/plotCylinderSamples.m b/matlab/+gtsam/plotCylinderSamples.m index 6c8a2249e..74768f641 100644 --- a/matlab/+gtsam/plotCylinderSamples.m +++ b/matlab/+gtsam/plotCylinderSamples.m @@ -11,7 +11,6 @@ function plotCylinderSamples(cylinders, fieldSize, figID) sampleDensity = 120; - for i = 1:num [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height); diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 184408f26..0001261ef 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -1,4 +1,4 @@ -function pts2dTracksMono = points2DTrackMonocular(cameras, imageSize, cylinders) +function pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders) % 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. % There is no optimization @@ -17,7 +17,7 @@ posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); pointPriorNoise = noiseModel.Isotropic.Sigma(3, pointNoiseSigma); measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); -cameraPosesNum = length(cameras); +cameraPosesNum = length(cameraPoses); %% add measurements and initial camera & points values pointsNum = 0; @@ -26,41 +26,40 @@ for i = 1:cylinderNum pointsNum = pointsNum + length(cylinders{i}.Points); end -pts3d = {}; +pts3d = cell(cameraPosesNum, 1); initialEstimate = Values; initialized = false; for i = 1:cameraPosesNum % add a constraint on the starting pose - camera = cameras{i}; + cameraPose = cameraPoses{i}; - pts3d.pts{i} = cylinderSampleProjection(camera, imageSize, cylinders); - pts3d.camera{i} = camera; + pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders); if ~initialized - graph.add(PriorFactorPose3(symbol('x', 1), camera.pose, posePriorNoise)); + graph.add(PriorFactorPose3(symbol('x', 1), cameraPose, posePriorNoise)); k = 0; - if ~isempty(pts3d.pts{i}.data{1+k}) + if ~isempty(pts3d{i}.data{1+k}) graph.add(PriorFactorPoint3(symbol('p', 1), ... - pts3d.pts{i}.data{1+k}, pointPriorNoise)); + pts3d{i}.data{1+k}, pointPriorNoise)); else k = k+1; end initialized = true; end - for j = 1:length(pts3d.pts{i}.Z) - if isempty(pts3d.pts{i}.Z{j}) + for j = 1:length(pts3d{i}.Z) + if isempty(pts3d{i}.Z{j}) continue; end - graph.add(GenericProjectionFactorCal3_S2(pts3d.pts{i}.Z{j}, ... - measurementNoise, symbol('x', i), symbol('p', j), camera.calibration) ); + graph.add(GenericProjectionFactorCal3_S2(pts3d{i}.Z{j}, ... + measurementNoise, symbol('x', i), symbol('p', j), K) ); end end %% initialize cameras and points close to ground truth for i = 1:cameraPosesNum - pose_i = camera.pose.retract(0.1*randn(6,1)); + pose_i = cameraPoses{i}.retract(0.1*randn(6,1)); initialEstimate.insert(symbol('x', i), pose_i); end ptsIdx = 0; @@ -75,24 +74,24 @@ 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 -ptx = 0; +ptx = 1; for k = 1:cameraPosesNum for i = 1:length(cylinders) for j = 1:length(cylinders{i}.Points) - if isempty(pts3d.pts{k}.index{i}{j}) + if isempty(pts3d{k}.index{i}{j}) continue; end + 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)); + ptx = ptx + 1; - - idx = pts3d.pts{k}.index{i}{j}; - pts2dTracksMono.pt3d{ptx} = pts3d.pts{k}.data{idx}; - pts2dTracksMono.Z{ptx} = pts3d.pts{k}.Z{idx}; - pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',idx)); end end @@ -100,7 +99,10 @@ end %% plot the result with covariance ellipses hold on; -plot3DPoints(initialEstimate, [], marginals); -plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); +%plot3DPoints(initialEstimate, [], marginals); +%plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); +plot3DTrajectory(initialEstimate, '*', 1, 8); +view(3); + end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 1eb301b38..f899eb3b3 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -20,8 +20,8 @@ cylinders = cell(cylinderNum, 1); theta = 0; for i = 1:cylinderNum theta = theta + 2*pi / 10; - x = 20 * cos(theta) + options.fieldSize.x/2; - y = 20 * sin(theta) + options.fieldSize.y/2; + x = 10 * cos(theta) + options.fieldSize.x/2; + y = 10 * sin(theta) + options.fieldSize.y/2; baseCentroid = Point2([x, y]'); cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1); end @@ -32,46 +32,40 @@ figID = 1; figure(figID); plotCylinderSamples(cylinders, options.fieldSize, figID); -%% generate camera trajectories -K = Cal3_S2(525,525,0,320,240); +%% generate camera trajectories: a circle +KMono = Cal3_S2(525,525,0,320,240); imageSize = Point2([640, 480]'); -cameras = cell(options.poseNum, 1); +cameraPoses = cell(options.poseNum, 1); % Generate ground truth trajectory r.w.t. the field center theta = 0; -r = 30; +r = 40; for i = 1:options.poseNum theta = (i-1)*2*pi/options.poseNum; - t = Point3([30*cos(theta), 30*sin(theta), 10]'); - cameras{i} = SimpleCamera.Lookat(t, ... + 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]'), K); + Point3([0,0,1]'), KMono); + cameraPoses{i} = camera.pose; end %% visibility validation % for a simple test, it will be removed later -visiblePoints3 = cylinderSampleProjection(cameras{1}, imageSize, cylinders); +%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(cameras, imageSize, cylinders); +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); -% for i = 1:options.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); +%pts2dTracksStereo = points2DTrackStereo(KStereo, cameraPoses, imageSize, cylinders); % plot the 2D tracks