From 6732beb1b445c4598ec6d54b254d82a82c1e64d2 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Wed, 7 Jan 2015 09:56:18 -0500 Subject: [PATCH 01/38] a cylinder MATLAB object --- matlab/gtsam_examples/CameraFlyingExample.m | 28 +++++++++++++++++++ .../gtsam_examples/cylinderSampleProjection.m | 18 ++++++++++++ matlab/gtsam_examples/cylinderSampling.m | 22 +++++++++++++++ 3 files changed, 68 insertions(+) create mode 100644 matlab/gtsam_examples/CameraFlyingExample.m create mode 100644 matlab/gtsam_examples/cylinderSampleProjection.m create mode 100644 matlab/gtsam_examples/cylinderSampling.m diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m new file mode 100644 index 000000000..018ffbb4a --- /dev/null +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -0,0 +1,28 @@ +clear all; +clc; +import gtsam.* + +cylinder_num = 10; +cylinders = cell(cylinder_num, 1); + +% generate a set of cylinders +for i = 1:cylinder_num + cylinder_center = Point2([10, 5 * i]'); + cylinders{i,1} = cylinderSampling(cylinder_center, 1, 5, 30); +end + +% visibility validation +%camera_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),y_shift); + +K = Cal3_S2(525,525,0,320,240); +cam_pose = Pose3(); +cam = SimpleCamera(cam_pose, K); + +% the projections of all visible 3D points +visiblePoints3 = cylinderSampleProjection(cam, cam_pose, cylinders); + +% + +% + + diff --git a/matlab/gtsam_examples/cylinderSampleProjection.m b/matlab/gtsam_examples/cylinderSampleProjection.m new file mode 100644 index 000000000..30b7d97c1 --- /dev/null +++ b/matlab/gtsam_examples/cylinderSampleProjection.m @@ -0,0 +1,18 @@ +function [] = cylinderSampleProjection(Cam, Pose3, cylinders) + + cylinder_num = size(cylinders, 1); + for i = 1:cylinder_num + cylinder = cylinders{i}; + + point_num = size(cylinder.Points, 1); + % to be finished + + % for j = 1:point_num +% +% cylinderPoints = +% +% end + + end + +end \ No newline at end of file diff --git a/matlab/gtsam_examples/cylinderSampling.m b/matlab/gtsam_examples/cylinderSampling.m new file mode 100644 index 000000000..3e6a6d9a8 --- /dev/null +++ b/matlab/gtsam_examples/cylinderSampling.m @@ -0,0 +1,22 @@ +function [cylinder] = cylinderSampling(Point2, radius, height, density) +% + import gtsam.* + % calculate the cylinder area + A = 2 * pi * radius * height; + + PointsNum = round(A * density); + + Points3 = cell(PointsNum, 1); + + % sample the points + for i = 1:PointsNum + theta = 2 * pi * rand; + x = radius * cos(theta) + Point2.x; + y = radius * sin(theta) + Point2.y; + z = height * rand; + Points3{i,1} = Point3([x,y,z]'); + end + + cylinder.area = A; + cylinder.Points = Points3; +end \ No newline at end of file From 9485553d99ec23aec0486a311d4184fca95869a3 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Fri, 9 Jan 2015 10:33:53 -0500 Subject: [PATCH 02/38] random sample cylinders and plot them on the fields --- matlab/+gtsam/plotCylinderSamples.m | 31 ++++++++++++ matlab/gtsam_examples/CameraFlyingExample.m | 28 ++++++----- .../gtsam_examples/cylinderSampleProjection.m | 47 ++++++++++++++----- matlab/gtsam_examples/cylinderSampling.m | 23 +++++---- 4 files changed, 96 insertions(+), 33 deletions(-) create mode 100644 matlab/+gtsam/plotCylinderSamples.m diff --git a/matlab/+gtsam/plotCylinderSamples.m b/matlab/+gtsam/plotCylinderSamples.m new file mode 100644 index 000000000..d1fe981db --- /dev/null +++ b/matlab/+gtsam/plotCylinderSamples.m @@ -0,0 +1,31 @@ +function plotCylinderSamples(cylinders, fieldSize) + + holdstate = ishold; + hold on + + num = size(cylinders, 1); + + sampleDensity = 120; + figure + + for i = 1:num + %base.z = cylinders{i}.centroid.z - cylinders{i}.height/2; + [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height); + + X = X + cylinders{i}.centroid.x; + Y = Y + cylinders{i}.centroid.y; + Z = Z * cylinders{i}.height; + + cylinderHandle = surf(X,Y,Z); + set(cylinderHandle, 'FaceAlpha', 0.5); + hold on + end + + axis equal + axis([0, fieldSize.x, 0, fieldSize.y, 0, 20]); + + if ~holdstate + hold off + end + +end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 018ffbb4a..3a2a8991a 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -2,27 +2,33 @@ clear all; clc; import gtsam.* -cylinder_num = 10; +cylinder_num = 20; cylinders = cell(cylinder_num, 1); % generate a set of cylinders +fieldSize = Point2([100, 100]'); + +% random generate cylinders on the fields for i = 1:cylinder_num - cylinder_center = Point2([10, 5 * i]'); - cylinders{i,1} = cylinderSampling(cylinder_center, 1, 5, 30); + baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]'); + cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 30); end +% plot all the cylinders and sampled points +% now is plotting on a 100 * 100 field +plotCylinderSamples(cylinders, fieldSize); + % visibility validation -%camera_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),y_shift); K = Cal3_S2(525,525,0,320,240); -cam_pose = Pose3(); -cam = SimpleCamera(cam_pose, K); +cameraPose = Pose3(); % now set to be default % the projections of all visible 3D points -visiblePoints3 = cylinderSampleProjection(cam, cam_pose, cylinders); - -% - -% +% visiblePoints3 = cylinderSampleProjection(K, cameraPose, cylinders); + + + + + diff --git a/matlab/gtsam_examples/cylinderSampleProjection.m b/matlab/gtsam_examples/cylinderSampleProjection.m index 30b7d97c1..33e031bc8 100644 --- a/matlab/gtsam_examples/cylinderSampleProjection.m +++ b/matlab/gtsam_examples/cylinderSampleProjection.m @@ -1,18 +1,41 @@ -function [] = cylinderSampleProjection(Cam, Pose3, cylinders) - +function [] = cylinderSampleProjection(K, cameraPose, imageSize, cylinders) +% Project sampled points on cylinder to camera frame +% Authors: Zhaoyang Lv + cylinder_num = size(cylinders, 1); + + camera = SimpleCamera(cameraPose, K); + for i = 1:cylinder_num - cylinder = cylinders{i}; + + point_num = size( cylinders{i}.Points, 1); - point_num = size(cylinder.Points, 1); - % to be finished - - % for j = 1:point_num -% -% cylinderPoints = -% -% end + % to check point visibility + for j = 1:point_num + sampledPoint3 = cylinders{i}.Poinsts{j}; + measurements2d = camera.project(sampledPoint3); + + % ignore points not visible in the scene + if measurements2d.x < 0 || measurements.x >= imageSize.x ... + || measurements2d.y < 0 || measurements.y >= imageSize.y + continue; + end + % ignore points occluded + % 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 + for k = 1:cylinder_num + + rayCameraToPoint = sampledPoint3 - cameraPose.t; + rayCameraToCylinder = cylinders{i} - cameraPose.t; + + projectedRay = dot(rayCameraToPoint, rayCameraToCylinder); + distCameraToCylinder = norm(rayCameraToCylinder); + + + end + end end -end \ No newline at end of file +end diff --git a/matlab/gtsam_examples/cylinderSampling.m b/matlab/gtsam_examples/cylinderSampling.m index 3e6a6d9a8..2ee304da1 100644 --- a/matlab/gtsam_examples/cylinderSampling.m +++ b/matlab/gtsam_examples/cylinderSampling.m @@ -1,22 +1,25 @@ -function [cylinder] = cylinderSampling(Point2, radius, height, density) +function [cylinder] = cylinderSampling(baseCentroid, radius, height, density) % import gtsam.* % calculate the cylinder area - A = 2 * pi * radius * height; + area = 2 * pi * radius * height; - PointsNum = round(A * density); + pointsNum = round(area * density); - Points3 = cell(PointsNum, 1); + points3 = cell(pointsNum, 1); % sample the points - for i = 1:PointsNum + for i = 1:pointsNum theta = 2 * pi * rand; - x = radius * cos(theta) + Point2.x; - y = radius * sin(theta) + Point2.y; + x = radius * cos(theta) + baseCentroid.x; + y = radius * sin(theta) + baseCentroid.y; z = height * rand; - Points3{i,1} = Point3([x,y,z]'); + points3{i,1} = Point3([x,y,z]'); end - cylinder.area = A; - cylinder.Points = Points3; + cylinder.area = area; + cylinder.radius = radius; + cylinder.height = height; + cylinder.Points = points3; + cylinder.centroid = Point3(baseCentroid.x, baseCentroid.y, height/2); end \ No newline at end of file From 5564aea3328e37e490b0f207d8d2551758d1740c Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Sun, 11 Jan 2015 23:20:37 -0500 Subject: [PATCH 03/38] calculate all the visible points from a camera view --- matlab/+gtsam/cylinderSampleProjection.m | 64 ++++++++++++++++++++++++ matlab/+gtsam/plotCylinderSamples.m | 13 +++-- 2 files changed, 73 insertions(+), 4 deletions(-) create mode 100644 matlab/+gtsam/cylinderSampleProjection.m diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m new file mode 100644 index 000000000..d74726956 --- /dev/null +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -0,0 +1,64 @@ +function [visiblePoints, visiblePointsCylinderIdx] = cylinderSampleProjection(K, cameraPose, imageSize, cylinders) +% Project sampled points on cylinder to camera frame +% Authors: Zhaoyang Lv + + import gtsam.* + + cylinder_num = size(cylinders, 1); + + %camera = SimpleCamera(cameraPose, K); + camera = SimpleCamera.Lookat(cameraPose.translation(), cylinders{10}.centroid, Point3([0,0,1]'), K); + + visiblePoints = {}; + visiblePointsCylinderIdx = []; + + for i = 1:cylinder_num + + point_num = size( cylinders{i}.Points, 1); + + % to check point visibility + for j = 1:point_num + sampledPoint3 = cylinders{i}.Points{j}; + measurements2d = camera.project(sampledPoint3); + + % ignore points not visible in the scene + if measurements2d.x < 0 || measurements2d.x >= imageSize.x ... + || measurements2d.y < 0 || measurements2d.y >= imageSize.y + continue; + end + + % ignore points occluded + % 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 + for k = 1:cylinder_num + + rayCameraToPoint = cameraPose.translation().between(sampledPoint3).vector(); + rayCameraToCylinder = cameraPose.translation().between(cylinders{i}.centroid).vector(); + rayCylinderToPoint = cylinders{i}.centroid.between(sampledPoint3).vector(); + + % Condition 1: all points in front of the cylinders' + % surfaces are visible + if dot(rayCylinderToPoint, rayCameraToCylinder) < 0 + visiblePoints{end+1} = sampledPoint3; + visiblePointsCylinderIdx = [visiblePointsCylinderIdx, i]; + 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{end+1} = sampledPoint3; + visiblePointsCylinderIdx = [visiblePointsCylinderIdx, i]; + end + end + + end + end + + end + +end diff --git a/matlab/+gtsam/plotCylinderSamples.m b/matlab/+gtsam/plotCylinderSamples.m index d1fe981db..6c8a2249e 100644 --- a/matlab/+gtsam/plotCylinderSamples.m +++ b/matlab/+gtsam/plotCylinderSamples.m @@ -1,4 +1,8 @@ -function plotCylinderSamples(cylinders, fieldSize) +function plotCylinderSamples(cylinders, fieldSize, figID) +% plot the cylinders on the given field +% @author: Zhaoyang Lv + + figure(figID); holdstate = ishold; hold on @@ -6,10 +10,9 @@ function plotCylinderSamples(cylinders, fieldSize) num = size(cylinders, 1); sampleDensity = 120; - figure + - for i = 1:num - %base.z = cylinders{i}.centroid.z - cylinders{i}.height/2; + for i = 1:num [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height); X = X + cylinders{i}.centroid.x; @@ -24,6 +27,8 @@ function plotCylinderSamples(cylinders, fieldSize) axis equal axis([0, fieldSize.x, 0, fieldSize.y, 0, 20]); + grid on + if ~holdstate hold off end From d5bebb93d2069760f67afb99178a798401ed60d6 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Sun, 11 Jan 2015 23:20:50 -0500 Subject: [PATCH 04/38] plot the visible samples on cylinders --- matlab/+gtsam/plotProjectedCylinderSamples.m | 36 ++++++++++++++++++ matlab/+gtsam/points2DTrackMonocular.m | 40 ++++++++++++++++++++ 2 files changed, 76 insertions(+) create mode 100644 matlab/+gtsam/plotProjectedCylinderSamples.m create mode 100644 matlab/+gtsam/points2DTrackMonocular.m diff --git a/matlab/+gtsam/plotProjectedCylinderSamples.m b/matlab/+gtsam/plotProjectedCylinderSamples.m new file mode 100644 index 000000000..5d9a06713 --- /dev/null +++ b/matlab/+gtsam/plotProjectedCylinderSamples.m @@ -0,0 +1,36 @@ +function plotProjectedCylinderSamples(visiblePoints3, cameraPose, figID) +% plot the visible projected points on the cylinders +% author: Zhaoyang Lv + + import gtsam.* + + figure(figID); + + holdstate = ishold; + hold on + + %plotCamera(cameraPose, 5); + + pointsNum = size(visiblePoints3, 1) + + for i=1:pointsNum + ray = visiblePoints3{i}.between(cameraPose.translation()).vector(); + dist = norm(ray); + + p = plot3(visiblePoints3{i}.x, visiblePoints3{i}.y, visiblePoints3{i}.z, ... + 'o', 'MarkerFaceColor', 'Green'); + + for t=0:0.1:dist + marchingRay = ray * t; + p.XData = visiblePoints3{i}.x + marchingRay(1); + p.YData = visiblePoints3{i}.y + marchingRay(2); + p.ZData = visiblePoints3{i}.z + marchingRay(3); + drawnow update + end + + end + + if ~holdstate + hold off + end +end \ No newline at end of file diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m new file mode 100644 index 000000000..1c1e641bf --- /dev/null +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -0,0 +1,40 @@ +function pts2dTracksmon = 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 +% @author: Zhaoyang Lv + +import gtsam.* + +%% create graph +graph = NonlinearFactorGraph; + +%% add a constraint on the starting pose +poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; +posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); +firstPose = cameraPoses{1}; +graph.add(PriorFactorPose3(symbol('x', l), firstPose, posePriorNoise)); + +cameraPosesNum = size(cameraPoses, 1); + +%% add measurements +initialEstimate = Values; +for i = 1:cameraPosesNum + [visiblePoints3, visiblePointsCylinderIdx] = cylinderSampleProjection(K, cameraPoses{i}, imageSize, cylinders); + + pointsNum = size(visiblePoints, 1); + + %% not finished + %for j = 1:pointsNum + % graph.add(); + %end +end + +marginals = Marginals(graph, initialEstimate); + +% should use all the points num to replace the num 100 +for i = 1:100 + marginals.marginalCovariance(symbol('p',i)); +end + +end From 377c46281854958bea5e6047d33da41774d22402 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Sun, 11 Jan 2015 23:21:59 -0500 Subject: [PATCH 05/38] cylinderSampling moved to gtsam+ folder --- matlab/+gtsam/cylinderSampling.m | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 matlab/+gtsam/cylinderSampling.m diff --git a/matlab/+gtsam/cylinderSampling.m b/matlab/+gtsam/cylinderSampling.m new file mode 100644 index 000000000..dcaa9c721 --- /dev/null +++ b/matlab/+gtsam/cylinderSampling.m @@ -0,0 +1,26 @@ +function [cylinder] = cylinderSampling(baseCentroid, radius, height, density) +% +% @author: Zhaoyang Lv + import gtsam.* + % calculate the cylinder area + area = 2 * pi * radius * height; + + pointsNum = round(area * density); + + points3 = cell(pointsNum, 1); + + % sample the points + for i = 1:pointsNum + theta = 2 * pi * rand; + x = radius * cos(theta) + baseCentroid.x; + y = radius * sin(theta) + baseCentroid.y; + z = height * rand; + points3{i,1} = Point3([x,y,z]'); + end + + cylinder.area = area; + cylinder.radius = radius; + cylinder.height = height; + cylinder.Points = points3; + cylinder.centroid = Point3(baseCentroid.x, baseCentroid.y, height/2); +end \ No newline at end of file From b45e81725b60460e6a35b5b526c0050da2cc257c Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Sun, 11 Jan 2015 23:22:42 -0500 Subject: [PATCH 06/38] an update of function 1&2 cameraFlyingExample. function3&4 in construction --- matlab/gtsam_examples/CameraFlyingExample.m | 34 +++++++++++++++++---- 1 file changed, 28 insertions(+), 6 deletions(-) diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 3a2a8991a..4d6e38720 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -2,7 +2,7 @@ clear all; clc; import gtsam.* -cylinder_num = 20; +cylinder_num = 10; cylinders = cell(cylinder_num, 1); % generate a set of cylinders @@ -11,21 +11,43 @@ fieldSize = Point2([100, 100]'); % random generate cylinders on the fields for i = 1:cylinder_num baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]'); - cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 30); + cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1); end % plot all the cylinders and sampled points % now is plotting on a 100 * 100 field -plotCylinderSamples(cylinders, fieldSize); +figID = 1; +figure(figID); +plotCylinderSamples(cylinders, fieldSize, figID); % visibility validation +% generate camera trajectories K = Cal3_S2(525,525,0,320,240); -cameraPose = Pose3(); % now set to be default +imageSize = Point2([640, 480]'); +poseNum = 10; +cameraPoses = cell(poseNum, 1); +cameraPoses{1} = Pose3(); +cameras = cell(poseNum, 1); +for i = 2:poseNum + incRot = Rot3.RzRyRx(0,0,pi/4); + incT = Point3(5*rand, 5*rand, 5*rand); + cameraPoses{i} = cameraPoses{i-1}.compose(Pose3(incRot, incT)); +end -% the projections of all visible 3D points -% visiblePoints3 = cylinderSampleProjection(K, cameraPose, cylinders); +[visiblePoints3, ~] = cylinderSampleProjection(K, cameraPoses{1}, imageSize, cylinders); +plotPose3(cameraPoses{1}, 5 ) +% plot all the projected points +plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); + +pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders); + +%pts2dTracksStereo = points2DTrackStereo(K, cameraPoses, imageSize, cylinders); + + +% ToDo: plot the trajectories +%plot3DTrajectory(); From a8bf2a4da1b1b8334ee41151891b1170fbb5ffce Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Mon, 12 Jan 2015 16:10:49 -0500 Subject: [PATCH 07/38] function3 add graph measurement and initial estimate --- matlab/+gtsam/cylinderSampleProjection.m | 123 ++++++++++-------- matlab/+gtsam/points2DTrackMonocular.m | 53 ++++++-- matlab/gtsam_examples/CameraFlyingExample.m | 36 ++--- .../gtsam_examples/cylinderSampleProjection.m | 41 ------ matlab/gtsam_examples/cylinderSampling.m | 25 ---- matlab/unstable_examples/.gitignore | 1 + 6 files changed, 131 insertions(+), 148 deletions(-) delete mode 100644 matlab/gtsam_examples/cylinderSampleProjection.m delete mode 100644 matlab/gtsam_examples/cylinderSampling.m diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m index d74726956..5cb5c64df 100644 --- a/matlab/+gtsam/cylinderSampleProjection.m +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -1,64 +1,83 @@ -function [visiblePoints, visiblePointsCylinderIdx] = cylinderSampleProjection(K, cameraPose, imageSize, cylinders) -% Project sampled points on cylinder to camera frame -% Authors: Zhaoyang Lv +function [visiblePoints] = cylinderSampleProjection(camera, imageSize, cylinders) - import gtsam.* +% Input: +% Output: +% visiblePoints: data{k} 3D Point in overal point clouds with index k +% Z{k} 2D measurements in overal point clouds with index k +% index {i}{j} +% i: the cylinder index; +% j: the point index on the cylinder; +% +% @Description: Project sampled points on cylinder to camera frame +% @Authors: Zhaoyang Lv - cylinder_num = size(cylinders, 1); - - %camera = SimpleCamera(cameraPose, K); - camera = SimpleCamera.Lookat(cameraPose.translation(), cylinders{10}.centroid, Point3([0,0,1]'), K); +import gtsam.* - visiblePoints = {}; - visiblePointsCylinderIdx = []; +%% memory allocation +cylinderNum = length(cylinders); +visiblePoints.index = cell(cylinderNum); + +pointCloudNum = 0; +for i = 1:cylinderNum + pointCloudNum = pointCloudNum + length(cylinders{i}.Points); + visiblePoints.index{i} = cell(pointCloudNum); +end +visiblePoints.data = cell(pointCloudNum); + +%% check visiblity of points on each cylinder +pointCloudIndex = 0; +for i = 1:cylinderNum - for i = 1:cylinder_num + pointNum = length(cylinders{i}.Points); - point_num = size( cylinders{i}.Points, 1); + % to check point visibility + for j = 1:pointNum + + pointCloudIndex = pointCloudIndex + 1; - % to check point visibility - for j = 1:point_num - sampledPoint3 = cylinders{i}.Points{j}; - measurements2d = camera.project(sampledPoint3); - - % ignore points not visible in the scene - if measurements2d.x < 0 || measurements2d.x >= imageSize.x ... - || measurements2d.y < 0 || measurements2d.y >= imageSize.y - continue; - end - - % ignore points occluded - % 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 - for k = 1:cylinder_num - - rayCameraToPoint = cameraPose.translation().between(sampledPoint3).vector(); - rayCameraToCylinder = cameraPose.translation().between(cylinders{i}.centroid).vector(); - rayCylinderToPoint = cylinders{i}.centroid.between(sampledPoint3).vector(); - - % Condition 1: all points in front of the cylinders' - % surfaces are visible - if dot(rayCylinderToPoint, rayCameraToCylinder) < 0 - visiblePoints{end+1} = sampledPoint3; - visiblePointsCylinderIdx = [visiblePointsCylinderIdx, i]; - continue; - end + sampledPoint3 = cylinders{i}.Points{j}; + Z2d = camera.project(sampledPoint3); - % 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{end+1} = sampledPoint3; - visiblePointsCylinderIdx = [visiblePointsCylinderIdx, i]; - end - end - + % ignore points not visible in the scene + if Z2d.x < 0 || Z2d.x >= imageSize.x ... + || Z2d.y < 0 || Z2d.y >= imageSize.y + continue; + end + + % ignore points occluded + % 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 + for k = 1:cylinderNum + + rayCameraToPoint = cameraPose.translation().between(sampledPoint3).vector(); + rayCameraToCylinder = cameraPose.translation().between(cylinders{i}.centroid).vector(); + rayCylinderToPoint = cylinders{i}.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} = sampledPoints3; + visiblePoints.Z{pointCloudIndex} = Z2d; + visiblePoints.index{i}{j} = pointCloudIndex; + end + end + end - end + +end end diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 1c1e641bf..c0830bde7 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -1,4 +1,4 @@ -function pts2dTracksmon = points2DTrackMonocular(K, cameraPoses, 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 @@ -9,32 +9,59 @@ import gtsam.* %% create graph graph = NonlinearFactorGraph; -%% add a constraint on the starting pose +pointNoiseSigma = 0.1; poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; + +%% add a constraint on the starting pose posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); firstPose = cameraPoses{1}; graph.add(PriorFactorPose3(symbol('x', l), firstPose, posePriorNoise)); -cameraPosesNum = size(cameraPoses, 1); +cameraPosesNum = length(cameraPoses); -%% add measurements +%% add measurements and initial camera & points values +pointsNum = 0; +cylinderNum = length(cylinders); +for i = 1:cylinderNum + pointsNum = pointsNum + length(cylinders{i}.Points); +end + +measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); + +pts3d = {}; initialEstimate = Values; for i = 1:cameraPosesNum - [visiblePoints3, visiblePointsCylinderIdx] = cylinderSampleProjection(K, cameraPoses{i}, imageSize, cylinders); + camera = SimpleCamera(K, cameraPoses{i}); - pointsNum = size(visiblePoints, 1); + pts3d.pts{i} = cylinderSampleProjection(camera, imageSize, cylinders); + pts3d.camera{i} = camera; + + for j = 1:length(pts3d.pts{i}.Z) + graph.add(GenericProjectionFactorCal3_S2(pts3d.pts{i}.Z{j}, ... + measurementNoise, symbol('x', i), symbol('p', j), camera.K) ); + + point_j = pts3d.pts{i}.data{j}.retract(0.1*randn(3,1)); + initialEstimate.insert(symbol('p', j), point_j); + end + + pose_i = camera.pose.retract(0.1*randn(6,1)); + initialEstimate.insert(symbole('x', i), pose_i); - %% not finished - %for j = 1:pointsNum - % graph.add(); - %end end +%% Print the graph +graph.print(sprintf('\nFactor graph:\n')); + marginals = Marginals(graph, initialEstimate); -% should use all the points num to replace the num 100 -for i = 1:100 - marginals.marginalCovariance(symbol('p',i)); +%% get all the 2d points track information +ptIdx = 0; +for i = 1:pointsNum + if isempty(pts3d.pts{i}) + continue; + end + %pts2dTrackMono.pts2d = pts3d.pts{i} + pts2dTracksMono.cov{ptIdx} = marginals.marginalCovariance(symbol('p',i)); end end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 4d6e38720..13dcbfbe4 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -2,47 +2,49 @@ clear all; clc; import gtsam.* +%% generate a set of cylinders and Samples +fieldSize = Point2([100, 100]'); cylinder_num = 10; cylinders = cell(cylinder_num, 1); -% generate a set of cylinders -fieldSize = Point2([100, 100]'); - -% random generate cylinders on the fields -for i = 1:cylinder_num +for i = 1:cylinderNum baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]'); cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1); end -% plot all the cylinders and sampled points +%% plot all the cylinders and sampled points % now is plotting on a 100 * 100 field figID = 1; figure(figID); plotCylinderSamples(cylinders, fieldSize, figID); -% visibility validation -% generate camera trajectories +%% generate camera trajectories K = Cal3_S2(525,525,0,320,240); imageSize = Point2([640, 480]'); poseNum = 10; -cameraPoses = cell(poseNum, 1); -cameraPoses{1} = Pose3(); cameras = cell(poseNum, 1); -for i = 2:poseNum - incRot = Rot3.RzRyRx(0,0,pi/4); +trans = Point3(); +% To ensure there are landmarks in view, look at one randomly chosen cylinder +% each time. +for i = 1:poseNum + camera = SimpleCamera.Lookat(trans, cylinders{round(cylinderNum*rand)}.centroid, ... + Point3([0,0,1]'), K); + incT = Point3(5*rand, 5*rand, 5*rand); - cameraPoses{i} = cameraPoses{i-1}.compose(Pose3(incRot, incT)); + trans = trans.compose(incT); end -[visiblePoints3, ~] = cylinderSampleProjection(K, cameraPoses{1}, imageSize, cylinders); +%% visibility validation +visiblePoints3 = cylinderSampleProjection(camera, imageSize, cylinders); -plotPose3(cameraPoses{1}, 5 ) -% plot all the projected points -plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); +%% plot all the projected points +%plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); +%% setp up monocular camera and get measurements pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders); +%% set up stereo camera and get measurements %pts2dTracksStereo = points2DTrackStereo(K, cameraPoses, imageSize, cylinders); diff --git a/matlab/gtsam_examples/cylinderSampleProjection.m b/matlab/gtsam_examples/cylinderSampleProjection.m deleted file mode 100644 index 33e031bc8..000000000 --- a/matlab/gtsam_examples/cylinderSampleProjection.m +++ /dev/null @@ -1,41 +0,0 @@ -function [] = cylinderSampleProjection(K, cameraPose, imageSize, cylinders) -% Project sampled points on cylinder to camera frame -% Authors: Zhaoyang Lv - - cylinder_num = size(cylinders, 1); - - camera = SimpleCamera(cameraPose, K); - - for i = 1:cylinder_num - - point_num = size( cylinders{i}.Points, 1); - - % to check point visibility - for j = 1:point_num - sampledPoint3 = cylinders{i}.Poinsts{j}; - measurements2d = camera.project(sampledPoint3); - - % ignore points not visible in the scene - if measurements2d.x < 0 || measurements.x >= imageSize.x ... - || measurements2d.y < 0 || measurements.y >= imageSize.y - continue; - end - % ignore points occluded - % 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 - for k = 1:cylinder_num - - rayCameraToPoint = sampledPoint3 - cameraPose.t; - rayCameraToCylinder = cylinders{i} - cameraPose.t; - - projectedRay = dot(rayCameraToPoint, rayCameraToCylinder); - distCameraToCylinder = norm(rayCameraToCylinder); - - - end - end - - end - -end diff --git a/matlab/gtsam_examples/cylinderSampling.m b/matlab/gtsam_examples/cylinderSampling.m deleted file mode 100644 index 2ee304da1..000000000 --- a/matlab/gtsam_examples/cylinderSampling.m +++ /dev/null @@ -1,25 +0,0 @@ -function [cylinder] = cylinderSampling(baseCentroid, radius, height, density) -% - import gtsam.* - % calculate the cylinder area - area = 2 * pi * radius * height; - - pointsNum = round(area * density); - - points3 = cell(pointsNum, 1); - - % sample the points - for i = 1:pointsNum - theta = 2 * pi * rand; - x = radius * cos(theta) + baseCentroid.x; - y = radius * sin(theta) + baseCentroid.y; - z = height * rand; - points3{i,1} = Point3([x,y,z]'); - end - - cylinder.area = area; - cylinder.radius = radius; - cylinder.height = height; - cylinder.Points = points3; - cylinder.centroid = Point3(baseCentroid.x, baseCentroid.y, height/2); -end \ No newline at end of file diff --git a/matlab/unstable_examples/.gitignore b/matlab/unstable_examples/.gitignore index 6d725d9bc..c47168f67 100644 --- a/matlab/unstable_examples/.gitignore +++ b/matlab/unstable_examples/.gitignore @@ -1 +1,2 @@ *.m~ +*.avi From 39f5aa499e6f9009d5acadf5b953b65abf4b803d Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Mon, 12 Jan 2015 23:27:06 -0500 Subject: [PATCH 08/38] 2D monocular track. Testing with random data now throws indeterminant linear system exception --- matlab/+gtsam/cylinderSampleProjection.m | 19 +++++--- matlab/+gtsam/points2DTrackMonocular.m | 57 +++++++++++++++++------- 2 files changed, 52 insertions(+), 24 deletions(-) diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m index 5cb5c64df..4a8c3b06a 100644 --- a/matlab/+gtsam/cylinderSampleProjection.m +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -15,14 +15,15 @@ import gtsam.* %% memory allocation cylinderNum = length(cylinders); -visiblePoints.index = cell(cylinderNum); +visiblePoints.index = cell(cylinderNum,1); pointCloudNum = 0; for i = 1:cylinderNum pointCloudNum = pointCloudNum + length(cylinders{i}.Points); - visiblePoints.index{i} = cell(pointCloudNum); + visiblePoints.index{i} = cell(pointCloudNum,1); end -visiblePoints.data = cell(pointCloudNum); +visiblePoints.data = cell(pointCloudNum,1); +visiblePoints.Z = cell(pointCloudNum, 1); %% check visiblity of points on each cylinder pointCloudIndex = 0; @@ -34,8 +35,12 @@ for i = 1:cylinderNum for j = 1:pointNum pointCloudIndex = pointCloudIndex + 1; - + sampledPoint3 = cylinders{i}.Points{j}; + sampledPoint3local = camera.pose.transform_to(sampledPoint3); + if sampledPoint3local.z < 0 + continue; % Cheirality Exception + end Z2d = camera.project(sampledPoint3); % ignore points not visible in the scene @@ -50,8 +55,8 @@ for i = 1:cylinderNum % 2. For points behind the cylinders' surfaces, the cylinder for k = 1:cylinderNum - rayCameraToPoint = cameraPose.translation().between(sampledPoint3).vector(); - rayCameraToCylinder = cameraPose.translation().between(cylinders{i}.centroid).vector(); + rayCameraToPoint = camera.pose.translation().between(sampledPoint3).vector(); + rayCameraToCylinder = camera.pose.translation().between(cylinders{i}.centroid).vector(); rayCylinderToPoint = cylinders{i}.centroid.between(sampledPoint3).vector(); % Condition 1: all points in front of the cylinders' @@ -69,7 +74,7 @@ for i = 1:cylinderNum rayCylinderToProjected = norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint; if rayCylinderToProjected(1) > cylinders{i}.radius && ... rayCylinderToProjected(2) > cylinders{i}.radius - visiblePoints.data{pointCloudIndex} = sampledPoints3; + visiblePoints.data{pointCloudIndex} = sampledPoint3; visiblePoints.Z{pointCloudIndex} = Z2d; visiblePoints.index{i}{j} = pointCloudIndex; end diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index c0830bde7..9bdd746ae 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -1,4 +1,4 @@ -function pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders) +function pts2dTracksMono = points2DTrackMonocular(cameras, 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 @@ -9,15 +9,15 @@ import gtsam.* %% create graph graph = NonlinearFactorGraph; +%% create the noise factors pointNoiseSigma = 0.1; poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; - -%% add a constraint on the starting pose +measurementNoiseSigma = 1.0; posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); -firstPose = cameraPoses{1}; -graph.add(PriorFactorPose3(symbol('x', l), firstPose, posePriorNoise)); +pointPriorNoise = noiseModel.Isotropic.Sigma(3, pointNoiseSigma); +measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); -cameraPosesNum = length(cameraPoses); +cameraPosesNum = length(cameras); %% add measurements and initial camera & points values pointsNum = 0; @@ -26,27 +26,50 @@ for i = 1:cylinderNum pointsNum = pointsNum + length(cylinders{i}.Points); end -measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); - pts3d = {}; initialEstimate = Values; +initialized = false; for i = 1:cameraPosesNum - camera = SimpleCamera(K, cameraPoses{i}); + % add a constraint on the starting pose + camera = cameras{i}; pts3d.pts{i} = cylinderSampleProjection(camera, imageSize, cylinders); pts3d.camera{i} = camera; - for j = 1:length(pts3d.pts{i}.Z) - graph.add(GenericProjectionFactorCal3_S2(pts3d.pts{i}.Z{j}, ... - measurementNoise, symbol('x', i), symbol('p', j), camera.K) ); + if ~initialized + graph.add(PriorFactorPose3(symbol('x', 1), camera.pose, posePriorNoise)); + k = 0; + if ~isempty(pts3d.pts{i}.data{1+k}) + graph.add(PriorFactorPoint3(symbol('p', 1), ... + pts3d.pts{i}.data{1+k}, pointPriorNoise)); + else + k = k+1; + end + initialized = true; + end - point_j = pts3d.pts{i}.data{j}.retract(0.1*randn(3,1)); - initialEstimate.insert(symbol('p', j), point_j); + for j = 1:length(pts3d.pts{i}.Z) + if isempty(pts3d.pts{i}.Z{j}) + continue; + end + graph.add(GenericProjectionFactorCal3_S2(pts3d.pts{i}.Z{j}, ... + measurementNoise, symbol('x', i), symbol('p', j), camera.calibration) ); end +end + +%% initialize cameras and points close to ground truth +for i = 1:cameraPosesNum pose_i = camera.pose.retract(0.1*randn(6,1)); - initialEstimate.insert(symbole('x', i), pose_i); - + initialEstimate.insert(symbol('x', i), pose_i); +end +ptsIdx = 0; +for i = 1:length(cylinders) + for j = 1:length(cylinders{i}.Points) + ptsIdx = ptsIdx + 1; + point_j = cylinders{i}.Points{j}.retract(0.1*randn(3,1)); + initialEstimate.insert(symbol('p', ptsIdx), point_j); + end end %% Print the graph @@ -55,12 +78,12 @@ graph.print(sprintf('\nFactor graph:\n')); marginals = Marginals(graph, initialEstimate); %% get all the 2d points track information +% currently throws the Indeterminant linear system exception ptIdx = 0; for i = 1:pointsNum if isempty(pts3d.pts{i}) continue; end - %pts2dTrackMono.pts2d = pts3d.pts{i} pts2dTracksMono.cov{ptIdx} = marginals.marginalCovariance(symbol('p',i)); end From 678aabce3eb9ecd1aaa1fab79d7bb1aada865529 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Mon, 12 Jan 2015 23:27:21 -0500 Subject: [PATCH 09/38] add stereo set up --- matlab/+gtsam/points2DTrackStereo.m | 90 +++++++++++++++++++++++++++++ 1 file changed, 90 insertions(+) create mode 100644 matlab/+gtsam/points2DTrackStereo.m diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m new file mode 100644 index 000000000..41f6668b1 --- /dev/null +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -0,0 +1,90 @@ +function pts2dTracksStereo = points2DTrackStereo(cameras, 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 +% @author: Zhaoyang Lv + +import gtsam.* + +%% create graph +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); +measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); + +cameraPosesNum = length(cameras); + +%% add measurements and initial camera & points values +pointsNum = 0; +cylinderNum = length(cylinders); +for i = 1:cylinderNum + pointsNum = pointsNum + length(cylinders{i}.Points); +end + +pts3d = {}; +initialEstimate = Values; +initialized = false; +for i = 1:cameraPosesNum + % add a constraint on the starting pose + camera = cameras{i}; + + pts3d.pts{i} = cylinderSampleProjection(camera, imageSize, cylinders); + pts3d.camera{i} = camera; + + if ~initialized + graph.add(PriorFactorPose3(symbol('x', 1), camera.pose, posePriorNoise)); + k = 0; + if ~isempty(pts3d.pts{i}.data{1+k}) + graph.add(PriorFactorPoint3(symbol('p', 1), ... + pts3d.pts{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}) + continue; + end + graph.add(GenericProjectionFactorCal3_S2(pts3d.pts{i}.Z{j}, ... + measurementNoise, symbol('x', i), symbol('p', j), camera.calibration) ); + end + +end + +%% initialize cameras and points close to ground truth +for i = 1:cameraPosesNum + pose_i = camera.pose.retract(0.1*randn(6,1)); + initialEstimate.insert(symbol('x', i), pose_i); +end +ptsIdx = 0; +for i = 1:length(cylinders) + for j = 1:length(cylinders{i}.Points) + ptsIdx = ptsIdx + 1; + point_j = cylinders{i}.Points{j}.retract(0.1*randn(3,1)); + initialEstimate.insert(symbol('p', ptsIdx), point_j); + end +end + +%% Print the graph +graph.print(sprintf('\nFactor graph:\n')); + +marginals = Marginals(graph, initialEstimate); + +%% get all the 2d points track information +% currently throws the Indeterminant linear system exception +ptIdx = 0; +for i = 1:pointsNum + if isempty(pts3d.pts{i}) + continue; + end + pts2dTracksMono.cov{ptIdx} = marginals.marginalCovariance(symbol('p',i)); +end + +end From 10dc767eda622637d565b6df010b4000b671f31d Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Mon, 12 Jan 2015 23:27:50 -0500 Subject: [PATCH 10/38] change monocular set up and add stereo test. Still under test --- matlab/gtsam_examples/CameraFlyingExample.m | 34 ++++++++++++++++----- 1 file changed, 27 insertions(+), 7 deletions(-) diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 13dcbfbe4..258bd968b 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -1,12 +1,15 @@ clear all; clc; +clf; import gtsam.* %% generate a set of cylinders and Samples fieldSize = Point2([100, 100]'); -cylinder_num = 10; -cylinders = cell(cylinder_num, 1); +cylinderNum = 10; +cylinders = cell(cylinderNum, 1); +% ToDo: it seems random generated cylinders doesn't work that well +% use fixed parameters instead for i = 1:cylinderNum baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]'); cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1); @@ -18,7 +21,6 @@ figID = 1; figure(figID); plotCylinderSamples(cylinders, fieldSize, figID); - %% generate camera trajectories K = Cal3_S2(525,525,0,320,240); imageSize = Point2([640, 480]'); @@ -28,7 +30,8 @@ trans = Point3(); % To ensure there are landmarks in view, look at one randomly chosen cylinder % each time. for i = 1:poseNum - camera = SimpleCamera.Lookat(trans, cylinders{round(cylinderNum*rand)}.centroid, ... + cylinderIdx = max(min(round(cylinderNum*rand), 10), 1); + cameras{i} = SimpleCamera.Lookat(trans, cylinders{cylinderIdx}.centroid, ... Point3([0,0,1]'), K); incT = Point3(5*rand, 5*rand, 5*rand); @@ -36,17 +39,34 @@ for i = 1:poseNum end %% visibility validation -visiblePoints3 = cylinderSampleProjection(camera, imageSize, cylinders); +% 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(K, cameraPoses, imageSize, cylinders); +%pts2dTracksMono = points2DTrackMonocular(cameras, imageSize, cylinders); %% set up stereo camera and get measurements -%pts2dTracksStereo = points2DTrackStereo(K, cameraPoses, imageSize, cylinders); +% load stereo calibration +calib = dlmread(findExampleDataFile('VO_calibration.txt')); +KStereo = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6)); +poseNum = 10; +camerasStereo = cell(poseNum, 1); +trans = Point3(); +for i = 1: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); + +% plot the 2D tracks % ToDo: plot the trajectories %plot3DTrajectory(); From 6ab95f60c2a40b6b1008dc912e1ade7de078d6be Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 13 Jan 2015 01:32:59 -0500 Subject: [PATCH 11/38] use circle generator to replace the random data generator. This can fix the indeterminant system error. --- matlab/gtsam_examples/CameraFlyingExample.m | 69 ++++++++++++--------- 1 file changed, 38 insertions(+), 31 deletions(-) diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 258bd968b..1eb301b38 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -3,15 +3,26 @@ clc; clf; import gtsam.* +%% define the options +options.fieldSize = Point2([100, 100]'); +options.cylinderNum = 10; +options.poseNum = 20; +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 +options.imageSize = Point2([640, 480]'); + %% generate a set of cylinders and Samples -fieldSize = Point2([100, 100]'); -cylinderNum = 10; +cylinderNum = options.cylinderNum; cylinders = cell(cylinderNum, 1); -% ToDo: it seems random generated cylinders doesn't work that well -% use fixed parameters instead +% It seems random generated cylinders doesn't work that well +% Now it set up a circle of cylinders +theta = 0; for i = 1:cylinderNum - baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]'); + theta = theta + 2*pi / 10; + x = 20 * cos(theta) + options.fieldSize.x/2; + y = 20 * sin(theta) + options.fieldSize.y/2; + baseCentroid = Point2([x, y]'); cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1); end @@ -19,23 +30,21 @@ end % now is plotting on a 100 * 100 field figID = 1; figure(figID); -plotCylinderSamples(cylinders, fieldSize, figID); +plotCylinderSamples(cylinders, options.fieldSize, figID); %% generate camera trajectories K = Cal3_S2(525,525,0,320,240); imageSize = Point2([640, 480]'); -poseNum = 10; -cameras = cell(poseNum, 1); -trans = Point3(); -% To ensure there are landmarks in view, look at one randomly chosen cylinder -% each time. -for i = 1:poseNum - cylinderIdx = max(min(round(cylinderNum*rand), 10), 1); - cameras{i} = SimpleCamera.Lookat(trans, cylinders{cylinderIdx}.centroid, ... - Point3([0,0,1]'), K); - - incT = Point3(5*rand, 5*rand, 5*rand); - trans = trans.compose(incT); +cameras = cell(options.poseNum, 1); +% Generate ground truth trajectory r.w.t. the field center +theta = 0; +r = 30; +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, ... + Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... + Point3([0,0,1]'), K); end %% visibility validation @@ -46,25 +55,23 @@ visiblePoints3 = cylinderSampleProjection(cameras{1}, imageSize, cylinders); %plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); %% setp up monocular camera and get measurements -%pts2dTracksMono = points2DTrackMonocular(cameras, imageSize, cylinders); +pts2dTracksMono = points2DTrackMonocular(cameras, 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)); -poseNum = 10; -camerasStereo = cell(poseNum, 1); -trans = Point3(); -for i = 1: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 +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(camerasStereo, imageSize, cylinders); % plot the 2D tracks From 1094739680ddc1961a90511b620eab46574e6797 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 13 Jan 2015 02:01:28 -0500 Subject: [PATCH 12/38] small fix of empty return points values --- matlab/+gtsam/points2DTrackMonocular.m | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 9bdd746ae..33a6b34f2 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -79,12 +79,15 @@ marginals = Marginals(graph, initialEstimate); %% get all the 2d points track information % currently throws the Indeterminant linear system exception -ptIdx = 0; +ptx = 0; for i = 1:pointsNum + ptx = ptx + 1; if isempty(pts3d.pts{i}) continue; end - pts2dTracksMono.cov{ptIdx} = marginals.marginalCovariance(symbol('p',i)); + % cylinder index and measurements + pts2dTracksMono.Points{ptx} = pts3d.pts{i}; + pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',i)); end end From f4da1f874b2046a327127bba1a8da7856acdb404 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 13 Jan 2015 11:34:24 -0500 Subject: [PATCH 13/38] get points track and visualize --- matlab/+gtsam/points2DTrackMonocular.m | 31 ++++++++++++++++++-------- 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 33a6b34f2..184408f26 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -77,17 +77,30 @@ graph.print(sprintf('\nFactor graph:\n')); marginals = Marginals(graph, initialEstimate); -%% get all the 2d points track information +%% get all the points track information % currently throws the Indeterminant linear system exception ptx = 0; -for i = 1:pointsNum - ptx = ptx + 1; - if isempty(pts3d.pts{i}) - continue; - end - % cylinder index and measurements - pts2dTracksMono.Points{ptx} = pts3d.pts{i}; - pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',i)); +for k = 1:cameraPosesNum + + for i = 1:length(cylinders) + for j = 1:length(cylinders{i}.Points) + if isempty(pts3d.pts{k}.index{i}{j}) + continue; + end + 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 + end +%% plot the result with covariance ellipses +hold on; +plot3DPoints(initialEstimate, [], marginals); +plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); + end From d62cb440db758cd1dc2aee9adc45f91151489501 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 13 Jan 2015 16:33:47 -0500 Subject: [PATCH 14/38] interface update --- matlab/+gtsam/cylinderSampleProjection.m | 4 +- matlab/+gtsam/plotCylinderSamples.m | 1 - matlab/+gtsam/points2DTrackMonocular.m | 50 +++++++++++---------- matlab/gtsam_examples/CameraFlyingExample.m | 34 ++++++-------- 4 files changed, 43 insertions(+), 46 deletions(-) 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 From da06689677780e7a6fbe690192a795cac7a689d3 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 13 Jan 2015 21:21:48 -0500 Subject: [PATCH 15/38] update the stereo model and occlusion detection --- .../+gtsam/cylinderSampleProjectionStereo.m | 84 +++++++++++++++++++ matlab/+gtsam/points2DTrackStereo.m | 46 +++++----- 2 files changed, 105 insertions(+), 25 deletions(-) create mode 100644 matlab/+gtsam/cylinderSampleProjectionStereo.m diff --git a/matlab/+gtsam/cylinderSampleProjectionStereo.m b/matlab/+gtsam/cylinderSampleProjectionStereo.m new file mode 100644 index 000000000..917068da0 --- /dev/null +++ b/matlab/+gtsam/cylinderSampleProjectionStereo.m @@ -0,0 +1,84 @@ +function [visiblePoints] = cylinderSampleProjectionStereo(K, pose, imageSize, cylinders) + +import gtsam.* + +%% memory allocation +cylinderNum = length(cylinders); +visiblePoints.index = cell(cylinderNum,1); + +pointCloudNum = 0; +for i = 1:cylinderNum + pointCloudNum = pointCloudNum + length(cylinders{i}.Points); + visiblePoints.index{i} = cell(pointCloudNum,1); +end +visiblePoints.data = cell(pointCloudNum,1); +visiblePoints.Z = cell(pointCloudNum, 1); + +%% check visiblity of points on each cylinder +pointCloudIndex = 0; +for i = 1:cylinderNum + + pointNum = length(cylinders{i}.Points); + + % to check point visibility + for j = 1:pointNum + + pointCloudIndex = pointCloudIndex + 1; + + % For Cheirality Exception + sampledPoint3 = cylinders{i}.Points{j}; + sampledPoint3local = pose.transform_to(sampledPoint3); + if sampledPoint3local.z < 0 + continue; + end + + % measurements + Z.du = K.fx() * K.baseline() / samplePoint3.z; + Z.uL = K.fx() * samplePoint3.x / samplePoint3.z + K.px(); + Z.uR = uL + du; + Z.v = K.fy() / samplePoint3.z + K.py(); + + % ignore points not visible in the scene + if Z.uL < 0 || Z.uL >= imageSize.x || ... + Z.uR < 0 || Z.uR >= imageSize.x || ... + Z.v < 0 || Z.v >= imageSize.y + continue; + end + + % ignore points occluded + % 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 + for k = 1:cylinderNum + + rayCameraToPoint = pose.translation().between(sampledPoint3).vector(); + rayCameraToCylinder = pose.translation().between(cylinders{i}.centroid).vector(); + rayCylinderToPoint = cylinders{i}.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} = Z; + 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} = Z; + visiblePoints.index{i}{j} = pointCloudIndex; + end + end + + end + end + +end + +end diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 41f6668b1..b3fea3d58 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -1,4 +1,4 @@ -function pts2dTracksStereo = points2DTrackStereo(cameras, imageSize, cylinders) +function pts2dTracksStereo = points2DTrackStereo(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 @@ -16,8 +16,9 @@ measurementNoiseSigma = 1.0; posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); pointPriorNoise = noiseModel.Isotropic.Sigma(3, pointNoiseSigma); measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); +stereoNoise = noiseModel.Isotropic.Sigma(3,1); -cameraPosesNum = length(cameras); +cameraPosesNum = length(cameraPoses); %% add measurements and initial camera & points values pointsNum = 0; @@ -26,25 +27,14 @@ 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}; +for i = 1:cameraPosesNum + pts3d{i} = cylinderSampleProjectionStereo(K, cameraPose, imageSize, cylinders); - pts3d.pts{i} = cylinderSampleProjection(camera, imageSize, cylinders); - pts3d.camera{i} = camera; - if ~initialized graph.add(PriorFactorPose3(symbol('x', 1), camera.pose, posePriorNoise)); - k = 0; - if ~isempty(pts3d.pts{i}.data{1+k}) - graph.add(PriorFactorPoint3(symbol('p', 1), ... - pts3d.pts{i}.data{1+k}, pointPriorNoise)); - else - k = k+1; - end initialized = true; end @@ -52,10 +42,9 @@ for i = 1:cameraPosesNum if isempty(pts3d.pts{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(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ... + stereoNoise, symbol('x', i), symbol('p', j), K)); end - end %% initialize cameras and points close to ground truth @@ -79,12 +68,19 @@ marginals = Marginals(graph, initialEstimate); %% get all the 2d points track information % currently throws the Indeterminant linear system exception -ptIdx = 0; -for i = 1:pointsNum - if isempty(pts3d.pts{i}) - continue; - end - pts2dTracksMono.cov{ptIdx} = marginals.marginalCovariance(symbol('p',i)); +ptx = 1; +for i = 1:length(cylinders) + for j = 1:length(cylinders{i}.Points) + 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; + end end end From 4a5d94ea59923361b5b106739098ffaf88a37479 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 13 Jan 2015 23:25:44 -0500 Subject: [PATCH 16/38] test visibility. monocular camera visibility tests passed --- matlab/+gtsam/cylinderSampleProjection.m | 54 ++++++++------- matlab/+gtsam/points2DTrackMonocular.m | 16 ++--- matlab/gtsam_examples/CameraFlyingExample.m | 76 +++++++++++++++------ 3 files changed, 90 insertions(+), 56 deletions(-) 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 From ea556c71d70e9d66d3d7492ce5693cb69c3f9017 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 13 Jan 2015 23:36:19 -0500 Subject: [PATCH 17/38] Stereo camera visibility tests passed --- .../+gtsam/cylinderSampleProjectionStereo.m | 49 ++++++++++--------- matlab/+gtsam/points2DTrackStereo.m | 36 +++++++------- matlab/gtsam_examples/CameraFlyingExample.m | 14 +++--- 3 files changed, 52 insertions(+), 47 deletions(-) diff --git a/matlab/+gtsam/cylinderSampleProjectionStereo.m b/matlab/+gtsam/cylinderSampleProjectionStereo.m index 917068da0..99cb83f87 100644 --- a/matlab/+gtsam/cylinderSampleProjectionStereo.m +++ b/matlab/+gtsam/cylinderSampleProjectionStereo.m @@ -33,10 +33,10 @@ for i = 1:cylinderNum end % measurements - Z.du = K.fx() * K.baseline() / samplePoint3.z; - Z.uL = K.fx() * samplePoint3.x / samplePoint3.z + K.px(); - Z.uR = uL + du; - Z.v = K.fy() / samplePoint3.z + K.py(); + Z.du = K.fx() * K.baseline() / sampledPoint3local.z; + Z.uL = K.fx() * sampledPoint3local.x / sampledPoint3local.z + K.px(); + Z.uR = Z.uL + Z.du; + Z.v = K.fy() / sampledPoint3local.z + K.py(); % ignore points not visible in the scene if Z.uL < 0 || Z.uL >= imageSize.x || ... @@ -49,34 +49,39 @@ 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 = pose.translation().between(sampledPoint3).vector(); - rayCameraToCylinder = pose.translation().between(cylinders{i}.centroid).vector(); - rayCylinderToPoint = cylinders{i}.centroid.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} = Z; - 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} = Z; - 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} = Z; + visiblePoints.index{i}{j} = pointCloudIndex; + end + end end diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index b3fea3d58..14b3b5cd2 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -10,12 +10,8 @@ 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); -measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); stereoNoise = noiseModel.Isotropic.Sigma(3,1); cameraPosesNum = length(cameraPoses); @@ -31,15 +27,15 @@ pts3d = cell(cameraPosesNum, 1); initialEstimate = Values; initialized = false; for i = 1:cameraPosesNum - pts3d{i} = cylinderSampleProjectionStereo(K, cameraPose, imageSize, cylinders); + pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, imageSize, cylinders); if ~initialized - graph.add(PriorFactorPose3(symbol('x', 1), camera.pose, posePriorNoise)); + graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{i}, posePriorNoise)); 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(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ... @@ -49,7 +45,7 @@ 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; @@ -69,17 +65,19 @@ marginals = Marginals(graph, initialEstimate); %% get all the 2d points track information % currently throws the Indeterminant linear system exception ptx = 1; -for i = 1:length(cylinders) - for j = 1:length(cylinders{i}.Points) - 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)); +for k = 1:cameraPosesNum + for i = 1:length(cylinders) + for j = 1:length(cylinders{i}.Points) + if isempty(pts3d{k}.index{i}{j}) + continue; + end + idx = pts3d{k}.index{i}{j}; + pts2dTracksStereo.pt3d{ptx} = pts3d{k}.data{idx}; + pts2dTracksStereo.Z{ptx} = pts3d{k}.Z{idx}; + pts2dTracksStereo.cov{ptx} = marginals.marginalCovariance(symbol('p',idx)); - ptx = ptx + 1; + ptx = ptx + 1; + end end end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 4b379d7a4..3a66bb498 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -31,7 +31,7 @@ options.imageSize = Point2([640, 480]'); % use Monocular camera or Stereo camera options.Mono = false; -%% test1: visibility +%% test1: visibility test in monocular camera cylinders{1}.centroid = Point3(30, 50, 5); cylinders{2}.centroid = Point3(50, 50, 5); cylinders{3}.centroid = Point3(70, 50, 5); @@ -44,16 +44,18 @@ for i = 1:3 cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0)); end -camera = SimpleCamera.Lookat(Point3(40, 50, 10), ... +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; -pts3d = cylinderSampleProjection(options.monoK, pose, options.imageSize, cylinders); +prjMonoResult = cylinderSampleProjection(options.monoK, pose, options.imageSize, cylinders); + +%% test2: visibility test in stereo camera +prjStereoResult = cylinderSampleProjectionStereo(options.stereoK, pose, options.imageSize, cylinders); + + -figID = 1; -figure(figID); -plotCylinderSamples(cylinders, options.fieldSize, figID); %% generate a set of cylinders and Samples cylinderNum = options.cylinderNum; From 2378d59632e35d75bb8c47ac2e105fc85a5dbfb4 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Wed, 14 Jan 2015 00:08:35 -0500 Subject: [PATCH 18/38] remove the redudant empty cells --- matlab/+gtsam/cylinderSampleProjection.m | 23 +++++-------- .../+gtsam/cylinderSampleProjectionStereo.m | 22 +++++-------- matlab/+gtsam/points2DTrackMonocular.m | 33 ++++++------------- matlab/+gtsam/points2DTrackStereo.m | 25 ++++---------- matlab/gtsam_examples/CameraFlyingExample.m | 11 +++---- 5 files changed, 37 insertions(+), 77 deletions(-) diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m index 890f174b7..0abd9cc3c 100644 --- a/matlab/+gtsam/cylinderSampleProjection.m +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -17,18 +17,10 @@ camera = SimpleCamera(pose, K); %% memory allocation cylinderNum = length(cylinders); -visiblePoints.index = cell(cylinderNum,1); - -pointCloudNum = 0; -for i = 1:cylinderNum - pointCloudNum = pointCloudNum + length(cylinders{i}.Points); - visiblePoints.index{i} = cell(pointCloudNum,1); -end -visiblePoints.data = cell(pointCloudNum,1); -visiblePoints.Z = cell(pointCloudNum, 1); %% check visiblity of points on each cylinder pointCloudIndex = 0; +visiblePointIdx = 1; for i = 1:cylinderNum pointNum = length(cylinders{i}.Points); @@ -70,8 +62,8 @@ for i = 1:cylinderNum 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 + if rayCylinderToPoint(1) > cylinders{k}.radius && ... + rayCylinderToPoint(2) > cylinders{k}.radius continue; else visible = false; @@ -83,10 +75,13 @@ for i = 1:cylinderNum end if visible - visiblePoints.data{pointCloudIndex} = sampledPoint3; - visiblePoints.Z{pointCloudIndex} = Z2d; - visiblePoints.index{i}{j} = pointCloudIndex; + visiblePoints.data{visiblePointIdx} = sampledPoint3; + visiblePoints.Z{visiblePointIdx} = Z2d; + visiblePoints.cylinderIdx{visiblePointIdx} = i; + visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex; + visiblePointIdx = visiblePointIdx + 1; end + end end diff --git a/matlab/+gtsam/cylinderSampleProjectionStereo.m b/matlab/+gtsam/cylinderSampleProjectionStereo.m index 99cb83f87..ae02c879c 100644 --- a/matlab/+gtsam/cylinderSampleProjectionStereo.m +++ b/matlab/+gtsam/cylinderSampleProjectionStereo.m @@ -4,18 +4,10 @@ import gtsam.* %% memory allocation cylinderNum = length(cylinders); -visiblePoints.index = cell(cylinderNum,1); - -pointCloudNum = 0; -for i = 1:cylinderNum - pointCloudNum = pointCloudNum + length(cylinders{i}.Points); - visiblePoints.index{i} = cell(pointCloudNum,1); -end -visiblePoints.data = cell(pointCloudNum,1); -visiblePoints.Z = cell(pointCloudNum, 1); %% check visiblity of points on each cylinder pointCloudIndex = 0; +visiblePointIdx = 1; for i = 1:cylinderNum pointNum = length(cylinders{i}.Points); @@ -64,8 +56,8 @@ for i = 1:cylinderNum 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 + if rayCylinderToPoint(1) > cylinders{k}.radius && ... + rayCylinderToPoint(2) > cylinders{k}.radius continue; else visible = false; @@ -77,9 +69,11 @@ for i = 1:cylinderNum end if visible - visiblePoints.data{pointCloudIndex} = sampledPoint3; - visiblePoints.Z{pointCloudIndex} = Z; - visiblePoints.index{i}{j} = pointCloudIndex; + visiblePoints.data{visiblePointIdx} = sampledPoint3; + visiblePoints.Z{visiblePointIdx} = Z; + visiblePoints.cylinderIdx{visiblePointIdx} = i; + visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex; + visiblePointIdx = visiblePointIdx + 1; end end diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 0f209bb43..7cdd99fd3 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -28,10 +28,9 @@ end pts3d = cell(cameraPosesNum, 1); initialEstimate = Values; initialized = false; -for i = 1:cameraPosesNum - % add a constraint on the starting pose - cameraPose = cameraPoses{i}; +for i = 1:cameraPosesNum + cameraPose = cameraPoses{i}; pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders); if ~initialized @@ -39,12 +38,10 @@ for i = 1:cameraPosesNum initialized = true; end - for j = 1:length(pts3d{i}.Z) - if isempty(pts3d{i}.Z{j}) - continue; - end + measurementNum = length(pts3d{i}.Z); + for j = 1:measurementNum graph.add(GenericProjectionFactorCal3_S2(pts3d{i}.Z{j}, ... - measurementNoise, symbol('x', i), symbol('p', j), K) ); + measurementNoise, symbol('x', i), symbol('p', pts3d{i}.overallIdx{j}), K) ); end end @@ -70,23 +67,13 @@ marginals = Marginals(graph, initialEstimate); %% get all the points track information % currently throws the Indeterminant linear system exception -ptx = 1; for k = 1:cameraPosesNum - - for i = 1:length(cylinders) - for j = 1:length(cylinders{i}.Points) - 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; - end + 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 diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 14b3b5cd2..0a27cf90b 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -34,12 +34,10 @@ for i = 1:cameraPosesNum initialized = true; end - for j = 1:length(pts3d{i}.Z) - if isempty(pts3d{i}.Z{j}) - continue; - end + measurementNum = length(pts3d{i}.Z); + for j = 1:measurementNum graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ... - stereoNoise, symbol('x', i), symbol('p', j), K)); + stereoNoise, symbol('x', i), symbol('p', pts3d{i}.overallIdx{j}), K)); end end @@ -64,21 +62,10 @@ marginals = Marginals(graph, initialEstimate); %% get all the 2d points track information % currently throws the Indeterminant linear system exception -ptx = 1; for k = 1:cameraPosesNum - for i = 1:length(cylinders) - for j = 1:length(cylinders{i}.Points) - if isempty(pts3d{k}.index{i}{j}) - continue; - end - idx = pts3d{k}.index{i}{j}; - pts2dTracksStereo.pt3d{ptx} = pts3d{k}.data{idx}; - pts2dTracksStereo.Z{ptx} = pts3d{k}.Z{idx}; - pts2dTracksStereo.cov{ptx} = marginals.marginalCovariance(symbol('p',idx)); - - ptx = ptx + 1; - end - end + pts2dTracksStereo.pt3d{ptx} = pts3d{k}.data{idx}; + pts2dTracksStereo.Z{ptx} = pts3d{k}.Z{idx}; + pts2dTracksStereo.cov{ptx} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{visiblePointIdx})); end end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 3a66bb498..1e5981614 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -29,7 +29,7 @@ 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; +options.Mono = true; %% test1: visibility test in monocular camera cylinders{1}.centroid = Point3(30, 50, 5); @@ -54,9 +54,6 @@ prjMonoResult = cylinderSampleProjection(options.monoK, pose, options.imageSize, %% 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); @@ -93,9 +90,6 @@ for i = 1:options.poseNum cameraPoses{i} = camera.pose; end -%% plot all the projected points -%plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); - %% set up camera and get measurements if options.Mono % use Monocular Camera @@ -107,6 +101,9 @@ else options.imageSize, cylinders); end +%% plot all the projected points +%plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); + % plot the 2D tracks % ToDo: plot the trajectories From 86f580b9ae3cd6c560e077a62ef1ed737883fb92 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Wed, 14 Jan 2015 00:19:17 -0500 Subject: [PATCH 19/38] stereo view works fine. but monocular camera still sufferes from the indeterminant system problem --- matlab/+gtsam/points2DTrackStereo.m | 16 +++++++++++++--- matlab/gtsam_examples/CameraFlyingExample.m | 7 +++++-- 2 files changed, 18 insertions(+), 5 deletions(-) diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 0a27cf90b..a65276e38 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -63,9 +63,19 @@ marginals = Marginals(graph, initialEstimate); %% get all the 2d points track information % currently throws the Indeterminant linear system exception for k = 1:cameraPosesNum - pts2dTracksStereo.pt3d{ptx} = pts3d{k}.data{idx}; - pts2dTracksStereo.Z{ptx} = pts3d{k}.Z{idx}; - pts2dTracksStereo.cov{ptx} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{visiblePointIdx})); + num = length(pts3d{k}.data); + for i = 1:num + pts2dTracksStereo.pt3d{i} = pts3d{k}.data{i}; + pts2dTracksStereo.Z{i} = pts3d{k}.Z{i}; + pts2dTracksStereo.cov{i} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{i})); + end end +%% plot the result with covariance ellipses +hold on; +%plot3DPoints(initialEstimate, [], marginals); +%plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); +plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); +view(3); + end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 1e5981614..c86c8cb83 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -20,6 +20,8 @@ import gtsam.* options.fieldSize = Point2([100, 100]'); % the number of cylinders options.cylinderNum = 10; +% point density on cylinder +options.density = 1; % The number of camera poses options.poseNum = 20; % Monocular Camera Calibration @@ -29,7 +31,8 @@ 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 = true; +options.Mono = false; + %% test1: visibility test in monocular camera cylinders{1}.centroid = Point3(30, 50, 5); @@ -66,7 +69,7 @@ for i = 1:cylinderNum 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); + cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, options.density); end %% plot all the cylinders and sampled points From 3cb1f96371c147737af0f8b332cb32f5648ce11d Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Wed, 14 Jan 2015 23:36:15 -0500 Subject: [PATCH 20/38] to make a straight line trajectory --- matlab/gtsam_examples/CameraFlyingExample.m | 64 +++++++++++++++++---- 1 file changed, 53 insertions(+), 11 deletions(-) diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index c86c8cb83..727374288 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -19,11 +19,17 @@ import gtsam.* % the testing field size options.fieldSize = Point2([100, 100]'); % the number of cylinders -options.cylinderNum = 10; +options.cylinderNum = 20; % point density on cylinder options.density = 1; + % The number of camera poses options.poseNum = 20; +% covariance scaling factor +options.scale = 1; + + +%% Camera Setup % Monocular Camera Calibration options.monoK = Cal3_S2(525,525,0,320,240); % Stereo Camera Calibration @@ -31,7 +37,13 @@ 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; +options.Mono = true; +% fps for image +options.fps = 20; +% camera flying speed +options.speed = 20; + + %% test1: visibility test in monocular camera @@ -57,7 +69,7 @@ prjMonoResult = cylinderSampleProjection(options.monoK, pose, options.imageSize, %% test2: visibility test in stereo camera 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; cylinders = cell(cylinderNum, 1); @@ -65,34 +77,49 @@ cylinders = cell(cylinderNum, 1); % Now it set up a circle of cylinders theta = 0; for i = 1:cylinderNum - theta = theta + 2*pi / 10; - x = 10 * cos(theta) + options.fieldSize.x/2; - y = 10 * sin(theta) + options.fieldSize.y/2; + theta = theta + 2*pi/10; + x = 30 * rand * cos(theta) + options.fieldSize.x/2; + y = 20 * rand * sin(theta) + options.fieldSize.y/2; baseCentroid = Point2([x, y]'); cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, options.density); end + %% plot all the cylinders and sampled points % now is plotting on a 100 * 100 field figID = 1; figure(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); 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]'); + t = Point3([(i-1)*(options.fieldSize.x - 20)/options.poseNum + 20, ... + 15, 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 + %% set up camera and get measurements if options.Mono % use Monocular Camera @@ -102,9 +129,24 @@ else % use Stereo Camera pts2dTracksStereo = points2DTrackStereo(options.stereoK, cameraPoses, ... 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 %% plot all the projected points + %plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); % plot the 2D tracks From 7eec7f7b455ed5b61d471adb314ff2f7e08f33f9 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Wed, 14 Jan 2015 23:37:10 -0500 Subject: [PATCH 21/38] to check single measurement constraint. --- matlab/+gtsam/points2DTrackMonocular.m | 62 +++++++++++++++++++------- 1 file changed, 45 insertions(+), 17 deletions(-) diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 7cdd99fd3..83bf45fbd 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -21,43 +21,58 @@ cameraPosesNum = length(cameraPoses); %% add measurements and initial camera & points values pointsNum = 0; cylinderNum = length(cylinders); +points3d = cell(0); for i = 1:cylinderNum - pointsNum = pointsNum + length(cylinders{i}.Points); + cylinderPointsNum = length(cylinders{i}.Points); + pointsNum = pointsNum + cylinderPointsNum; + for j = 1:cylinderPointsNum + points3d{end+1}.data = cylinders{i}.Points{j}; + points3d{end}.Z = cell(0); + points3d{end}.cameraConstraint = cell(0); + points3d{end}.visiblity = false; + end end +graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise)); + pts3d = cell(cameraPosesNum, 1); initialEstimate = Values; -initialized = false; for i = 1:cameraPosesNum cameraPose = cameraPoses{i}; pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders); - - if ~initialized - graph.add(PriorFactorPose3(symbol('x', 1), cameraPose, posePriorNoise)); - initialized = true; - end measurementNum = length(pts3d{i}.Z); for j = 1:measurementNum - graph.add(GenericProjectionFactorCal3_S2(pts3d{i}.Z{j}, ... - measurementNoise, symbol('x', i), symbol('p', pts3d{i}.overallIdx{j}), K) ); + index = pts3d{i}.overallIdx{j}; + points3d{index}.Z{end+1} = pts3d{i}.Z{j}; + points3d{index}.cameraConstraint{end+1} = i; + points3d{index}.visiblity = true; end - + end -%% initialize cameras and points close to ground truth +%% initialize graph and values for i = 1:cameraPosesNum pose_i = cameraPoses{i}.retract(0.1*randn(6,1)); initialEstimate.insert(symbol('x', i), pose_i); end -ptsIdx = 0; -for i = 1:length(cylinders) - for j = 1:length(cylinders{i}.Points) - ptsIdx = ptsIdx + 1; - point_j = cylinders{i}.Points{j}.retract(0.1*randn(3,1)); - initialEstimate.insert(symbol('p', ptsIdx), point_j); + +for i = 1:pointsNum + % single measurement. not added to graph + factorNum = length(points3d{i}.Z); + if factorNum > 1 + for j = 1:factorNum + cameraIdx = points3d{i}.cameraConstraint{j}; + graph.add(GenericProjectionFactorCal3_S2(points3d{i}.Z{j}, ... + measurementNoise, symbol('x', cameraIdx), symbol('p', points3d{i}.cameraConstraint{j}), K) ); + end end + + % add in values + point_j = points3d{i}.data.retract(0.1*randn(3,1)); + initialEstimate.insert(symbol('p', i), point_j); + end %% Print the graph @@ -67,6 +82,19 @@ marginals = Marginals(graph, initialEstimate); %% get all the points track information % currently throws the Indeterminant linear system exception +for i = 1:pointsNum + if points3d{i}.visiblity + pts2dTracksMono.pt3d{i} = points3d{i}.data; + pts2dTracksMono.Z = points3d{i}.Z; + + if length(points3d{i}.Z) == 1 + %pts2dTracksMono.cov{i} singular matrix + else + pts2dTracksMono.cov{i} = marginals.marginalCovariance(symbol('p', i)); + end + end +end + for k = 1:cameraPosesNum num = length(pts3d{k}.data); for i = 1:num From 27b3b5ebedbc5c73f3efd9e70e3707763db0ccab Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 15 Jan 2015 00:20:06 -0500 Subject: [PATCH 22/38] plot the points covariance --- .../+gtsam/cylinderSampleProjectionStereo.m | 5 +++ matlab/+gtsam/plotProjectedCylinderSamples.m | 45 ++++++++++--------- matlab/+gtsam/points2DTrackStereo.m | 8 +++- matlab/gtsam_examples/CameraFlyingExample.m | 17 +------ 4 files changed, 39 insertions(+), 36 deletions(-) diff --git a/matlab/+gtsam/cylinderSampleProjectionStereo.m b/matlab/+gtsam/cylinderSampleProjectionStereo.m index ae02c879c..51cda12ac 100644 --- a/matlab/+gtsam/cylinderSampleProjectionStereo.m +++ b/matlab/+gtsam/cylinderSampleProjectionStereo.m @@ -5,6 +5,11 @@ import gtsam.* %% memory allocation cylinderNum = length(cylinders); +visiblePoints.data = cell(1); +visiblePoints.Z = cell(1); +visiblePoints.cylinderIdx = cell(1); +visiblePoints.overallIdx = cell(1); + %% check visiblity of points on each cylinder pointCloudIndex = 0; visiblePointIdx = 1; diff --git a/matlab/+gtsam/plotProjectedCylinderSamples.m b/matlab/+gtsam/plotProjectedCylinderSamples.m index 5d9a06713..a58526632 100644 --- a/matlab/+gtsam/plotProjectedCylinderSamples.m +++ b/matlab/+gtsam/plotProjectedCylinderSamples.m @@ -1,4 +1,4 @@ -function plotProjectedCylinderSamples(visiblePoints3, cameraPose, figID) +function plotProjectedCylinderSamples(pts3d, covariance, figID) % plot the visible projected points on the cylinders % author: Zhaoyang Lv @@ -9,27 +9,32 @@ function plotProjectedCylinderSamples(visiblePoints3, cameraPose, figID) holdstate = ishold; hold on - %plotCamera(cameraPose, 5); - - pointsNum = size(visiblePoints3, 1) - - for i=1:pointsNum - ray = visiblePoints3{i}.between(cameraPose.translation()).vector(); - dist = norm(ray); - - p = plot3(visiblePoints3{i}.x, visiblePoints3{i}.y, visiblePoints3{i}.z, ... - 'o', 'MarkerFaceColor', 'Green'); - - for t=0:0.1:dist - marchingRay = ray * t; - p.XData = visiblePoints3{i}.x + marchingRay(1); - p.YData = visiblePoints3{i}.y + marchingRay(2); - p.ZData = visiblePoints3{i}.z + marchingRay(3); - drawnow update - end - + pointsNum = length(pts3d); + for i = 1:pointsNum + plotPoint3(pts3d{i}, 'green', 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 diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index a65276e38..141596cea 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -12,7 +12,7 @@ graph = NonlinearFactorGraph; %% create the noise factors poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); -stereoNoise = noiseModel.Isotropic.Sigma(3,1); +stereoNoise = noiseModel.Isotropic.Sigma(3,0.2); cameraPosesNum = length(cameraPoses); @@ -35,6 +35,9 @@ for i = 1:cameraPosesNum end measurementNum = length(pts3d{i}.Z); + if measurementNum < 1 + continue; + end for j = 1:measurementNum graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ... stereoNoise, symbol('x', i), symbol('p', pts3d{i}.overallIdx{j}), K)); @@ -64,6 +67,9 @@ marginals = Marginals(graph, initialEstimate); % currently throws the Indeterminant linear system exception for k = 1:cameraPosesNum num = length(pts3d{k}.data); + if num < 1 + continue; + end for i = 1:num pts2dTracksStereo.pt3d{i} = pts3d{k}.data{i}; pts2dTracksStereo.Z{i} = pts3d{k}.Z{i}; diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 727374288..eb047b50a 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -37,15 +37,13 @@ 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 = true; +options.Mono = false; % fps for image options.fps = 20; % camera flying speed options.speed = 20; - - %% test1: visibility test in monocular camera cylinders{1}.centroid = Point3(30, 50, 5); cylinders{2}.centroid = Point3(50, 50, 5); @@ -131,23 +129,12 @@ else options.imageSize, cylinders); figID = 2; - figure(figID) + plotProjectedCylinderSamples(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, 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 %% plot all the projected points -%plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); % plot the 2D tracks From 26df490c55b1750496728c1a4ebb39640d9fea4d Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 15 Jan 2015 01:19:21 -0500 Subject: [PATCH 23/38] remove dulipcate points in stereo camera set up --- matlab/+gtsam/plotCylinderSamples.m | 4 +- matlab/+gtsam/plotProjectedCylinderSamples.m | 7 +- matlab/+gtsam/points2DTrackMonocular.m | 50 ++++++------ matlab/+gtsam/points2DTrackStereo.m | 82 ++++++++++++++------ matlab/gtsam_examples/CameraFlyingExample.m | 4 +- 5 files changed, 95 insertions(+), 52 deletions(-) diff --git a/matlab/+gtsam/plotCylinderSamples.m b/matlab/+gtsam/plotCylinderSamples.m index 74768f641..ec1795dea 100644 --- a/matlab/+gtsam/plotCylinderSamples.m +++ b/matlab/+gtsam/plotCylinderSamples.m @@ -1,4 +1,4 @@ -function plotCylinderSamples(cylinders, fieldSize, figID) +function plotCylinderSamples(cylinders, options, figID) % plot the cylinders on the given field % @author: Zhaoyang Lv @@ -24,7 +24,7 @@ function plotCylinderSamples(cylinders, fieldSize, figID) end axis equal - axis([0, fieldSize.x, 0, fieldSize.y, 0, 20]); + axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); grid on diff --git a/matlab/+gtsam/plotProjectedCylinderSamples.m b/matlab/+gtsam/plotProjectedCylinderSamples.m index a58526632..f59434d96 100644 --- a/matlab/+gtsam/plotProjectedCylinderSamples.m +++ b/matlab/+gtsam/plotProjectedCylinderSamples.m @@ -1,5 +1,5 @@ -function plotProjectedCylinderSamples(pts3d, covariance, figID) -% plot the visible projected points on the cylinders +function plotProjectedCylinderSamples(pts3d, covariance, options, figID) +% plot the visible points on the cylinders % author: Zhaoyang Lv import gtsam.* @@ -11,7 +11,8 @@ function plotProjectedCylinderSamples(pts3d, covariance, figID) pointsNum = length(pts3d); for i = 1:pointsNum - plotPoint3(pts3d{i}, 'green', covariance{i}); + %gtsam.plotPoint3(p, 'g', covariance{i}); + plotPoint3(pts3d{i}, 'r', covariance{i}); hold on end diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 83bf45fbd..a4e1a2e5c 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -59,6 +59,13 @@ for i = 1:cameraPosesNum end for i = 1:pointsNum + % add in values + point_j = points3d{i}.data.retract(0.1*randn(3,1)); + initialEstimate.insert(symbol('p', i), point_j); + + if ~points3d{i}.visiblity + continue; + end % single measurement. not added to graph factorNum = length(points3d{i}.Z); if factorNum > 1 @@ -68,41 +75,40 @@ for i = 1:pointsNum measurementNoise, symbol('x', cameraIdx), symbol('p', points3d{i}.cameraConstraint{j}), K) ); end end - - % add in values - point_j = points3d{i}.data.retract(0.1*randn(3,1)); - initialEstimate.insert(symbol('p', i), point_j); - + end %% Print the graph graph.print(sprintf('\nFactor graph:\n')); +%% linearize the graph +% currently throws the Indeterminant linear system exception marginals = Marginals(graph, initialEstimate); %% get all the points track information -% currently throws the Indeterminant linear system exception for i = 1:pointsNum - if points3d{i}.visiblity - pts2dTracksMono.pt3d{i} = points3d{i}.data; - pts2dTracksMono.Z = points3d{i}.Z; + if ~points3d{i}.visiblity + continue; + end + + pts2dTracksMono.pt3d{end+1} = points3d{i}.data; + pts2dTracksMono.Z{end+1} = points3d{i}.Z; - if length(points3d{i}.Z) == 1 - %pts2dTracksMono.cov{i} singular matrix - else - pts2dTracksMono.cov{i} = marginals.marginalCovariance(symbol('p', i)); - end + if length(points3d{i}.Z) == 1 + %pts2dTracksMono.cov{i} singular matrix + else + pts2dTracksMono.cov{end+1} = marginals.marginalCovariance(symbol('p', i)); 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 +% 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; diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 141596cea..2359654d3 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -19,64 +19,100 @@ cameraPosesNum = length(cameraPoses); %% add measurements and initial camera & points values pointsNum = 0; cylinderNum = length(cylinders); +points3d = cell(0); for i = 1:cylinderNum + cylinderPointsNum = length(cylinders{i}.Points); pointsNum = pointsNum + length(cylinders{i}.Points); + for j = 1:cylinderPointsNum + points3d{end+1}.data = cylinders{i}.Points{j}; + points3d{end}.Z = cell(0); + points3d{end}.cameraConstraint = cell(0); + points3d{end}.visiblity = false; + end end +graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise)); + pts3d = cell(cameraPosesNum, 1); initialEstimate = Values; -initialized = false; for i = 1:cameraPosesNum pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, imageSize, cylinders); - if ~initialized - graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{i}, posePriorNoise)); - initialized = true; + if isempty(pts3d{i}.Z) + continue; end measurementNum = length(pts3d{i}.Z); - if measurementNum < 1 - continue; - end for j = 1:measurementNum + index = pts3d{i}.overallIdx{j}; + points3d{index}.Z{end+1} = pts3d{i}.Z{j}; + points3d{index}.cameraConstraint{end+1} = i; + points3d{index}.visiblity = true; + end + + for j = 1:length(pts3d{i}.Z) graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ... stereoNoise, symbol('x', i), symbol('p', pts3d{i}.overallIdx{j}), K)); end end -%% initialize cameras and points close to ground truth +%% initialize graph and values for i = 1:cameraPosesNum pose_i = cameraPoses{i}.retract(0.1*randn(6,1)); initialEstimate.insert(symbol('x', i), pose_i); end -ptsIdx = 0; -for i = 1:length(cylinders) - for j = 1:length(cylinders{i}.Points) - ptsIdx = ptsIdx + 1; - point_j = cylinders{i}.Points{j}.retract(0.1*randn(3,1)); - initialEstimate.insert(symbol('p', ptsIdx), point_j); + +for i = 1:pointsNum + point_j = points3d{i}.data.retract(0.1*randn(3,1)); + initialEstimate.insert(symbol('p', i), point_j); + + if ~points3d{i}.visiblity + continue; end + + factorNum = length(points3d{i}.Z); + for j = 1:factorNum + cameraIdx = points3d{i}.cameraConstraint{j}; + graph.add(GenericStereoFactor3D(StereoPoint2(points3d{i}.Z{j}.uL, points3d{i}.Z{j}.uR, points3d{i}.Z{j}.v), ... + stereoNoise, symbol('x', cameraIdx), symbol('p', points3d{i}.cameraConstraint{j}), K)); + end end + %% Print the graph graph.print(sprintf('\nFactor graph:\n')); +%% linearize the graph marginals = Marginals(graph, initialEstimate); %% get all the 2d points track information -% currently throws the Indeterminant linear system exception -for k = 1:cameraPosesNum - num = length(pts3d{k}.data); - if num < 1 +pts2dTracksStereo.pt3d = cell(0); +pts2dTracksStereo.Z = cell(0); +pts2dTracksStereo.cov = cell(0); +for i = 1:pointsNum + if ~points3d{i}.visiblity continue; end - for i = 1:num - pts2dTracksStereo.pt3d{i} = pts3d{k}.data{i}; - pts2dTracksStereo.Z{i} = pts3d{k}.Z{i}; - pts2dTracksStereo.cov{i} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{i})); - end + + pts2dTracksStereo.pt3d{end+1} = points3d{i}.data; + pts2dTracksStereo.Z{end+1} = points3d{i}.Z; + pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i)); + 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 hold on; %plot3DPoints(initialEstimate, [], marginals); diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index eb047b50a..ba9434080 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -87,7 +87,7 @@ end % now is plotting on a 100 * 100 field figID = 1; figure(figID); -plotCylinderSamples(cylinders, options.fieldSize, figID); +plotCylinderSamples(cylinders, options, figID); % %% generate ground truth camera trajectories: a circle % KMono = Cal3_S2(525,525,0,320,240); @@ -129,7 +129,7 @@ else options.imageSize, cylinders); figID = 2; - plotProjectedCylinderSamples(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, figID); + plotProjectedCylinderSamples(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, options, figID); end From d0579dff50221c9fa79eed0456137ad31af3f253 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 15 Jan 2015 16:17:53 +0100 Subject: [PATCH 24/38] Wrapping Cal3Unified as a derived class of Cal3DS2_Base --- gtsam.h | 79 +++++++++++++++++++++++++---------- gtsam/geometry/Cal3DS2.h | 14 ++++++- gtsam/geometry/Cal3DS2_Base.h | 28 ++++++++++--- gtsam/geometry/Cal3Unified.h | 10 +++-- 4 files changed, 100 insertions(+), 31 deletions(-) diff --git a/gtsam.h b/gtsam.h index 1fbc0f907..c5528309e 100644 --- a/gtsam.h +++ b/gtsam.h @@ -629,28 +629,13 @@ class Cal3_S2 { void serialize() const; }; -#include -class Cal3DS2 { +#include +virtual class Cal3DS2_Base { // Standard Constructors - Cal3DS2(); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4); - Cal3DS2(Vector v); + Cal3DS2_Base(); // Testable void print(string s) const; - bool equals(const gtsam::Cal3DS2& rhs, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3DS2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3DS2& c) const; - - // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; - gtsam::Point2 calibrate(const gtsam::Point2& p) const; - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - // TODO: D2d functions that start with an uppercase letter // Standard Interface double fx() const; @@ -658,14 +643,66 @@ class Cal3DS2 { double skew() const; double px() const; double py() const; - Vector vector() const; - Vector k() const; - //Matrix K() const; //FIXME: Uppercase + double k1() const; + double k2() const; + + // Action on Point2 + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; // enabling serialization functionality void serialize() const; }; +#include +virtual class Cal3DS2 : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3DS2(); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2); + Cal3DS2(Vector v); + + // Testable + bool equals(const gtsam::Cal3DS2& rhs, double tol) const; + + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3DS2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3DS2& c) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class Cal3Unified : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3Unified(); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi); + Cal3Unified(Vector v); + + // Testable + bool equals(const gtsam::Cal3Unified& rhs, double tol) const; + + // Standard Interface + double xi() const; + gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const; + gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const; + + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3Unified retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Unified& c) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include class Cal3_S2Stereo { // Standard Constructors Cal3_S2Stereo(); diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 0c77eebbc..2fb0c8653 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -68,7 +68,7 @@ public: /// @{ /// print with optional string - void print(const std::string& s = "") const ; + virtual void print(const std::string& s = "") const ; /// assert equality up to a tolerance bool equals(const Cal3DS2& K, double tol = 10e-9) const; @@ -89,10 +89,20 @@ public: /// Return dimensions of calibration manifold object static size_t Dim() { return 9; } //TODO: make a final dimension variable + /// @} + /// @name Clone + /// @{ + + /// @return a deep copy of this object + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(new Cal3DS2(*this)); + } + + /// @} + private: - /// @} /// @name Advanced Interface /// @{ diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index 37c156743..d4f4cabe5 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -45,9 +45,6 @@ protected: double p1_, p2_ ; // tangential distortion public: - Matrix3 K() const ; - Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); } - Vector9 vector() const ; /// @name Standard Constructors /// @{ @@ -59,6 +56,8 @@ public: double k1, double k2, double p1 = 0.0, double p2 = 0.0) : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {} + virtual ~Cal3DS2_Base() {} + /// @} /// @name Advanced Constructors /// @{ @@ -70,7 +69,7 @@ public: /// @{ /// print with optional string - void print(const std::string& s = "") const ; + virtual void print(const std::string& s = "") const ; /// assert equality up to a tolerance bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const; @@ -106,6 +105,15 @@ public: /// Second tangential distortion coefficient inline double p2() const { return p2_;} + /// return calibration matrix -- not really applicable + Matrix3 K() const; + + /// return distortion parameter vector + Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); } + + /// Return all parameters as a vector + Vector9 vector() const; + /** * convert intrinsic coordinates xy to (distorted) image coordinates uv * @param p point in intrinsic coordinates @@ -126,9 +134,19 @@ public: /// Derivative of uncalibrate wrpt the calibration parameters Matrix29 D2d_calibration(const Point2& p) const ; -private: + /// @} + /// @name Clone + /// @{ + + /// @return a deep copy of this object + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(new Cal3DS2_Base(*this)); + } /// @} + +private: + /// @name Advanced Interface /// @{ diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index d0e0f3891..8e4394c0d 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -50,9 +50,8 @@ private: double xi_; // mirror parameter public: - enum { dimension = 10 }; - Vector10 vector() const ; + enum { dimension = 10 }; /// @name Standard Constructors /// @{ @@ -77,7 +76,7 @@ public: /// @{ /// print with optional string - void print(const std::string& s = "") const ; + virtual void print(const std::string& s = "") const ; /// assert equality up to a tolerance bool equals(const Cal3Unified& K, double tol = 10e-9) const; @@ -125,6 +124,11 @@ public: /// Return dimensions of calibration manifold object static size_t Dim() { return 10; } //TODO: make a final dimension variable + /// Return all parameters as a vector + Vector10 vector() const ; + + /// @} + private: /** Serialization function */ From d42391d28d7b67787017c395a765fe800286fe28 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 15 Jan 2015 16:20:22 +0100 Subject: [PATCH 25/38] Added test of Cal3Unified and cleaned up a bit --- matlab/gtsam_tests/testCal3Unified.m | 7 +++++++ matlab/gtsam_tests/test_gtsam.m | 21 +++++++++++++++------ 2 files changed, 22 insertions(+), 6 deletions(-) create mode 100644 matlab/gtsam_tests/testCal3Unified.m diff --git a/matlab/gtsam_tests/testCal3Unified.m b/matlab/gtsam_tests/testCal3Unified.m new file mode 100644 index 000000000..498c65343 --- /dev/null +++ b/matlab/gtsam_tests/testCal3Unified.m @@ -0,0 +1,7 @@ +% test Cal3Unified +import gtsam.*; + +K = Cal3Unified; +EXPECT('fx',K.fx()==1); +EXPECT('fy',K.fy()==1); + diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index 1a6856a9a..4cbe42364 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -1,17 +1,25 @@ % Test runner script - runs each test -% display 'Starting: testPriorFactor' -% testPriorFactor +%% geometry +display 'Starting: testCal3Unified' +testCal3Unified -display 'Starting: testValues' -testValues +%% linear +display 'Starting: testKalmanFilter' +testKalmanFilter display 'Starting: testJacobianFactor' testJacobianFactor -display 'Starting: testKalmanFilter' -testKalmanFilter +%% nonlinear +display 'Starting: testValues' +testValues +%% SLAM +display 'Starting: testPriorFactor' +testPriorFactor + +%% examples display 'Starting: testLocalizationExample' testLocalizationExample @@ -36,6 +44,7 @@ testStereoVOExample display 'Starting: testVisualISAMExample' testVisualISAMExample +%% MATLAB specific display 'Starting: testUtilities' testUtilities From b202bbd5f1c6d589270e90d421bcd7fd74184780 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Mon, 19 Jan 2015 16:18:18 -0500 Subject: [PATCH 26/38] add in simulated camera options --- matlab/+gtsam/plotFlyingResults.m | 41 ++++ matlab/+gtsam/plotProjectedCylinderSamples.m | 42 ---- matlab/+gtsam/points2DTrackMonocular.m | 12 -- matlab/+gtsam/points2DTrackStereo.m | 22 +-- matlab/gtsam_examples/CameraFlyingExample.m | 193 +++++++++++-------- 5 files changed, 160 insertions(+), 150 deletions(-) create mode 100644 matlab/+gtsam/plotFlyingResults.m delete mode 100644 matlab/+gtsam/plotProjectedCylinderSamples.m diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m new file mode 100644 index 000000000..22b6a2de5 --- /dev/null +++ b/matlab/+gtsam/plotFlyingResults.m @@ -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 \ No newline at end of file diff --git a/matlab/+gtsam/plotProjectedCylinderSamples.m b/matlab/+gtsam/plotProjectedCylinderSamples.m deleted file mode 100644 index f59434d96..000000000 --- a/matlab/+gtsam/plotProjectedCylinderSamples.m +++ /dev/null @@ -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 \ No newline at end of file diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index a4e1a2e5c..04c896188 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -101,20 +101,8 @@ for i = 1:pointsNum 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); %plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); -plot3DTrajectory(initialEstimate, '*', 1, 8); view(3); diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 2359654d3..95f225ea5 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -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. % After creation of the factor graph for each track, linearize it around ground truth. % There is no optimization @@ -100,24 +100,10 @@ for i = 1:pointsNum 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 -hold on; -%plot3DPoints(initialEstimate, [], marginals); +plotFlyingResults(pts2dTracksStereo.pts3d, pts2dTracksStereo.cov, initialiEstimate, marginals); + %plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); -plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); -view(3); +%view(3); end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index ba9434080..08986f83c 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -10,100 +10,135 @@ % @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 = 20; -% point density on cylinder -options.density = 1; +% test or run +options.enableTests = false; -% The number of camera poses -options.poseNum = 20; -% covariance scaling factor +% the number of cylinders in the field +options.cylinder.cylinderNum = 15; % pls be smaller than 20 +% 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; - -%% Camera Setup -% Monocular Camera Calibration -options.monoK = Cal3_S2(525,525,0,320,240); -% 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; -% fps for image -options.fps = 20; -% camera flying speed -options.speed = 20; +% if(options.writeVideo) +% videoObj = VideoWriter('Camera_Flying_Example.avi'); +% videoObj.Quality = 100; +% videoObj.FrameRate = options.fps; +% end -%% test1: visibility test in monocular camera -cylinders{1}.centroid = Point3(30, 50, 5); -cylinders{2}.centroid = Point3(50, 50, 5); -cylinders{3}.centroid = Point3(70, 50, 5); +%% This is for tests +if options.enableTests + % test1: visibility test in monocular camera + 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)); + 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(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 -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.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; +%% generate a set of cylinders and point samples on cylinders +cylinderNum = options.cylinder.cylinderNum; cylinders = cell(cylinderNum, 1); - -% It seems random generated cylinders doesn't work that well -% Now it set up a circle of cylinders +baseCentroid = cell(cylinderNum, 1); theta = 0; -for i = 1:cylinderNum +i = 1; +while i <= cylinderNum 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; - baseCentroid = Point2([x, y]'); - cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, options.density); + baseCentroid{i} = Point2([x, y]'); + + % 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 - %% plot all the cylinders and sampled points % now is plotting on a 100 * 100 field figID = 1; figure(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 KMono = Cal3_S2(525,525,0,320,240); cameraPoses = cell(options.poseNum, 1); @@ -113,24 +148,22 @@ for i = 1:options.poseNum 15, 10]'); camera = SimpleCamera.Lookat(t, ... 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; end %% set up camera and get measurements -if options.Mono +if options.camera.IS_MONO % use Monocular Camera - pts2dTracksMono = points2DTrackMonocular(options.monoK, cameraPoses, ... - options.imageSize, cylinders); + pts2dTracksMono = points2DTrackMonocular(options.camera.monoK, cameraPoses, ... + options.camera.resolution, cylinders); else % use Stereo Camera - pts2dTracksStereo = points2DTrackStereo(options.stereoK, cameraPoses, ... - options.imageSize, cylinders); + [pts2dTracksStereo, estimateValuesStereo] = points2DTrackStereo(options.camera.stereoK, ... + cameraPoses, options.camera.resolution, cylinders); - figID = 2; - plotProjectedCylinderSamples(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, options, figID); - + plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, estimateValuesStereo, options, figID); end %% plot all the projected points @@ -142,6 +175,10 @@ end %plot3DTrajectory(); +%%close video +% if(options.writeVideo) +% close(videoObj); +% end From 1c5cdb830b3cbcb955ae9ae51425e8c286fd1241 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Mon, 19 Jan 2015 16:52:48 -0500 Subject: [PATCH 27/38] change of point density to make it plotable --- matlab/+gtsam/plotFlyingResults.m | 5 +++-- matlab/+gtsam/points2DTrackStereo.m | 2 +- matlab/gtsam_examples/CameraFlyingExample.m | 4 +--- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m index 22b6a2de5..c214d948d 100644 --- a/matlab/+gtsam/plotFlyingResults.m +++ b/matlab/+gtsam/plotFlyingResults.m @@ -24,16 +24,17 @@ for i = 0:keys.size - 1 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 diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 95f225ea5..3651bff6d 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -101,7 +101,7 @@ for i = 1:pointsNum end %% plot the result with covariance ellipses -plotFlyingResults(pts2dTracksStereo.pts3d, pts2dTracksStereo.cov, initialiEstimate, marginals); +plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, initialEstimate, marginals); %plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); %view(3); diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 08986f83c..99fe4ca80 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -26,7 +26,7 @@ options.cylinder.cylinderNum = 15; % pls be smaller than 20 options.cylinder.radius = 3; % pls be smaller than 5 options.cylinder.height = 10; % point density on cylinder -options.cylinder.pointDensity = 1; +options.cylinder.pointDensity = 0.05; %% set up the camera % parameters set according to the stereo camera: @@ -162,8 +162,6 @@ else % use Stereo Camera [pts2dTracksStereo, estimateValuesStereo] = points2DTrackStereo(options.camera.stereoK, ... cameraPoses, options.camera.resolution, cylinders); - - plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, estimateValuesStereo, options, figID); end %% plot all the projected points From 73455833fc23889104049e225ca632291660f2f4 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Mon, 19 Jan 2015 23:56:04 -0500 Subject: [PATCH 28/38] ploting trajectories animation --- matlab/+gtsam/covarianceEllipse3D.m | 2 +- matlab/+gtsam/plotFlyingResults.m | 86 ++++++++++++++++----- matlab/+gtsam/points2DTrackStereo.m | 34 ++++---- matlab/gtsam_examples/CameraFlyingExample.m | 27 +------ 4 files changed, 83 insertions(+), 66 deletions(-) diff --git a/matlab/+gtsam/covarianceEllipse3D.m b/matlab/+gtsam/covarianceEllipse3D.m index 9682a9fc1..99a164068 100644 --- a/matlab/+gtsam/covarianceEllipse3D.m +++ b/matlab/+gtsam/covarianceEllipse3D.m @@ -1,4 +1,4 @@ -function covarianceEllipse3D(c,P) +function sc = covarianceEllipse3D(c,P) % covarianceEllipse3D plots a Gaussian as an uncertainty ellipse % Based on Maybeck Vol 1, page 366 % k=2.296 corresponds to 1 std, 68.26% of all probability diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m index c214d948d..3c3e1e670 100644 --- a/matlab/+gtsam/plotFlyingResults.m +++ b/matlab/+gtsam/plotFlyingResults.m @@ -1,42 +1,90 @@ -function plotFlyingResults(pts3d, covariance, values, marginals) +function plotFlyingResults(pts3d, pts3dCov, poses, posesCov, cylinders, options) % 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); +if(options.writeVideo) + videoObj = VideoWriter('Camera_Flying_Example.avi'); + videoObj.Quality = 100; + videoObj.FrameRate = options.camera.fps; +end + +%% plot all the cylinders and sampled points +% now is plotting on a 100 * 100 field +figID = 1; +figure(figID); + +axis equal +axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); + +view(3); + +sampleDensity = 120; +cylinderNum = length(cylinders); +for i = 1:cylinderNum + [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height); + + X = X + cylinders{i}.centroid.x; + Y = Y + cylinders{i}.centroid.y; + Z = Z * cylinders{i}.height; + + cylinderHandle = surf(X,Y,Z); + set(cylinderHandle, 'FaceAlpha', 0.5); + hold on +end %% plot trajectories -for i = 0:keys.size - 1 - if exist('h_result', 'var') - delete(h_result); +posesSize = length(poses); +for i = 1:posesSize + if i > 1 + plot3([poses{i}.x; poses{i-1}.x], [poses{i}.y; poses{i-1}.y], [poses{i}.z; poses{i-1}.z], '-b'); end - - key = keys.at(i); - pose = keys.at(key); - P = marginals.marginalCovariance(key); + if exist('h_cov', 'var') + delete(h_cov); + end - h_result = gtsam.plotPose3(pose, P, 1); - + gRp = poses{i}.rotation().matrix(); % rotation from pose to global + C = poses{i}.translation().vector(); + axisLength = 2; + + xAxis = C+gRp(:,1)*axisLength; + L = [C xAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','r'); + + yAxis = C+gRp(:,2)*axisLength; + L = [C yAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','g'); + + zAxis = C+gRp(:,3)*axisLength; + L = [C zAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','b'); + + pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame + gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame + h_cov = gtsam.covarianceEllipse3D(C,gPp); + + drawnow; end %% plot point covariance - - -if exist('h_result', 'var') - delete(h_result); +pointSize = length(pts3d); +for i = 1:pointSize + end if ~holdstate hold off end - + +%%close video +if(options.writeVideo) + close(videoObj); +end + end \ No newline at end of file diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 3651bff6d..c80c90d67 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -1,4 +1,4 @@ -function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(K, cameraPoses, imageSize, cylinders) +function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(K, cameraPoses, options, 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 @@ -36,7 +36,7 @@ graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise)); pts3d = cell(cameraPosesNum, 1); initialEstimate = Values; for i = 1:cameraPosesNum - pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, imageSize, cylinders); + pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, options.camera.resolution, cylinders); if isempty(pts3d{i}.Z) continue; @@ -48,12 +48,11 @@ for i = 1:cameraPosesNum points3d{index}.Z{end+1} = pts3d{i}.Z{j}; points3d{index}.cameraConstraint{end+1} = i; points3d{index}.visiblity = true; + + graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ... + stereoNoise, symbol('x', i), symbol('p', index), K)); end - for j = 1:length(pts3d{i}.Z) - graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ... - stereoNoise, symbol('x', i), symbol('p', pts3d{i}.overallIdx{j}), K)); - end end %% initialize graph and values @@ -64,18 +63,7 @@ end for i = 1:pointsNum point_j = points3d{i}.data.retract(0.1*randn(3,1)); - initialEstimate.insert(symbol('p', i), point_j); - - if ~points3d{i}.visiblity - continue; - end - - factorNum = length(points3d{i}.Z); - for j = 1:factorNum - cameraIdx = points3d{i}.cameraConstraint{j}; - graph.add(GenericStereoFactor3D(StereoPoint2(points3d{i}.Z{j}.uL, points3d{i}.Z{j}.uR, points3d{i}.Z{j}.v), ... - stereoNoise, symbol('x', cameraIdx), symbol('p', points3d{i}.cameraConstraint{j}), K)); - end + initialEstimate.insert(symbol('p', i), point_j); end @@ -96,12 +84,16 @@ for i = 1:pointsNum pts2dTracksStereo.pt3d{end+1} = points3d{i}.data; pts2dTracksStereo.Z{end+1} = points3d{i}.Z; - pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i)); - + pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i)); +end + +cameraPosesCov = cell(cameraPosesNum, 1); +for i = 1:cameraPosesNum + cameraPosesCov{i} = marginals.marginalCovariance(symbol('x', i)); end %% plot the result with covariance ellipses -plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, initialEstimate, marginals); +plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, cameraPoses, cameraPosesCov, cylinders, options); %plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); %view(3); diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 99fe4ca80..bcb3861f3 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -37,7 +37,7 @@ options.camera.IS_MONO = false; % the field of view of camera options.camera.fov = 120; % fps for image -options.camera.fps = 25; +options.camera.fps = 10; % camera pixel resolution options.camera.resolution = Point2(752, 480); % camera horizon @@ -69,12 +69,6 @@ options.poseNum = options.fieldSize.x / (options.speed / options.camera.fps); % display covariance scaling factor options.scale = 1; -% if(options.writeVideo) -% videoObj = VideoWriter('Camera_Flying_Example.avi'); -% videoObj.Quality = 100; -% videoObj.FrameRate = options.fps; -% end - %% This is for tests if options.enableTests @@ -133,12 +127,6 @@ while i <= cylinderNum i = i+1; end -%% plot all the cylinders and sampled points -% now is plotting on a 100 * 100 field -figID = 1; -figure(figID); -plotCylinderSamples(cylinders, options, figID); - %% generate ground truth camera trajectories: a line KMono = Cal3_S2(525,525,0,320,240); cameraPoses = cell(options.poseNum, 1); @@ -161,22 +149,11 @@ if options.camera.IS_MONO else % use Stereo Camera [pts2dTracksStereo, estimateValuesStereo] = points2DTrackStereo(options.camera.stereoK, ... - cameraPoses, options.camera.resolution, cylinders); + cameraPoses, options, cylinders); end -%% plot all the projected points -% plot the 2D tracks - -% ToDo: plot the trajectories -%plot3DTrajectory(); - - -%%close video -% if(options.writeVideo) -% close(videoObj); -% end From 47c68f678c51be0668c92eda79f66c066f78bcb1 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 20 Jan 2015 14:06:39 -0500 Subject: [PATCH 29/38] generate a flying camera video --- matlab/+gtsam/plotFlyingResults.m | 37 +++++++++++++++++++++++++++++-- 1 file changed, 35 insertions(+), 2 deletions(-) diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m index 3c3e1e670..77a096f17 100644 --- a/matlab/+gtsam/plotFlyingResults.m +++ b/matlab/+gtsam/plotFlyingResults.m @@ -11,6 +11,7 @@ if(options.writeVideo) videoObj = VideoWriter('Camera_Flying_Example.avi'); videoObj.Quality = 100; videoObj.FrameRate = options.camera.fps; + open(videoObj); end %% plot all the cylinders and sampled points @@ -32,11 +33,18 @@ for i = 1:cylinderNum Y = Y + cylinders{i}.centroid.y; Z = Z * cylinders{i}.height; - cylinderHandle = surf(X,Y,Z); - set(cylinderHandle, 'FaceAlpha', 0.5); + h_cylinder = surf(X,Y,Z); + set(h_cylinder, 'FaceAlpha', 0.5); hold on end +drawnow; + +if options.writeVideo + currFrame = getframe(gcf); + writeVideo(videoObj, currFrame); +end + %% plot trajectories posesSize = length(poses); for i = 1:posesSize @@ -69,13 +77,38 @@ for i = 1:posesSize h_cov = gtsam.covarianceEllipse3D(C,gPp); drawnow; + + if options.writeVideo + currFrame = getframe(gcf); + writeVideo(videoObj, currFrame); + end end + +if exist('h_cov', 'var') + delete(h_cov); +end + +% wait for two seconds +pause(2); + + %% plot point covariance +% if exist('h_cylinder', 'var') +% delete(h_cylinder); +% end + pointSize = length(pts3d); for i = 1:pointSize + plot3(pts3d{i}.x, pts3d{i}.y, pts3d{i}.z); + gtsam.covarianceEllipse3D([pts3d{i}.x; pts3d{i}.y; pts3d{i}.z], pts3dCov{i}); + %drawnow; + if options.writeVideo + currFrame = getframe(gcf); + writeVideo(videoObj, currFrame); + end end if ~holdstate From 5cde63acd26968924186927e64960b74aebb2c32 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Wed, 21 Jan 2015 00:14:37 -0500 Subject: [PATCH 30/38] plot incremental position --- matlab/+gtsam/plotFlyingResults.m | 46 +++++++++++------ matlab/+gtsam/points2DTrackStereo.m | 57 ++++++++++----------- matlab/gtsam_examples/CameraFlyingExample.m | 15 ++++-- 3 files changed, 67 insertions(+), 51 deletions(-) diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m index 77a096f17..7a0ca77c8 100644 --- a/matlab/+gtsam/plotFlyingResults.m +++ b/matlab/+gtsam/plotFlyingResults.m @@ -1,4 +1,4 @@ -function plotFlyingResults(pts3d, pts3dCov, poses, posesCov, cylinders, options) +function plotFlyingResults(pts3d, poses, posesCov, cylinders, options) % plot the visible points on the cylinders and trajectories % author: Zhaoyang Lv @@ -45,15 +45,16 @@ if options.writeVideo writeVideo(videoObj, currFrame); end -%% plot trajectories +%% plot trajectories and points posesSize = length(poses); +pointSize = length(pts3d); for i = 1:posesSize if i > 1 plot3([poses{i}.x; poses{i-1}.x], [poses{i}.y; poses{i-1}.y], [poses{i}.z; poses{i-1}.z], '-b'); end if exist('h_cov', 'var') - delete(h_cov); + delete(h_pose_cov); end gRp = poses{i}.rotation().matrix(); % rotation from pose to global @@ -74,7 +75,28 @@ for i = 1:posesSize pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame - h_cov = gtsam.covarianceEllipse3D(C,gPp); + h_pose_cov = gtsam.covarianceEllipse3D(C,gPp); + + + if exist('h_point', 'var') + for j = 1:pointSize + delete(h_point{j}); + end + end + if exist('h_point_cov', 'var') + for j = 1:pointSize + delete(h_point_cov{j}); + end + end + + h_point = cell(pointSize, 1); + h_point_cov = cell(pointSize, 1); + for j = 1:pointSize + if ~isempty(pts3d{j}.cov{i}) + h_point{j} = plot3(pts3d{j}.data.x, pts3d{j}.data.y, pts3d{j}.data.z); + h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], pts3d{j}.cov{i}); + end + end drawnow; @@ -85,8 +107,8 @@ for i = 1:posesSize end -if exist('h_cov', 'var') - delete(h_cov); +if exist('h_pose_cov', 'var') + delete(h_pose_cov); end % wait for two seconds @@ -99,17 +121,7 @@ pause(2); % delete(h_cylinder); % end -pointSize = length(pts3d); -for i = 1:pointSize - plot3(pts3d{i}.x, pts3d{i}.y, pts3d{i}.z); - gtsam.covarianceEllipse3D([pts3d{i}.x; pts3d{i}.y; pts3d{i}.z], pts3dCov{i}); - %drawnow; - - if options.writeVideo - currFrame = getframe(gcf); - writeVideo(videoObj, currFrame); - end -end + if ~holdstate hold off diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index c80c90d67..295b4d18e 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -12,7 +12,7 @@ graph = NonlinearFactorGraph; %% create the noise factors poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); -stereoNoise = noiseModel.Isotropic.Sigma(3,0.2); +stereoNoise = noiseModel.Isotropic.Sigma(3, 0.05); cameraPosesNum = length(cameraPoses); @@ -28,13 +28,21 @@ for i = 1:cylinderNum points3d{end}.Z = cell(0); points3d{end}.cameraConstraint = cell(0); points3d{end}.visiblity = false; + points3d{end}.cov = cell(cameraPosesNum); end end graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise)); -pts3d = cell(cameraPosesNum, 1); +%% initialize graph and values initialEstimate = Values; +for i = 1:pointsNum + point_j = points3d{i}.data.retract(0.05*randn(3,1)); + initialEstimate.insert(symbol('p', i), point_j); +end + +pts3d = cell(cameraPosesNum, 1); +cameraPosesCov = cell(cameraPosesNum, 1); for i = 1:cameraPosesNum pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, options.camera.resolution, cylinders); @@ -52,26 +60,24 @@ for i = 1:cameraPosesNum graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ... stereoNoise, symbol('x', i), symbol('p', index), K)); end + + pose_i = cameraPoses{i}.retract(poseNoiseSigmas); + initialEstimate.insert(symbol('x', i), pose_i); + + %% linearize the graph + marginals = Marginals(graph, initialEstimate); + + for j = 1:pointsNum + if points3d{j}.visiblity + points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p', j)); + end + end + cameraPosesCov{i} = marginals.marginalCovariance(symbol('x', i)); end -%% initialize graph and values -for i = 1:cameraPosesNum - pose_i = cameraPoses{i}.retract(0.1*randn(6,1)); - initialEstimate.insert(symbol('x', i), pose_i); -end - -for i = 1:pointsNum - point_j = points3d{i}.data.retract(0.1*randn(3,1)); - initialEstimate.insert(symbol('p', i), point_j); -end - - -%% Print the graph -graph.print(sprintf('\nFactor graph:\n')); - -%% linearize the graph -marginals = Marginals(graph, initialEstimate); +%% Plot the result +plotFlyingResults(points3d, cameraPoses, cameraPosesCov, cylinders, options); %% get all the 2d points track information pts2dTracksStereo.pt3d = cell(0); @@ -87,15 +93,8 @@ for i = 1:pointsNum pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i)); end -cameraPosesCov = cell(cameraPosesNum, 1); -for i = 1:cameraPosesNum - cameraPosesCov{i} = marginals.marginalCovariance(symbol('x', i)); -end - -%% plot the result with covariance ellipses -plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, cameraPoses, cameraPosesCov, cylinders, options); - -%plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); -%view(3); +% +% %% plot the result with covariance ellipses +% plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, cameraPoses, cameraPosesCov, cylinders, options); end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index bcb3861f3..01e335d15 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -20,6 +20,7 @@ import gtsam.* % test or run options.enableTests = false; +%% cylinder options % the number of cylinders in the field options.cylinder.cylinderNum = 15; % pls be smaller than 20 % cylinder size @@ -28,7 +29,7 @@ options.cylinder.height = 10; % point density on cylinder options.cylinder.pointDensity = 0.05; -%% set up the camera +%% camera options % parameters set according to the stereo camera: % http://www.matrix-vision.com/USB2.0-single-board-camera-mvbluefox-mlc.html @@ -66,9 +67,14 @@ options.fieldSize = Point2([100, 100]'); 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; +%% ploting options +% display covariance scaling factor +options.plot.scale = 1; +% plot the trajectory covariance +options.plot.DISP_TRAJ_COV = true; +% plot points covariance +options.plot.POINTS_COV = true; %% This is for tests if options.enableTests @@ -140,7 +146,6 @@ for i = 1:options.poseNum cameraPoses{i} = camera.pose; end - %% set up camera and get measurements if options.camera.IS_MONO % use Monocular Camera @@ -148,7 +153,7 @@ if options.camera.IS_MONO options.camera.resolution, cylinders); else % use Stereo Camera - [pts2dTracksStereo, estimateValuesStereo] = points2DTrackStereo(options.camera.stereoK, ... + pts2dTracksStereo = points2DTrackStereo(options.camera.stereoK, ... cameraPoses, options, cylinders); end From 2627f9a9cd23b3b4aace8232f26de6a745baa715 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Wed, 21 Jan 2015 16:18:03 -0500 Subject: [PATCH 31/38] flying camera view changes --- matlab/+gtsam/plotFlyingResults.m | 15 ++++++++++----- matlab/+gtsam/points2DTrackMonocular.m | 2 -- matlab/gtsam_examples/CameraFlyingExample.m | 2 +- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m index 7a0ca77c8..d09842934 100644 --- a/matlab/+gtsam/plotFlyingResults.m +++ b/matlab/+gtsam/plotFlyingResults.m @@ -19,10 +19,7 @@ end figID = 1; figure(figID); -axis equal -axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); - -view(3); +view([30, 0]); sampleDensity = 120; cylinderNum = length(cylinders); @@ -34,7 +31,7 @@ for i = 1:cylinderNum Z = Z * cylinders{i}.height; h_cylinder = surf(X,Y,Z); - set(h_cylinder, 'FaceAlpha', 0.5); + set(h_cylinder, 'FaceColor', [0 0 0.5], 'FaceAlpha', 0.2, 'EdgeColor', [0 0 1]); hold on end @@ -98,6 +95,9 @@ for i = 1:posesSize end end + axis equal + axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); + drawnow; if options.writeVideo @@ -114,6 +114,11 @@ end % wait for two seconds pause(2); +% change views +for i = 0 : 0.5 : 60 + view([i + 30, i]); +end + %% plot point covariance diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 04c896188..3d552aaa2 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -103,7 +103,5 @@ end %plot3DPoints(initialEstimate, [], marginals); %plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); -view(3); - end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 01e335d15..1d779b135 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -138,7 +138,7 @@ KMono = Cal3_S2(525,525,0,320,240); cameraPoses = cell(options.poseNum, 1); theta = 0; for i = 1:options.poseNum - t = Point3([(i-1)*(options.fieldSize.x - 20)/options.poseNum + 20, ... + t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ... 15, 10]'); camera = SimpleCamera.Lookat(t, ... Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... From fa023aac1af4d59200a84206b38753845ce3636d Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 22 Jan 2015 00:25:06 -0500 Subject: [PATCH 32/38] change lightings and add flying through sequence --- matlab/+gtsam/plotCamera.m | 8 ++--- matlab/+gtsam/plotFlyingResults.m | 59 +++++++++++++++++-------------- 2 files changed, 37 insertions(+), 30 deletions(-) diff --git a/matlab/+gtsam/plotCamera.m b/matlab/+gtsam/plotCamera.m index ba352b757..f5f164678 100644 --- a/matlab/+gtsam/plotCamera.m +++ b/matlab/+gtsam/plotCamera.m @@ -4,15 +4,15 @@ function plotCamera(pose, axisLength) xAxis = C+R(:,1)*axisLength; L = [C xAxis]'; - line(L(:,1),L(:,2),L(:,3),'Color','r'); + h_x = line(L(:,1),L(:,2),L(:,3),'Color','r'); yAxis = C+R(:,2)*axisLength; L = [C yAxis]'; - line(L(:,1),L(:,2),L(:,3),'Color','g'); + h_y = line(L(:,1),L(:,2),L(:,3),'Color','g'); zAxis = C+R(:,3)*axisLength; L = [C zAxis]'; - line(L(:,1),L(:,2),L(:,3),'Color','b'); + h_z = line(L(:,1),L(:,2),L(:,3),'Color','b'); axis equal -end \ No newline at end of file +end diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m index d09842934..57a656a7f 100644 --- a/matlab/+gtsam/plotFlyingResults.m +++ b/matlab/+gtsam/plotFlyingResults.m @@ -19,7 +19,10 @@ end figID = 1; figure(figID); -view([30, 0]); +view([30, 30]); + +hlight = camlight('headlight'); +lighting gouraud sampleDensity = 120; cylinderNum = length(cylinders); @@ -31,7 +34,8 @@ for i = 1:cylinderNum Z = Z * cylinders{i}.height; h_cylinder = surf(X,Y,Z); - set(h_cylinder, 'FaceColor', [0 0 0.5], 'FaceAlpha', 0.2, 'EdgeColor', [0 0 1]); + set(h_cylinder, 'FaceColor', [0 0 1], 'FaceAlpha', 0.2); + h_cylinder.AmbientStrength = 1; hold on end @@ -53,23 +57,25 @@ for i = 1:posesSize if exist('h_cov', 'var') delete(h_pose_cov); end + + plotCamera(poses{i}, 2); - gRp = poses{i}.rotation().matrix(); % rotation from pose to global - C = poses{i}.translation().vector(); - axisLength = 2; - - xAxis = C+gRp(:,1)*axisLength; - L = [C xAxis]'; - line(L(:,1),L(:,2),L(:,3),'Color','r'); - - yAxis = C+gRp(:,2)*axisLength; - L = [C yAxis]'; - line(L(:,1),L(:,2),L(:,3),'Color','g'); - - zAxis = C+gRp(:,3)*axisLength; - L = [C zAxis]'; - line(L(:,1),L(:,2),L(:,3),'Color','b'); - + gRp = poses{i}.rotation().matrix(); % rotation from pose to global + C = poses{i}.translation().vector(); +% axisLength = 2; +% +% xAxis = C+gRp(:,1)*axisLength; +% L = [C xAxis]'; +% line(L(:,1),L(:,2),L(:,3),'Color','r'); +% +% yAxis = C+gRp(:,2)*axisLength; +% L = [C yAxis]'; +% line(L(:,1),L(:,2),L(:,3),'Color','g'); +% +% zAxis = C+gRp(:,3)*axisLength; +% L = [C zAxis]'; +% line(L(:,1),L(:,2),L(:,3),'Color','b'); +% pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame h_pose_cov = gtsam.covarianceEllipse3D(C,gPp); @@ -114,18 +120,19 @@ end % wait for two seconds pause(2); -% change views +% change views angle for i = 0 : 0.5 : 60 view([i + 30, i]); end - -%% plot point covariance - -% if exist('h_cylinder', 'var') -% delete(h_cylinder); -% end - +% camera flying through +for i = 1 : posesSize + campos([poses{i}.x, poses{i}.y, poses{i}.z]); + camtarget([options.fieldSize.x/2, options.fieldSize.y/2, 0]); + camlight(hlight, 'headlight'); + + drawnow +end if ~holdstate From e74d64c90bbd922a85189f0b6bf71014a612a475 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 22 Jan 2015 11:24:43 -0500 Subject: [PATCH 33/38] add labels --- matlab/+gtsam/covarianceEllipse3D.m | 2 + matlab/+gtsam/plotCamera.m | 2 + matlab/+gtsam/plotFlyingResults.m | 124 +++++++++++++++++----------- matlab/+gtsam/points2DTrackStereo.m | 1 + 4 files changed, 81 insertions(+), 48 deletions(-) diff --git a/matlab/+gtsam/covarianceEllipse3D.m b/matlab/+gtsam/covarianceEllipse3D.m index 99a164068..c99b19117 100644 --- a/matlab/+gtsam/covarianceEllipse3D.m +++ b/matlab/+gtsam/covarianceEllipse3D.m @@ -6,6 +6,8 @@ function sc = covarianceEllipse3D(c,P) % % Modified from http://www.mathworks.com/matlabcentral/newsreader/view_thread/42966 +hold on + [e,s] = svd(P); k = 11.82; radii = k*sqrt(diag(s)); diff --git a/matlab/+gtsam/plotCamera.m b/matlab/+gtsam/plotCamera.m index f5f164678..d0d1f45b7 100644 --- a/matlab/+gtsam/plotCamera.m +++ b/matlab/+gtsam/plotCamera.m @@ -1,4 +1,6 @@ function plotCamera(pose, axisLength) + hold on + C = pose.translation().vector(); R = pose.rotation().matrix(); diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m index 57a656a7f..85042f5a8 100644 --- a/matlab/+gtsam/plotFlyingResults.m +++ b/matlab/+gtsam/plotFlyingResults.m @@ -1,11 +1,35 @@ function plotFlyingResults(pts3d, poses, posesCov, cylinders, options) % plot the visible points on the cylinders and trajectories +% % author: Zhaoyang Lv import gtsam.* -holdstate = ishold; -hold on +figID = 1; +figure(figID); +set(gcf, 'Position', [80,1,1800,1000]); + + +%% plot all the cylinders and sampled points + +axis equal +axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 30]); +xlabel('X (m)'); +ylabel('Y (m)'); +zlabel('Height (m)'); + +if options.camera.IS_MONO + h_title = title('Quadrotor Flight Simulation with Monocular Camera'); +else + h_title = title('Quadrotor Flight Simulation with Stereo Camera'); +end + +text(100,1750,0, sprintf('Flying Speed: %0.1f\n', options.speed)) + +view([30, 30]); + +hlight = camlight('headlight'); +lighting gouraud if(options.writeVideo) videoObj = VideoWriter('Camera_Flying_Example.avi'); @@ -14,36 +38,24 @@ if(options.writeVideo) open(videoObj); end -%% plot all the cylinders and sampled points -% now is plotting on a 100 * 100 field -figID = 1; -figure(figID); - -view([30, 30]); - -hlight = camlight('headlight'); -lighting gouraud sampleDensity = 120; cylinderNum = length(cylinders); -for i = 1:cylinderNum +h_cylinder = cell(cylinderNum); +for i = 1:cylinderNum + + hold on + [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height); X = X + cylinders{i}.centroid.x; Y = Y + cylinders{i}.centroid.y; Z = Z * cylinders{i}.height; - h_cylinder = surf(X,Y,Z); - set(h_cylinder, 'FaceColor', [0 0 1], 'FaceAlpha', 0.2); - h_cylinder.AmbientStrength = 1; - hold on -end - -drawnow; - -if options.writeVideo - currFrame = getframe(gcf); - writeVideo(videoObj, currFrame); + h_cylinder{i} = surf(X,Y,Z); + set(h_cylinder{i}, 'FaceColor', [0 0 1], 'FaceAlpha', 0.2); + h_cylinder{i}.AmbientStrength = 0.8; + end %% plot trajectories and points @@ -51,35 +63,35 @@ posesSize = length(poses); pointSize = length(pts3d); for i = 1:posesSize if i > 1 + hold on plot3([poses{i}.x; poses{i-1}.x], [poses{i}.y; poses{i-1}.y], [poses{i}.z; poses{i-1}.z], '-b'); end - if exist('h_cov', 'var') + if exist('h_pose_cov', 'var') delete(h_pose_cov); end - plotCamera(poses{i}, 2); + %plotCamera(poses{i}, 3); + + gRp = poses{i}.rotation().matrix(); % rotation from pose to global + C = poses{i}.translation().vector(); + axisLength = 3; + + xAxis = C+gRp(:,1)*axisLength; + L = [C xAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','r'); + + yAxis = C+gRp(:,2)*axisLength; + L = [C yAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','g'); + + zAxis = C+gRp(:,3)*axisLength; + L = [C zAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','b'); - gRp = poses{i}.rotation().matrix(); % rotation from pose to global - C = poses{i}.translation().vector(); -% axisLength = 2; -% -% xAxis = C+gRp(:,1)*axisLength; -% L = [C xAxis]'; -% line(L(:,1),L(:,2),L(:,3),'Color','r'); -% -% yAxis = C+gRp(:,2)*axisLength; -% L = [C yAxis]'; -% line(L(:,1),L(:,2),L(:,3),'Color','g'); -% -% zAxis = C+gRp(:,3)*axisLength; -% L = [C zAxis]'; -% line(L(:,1),L(:,2),L(:,3),'Color','b'); -% pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame h_pose_cov = gtsam.covarianceEllipse3D(C,gPp); - if exist('h_point', 'var') for j = 1:pointSize @@ -96,15 +108,16 @@ for i = 1:posesSize h_point_cov = cell(pointSize, 1); for j = 1:pointSize if ~isempty(pts3d{j}.cov{i}) + hold on h_point{j} = plot3(pts3d{j}.data.x, pts3d{j}.data.y, pts3d{j}.data.z); h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], pts3d{j}.cov{i}); end end axis equal - axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); + axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); - drawnow; + drawnow if options.writeVideo currFrame = getframe(gcf); @@ -120,16 +133,31 @@ end % wait for two seconds pause(2); -% change views angle -for i = 0 : 0.5 : 60 - view([i + 30, i]); +%% change views angle +for i = 30 : i : 90 + view([30, i]); + + if options.writeVideo + currFrame = getframe(gcf); + writeVideo(videoObj, currFrame); + end + + drawnow end -% camera flying through +% changing perspective + + +%% camera flying through video for i = 1 : posesSize campos([poses{i}.x, poses{i}.y, poses{i}.z]); camtarget([options.fieldSize.x/2, options.fieldSize.y/2, 0]); camlight(hlight, 'headlight'); + + if options.writeVideo + currFrame = getframe(gcf); + writeVideo(videoObj, currFrame); + end drawnow end diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 295b4d18e..60c9f1295 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -2,6 +2,7 @@ function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(K, cameraPos % 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 +% % @author: Zhaoyang Lv import gtsam.* From d47e4d4853e9acdef27e956cd8585073f530fa3a Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 22 Jan 2015 15:00:21 -0500 Subject: [PATCH 34/38] changes to monocular camera --- matlab/+gtsam/points2DTrackMonocular.m | 55 +++++++++++--------------- 1 file changed, 23 insertions(+), 32 deletions(-) diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 3d552aaa2..3ac501904 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -12,7 +12,6 @@ graph = NonlinearFactorGraph; %% create the noise factors poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); - measurementNoiseSigma = 1.0; measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); @@ -30,15 +29,22 @@ for i = 1:cylinderNum points3d{end}.Z = cell(0); points3d{end}.cameraConstraint = cell(0); points3d{end}.visiblity = false; + points3d{end}.cov = cell(cameraPosesNum); end end graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise)); -pts3d = cell(cameraPosesNum, 1); +%% initialize graph and values initialEstimate = Values; -for i = 1:cameraPosesNum - +for i = 1:pointsNum + point_j = points3d{i}.data.retract(0.1*randn(3,1)); + initialEstimate.insert(symbol('p', i), point_j); +end + +pts3d = cell(cameraPosesNum, 1); +cameraPosesCov = cell(cameraPosesNum, 1); +for i = 1:cameraPosesNum cameraPose = cameraPoses{i}; pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders); @@ -47,43 +53,31 @@ for i = 1:cameraPosesNum index = pts3d{i}.overallIdx{j}; points3d{index}.Z{end+1} = pts3d{i}.Z{j}; points3d{index}.cameraConstraint{end+1} = i; - points3d{index}.visiblity = true; + points3d{index}.visiblity = true; + + graph.add(GenericProjectionFactorCal3_S2(pts3d{i}.Z{j}, ... + measurementNoise, symbol('x', i), symbol('p', index), K) ); end - -end -%% initialize graph and values -for i = 1:cameraPosesNum pose_i = cameraPoses{i}.retract(0.1*randn(6,1)); initialEstimate.insert(symbol('x', i), pose_i); -end -for i = 1:pointsNum - % add in values - point_j = points3d{i}.data.retract(0.1*randn(3,1)); - initialEstimate.insert(symbol('p', i), point_j); + marginals = Marginals(graph, initialEstimate); - if ~points3d{i}.visiblity - continue; - end - % single measurement. not added to graph - factorNum = length(points3d{i}.Z); - if factorNum > 1 - for j = 1:factorNum - cameraIdx = points3d{i}.cameraConstraint{j}; - graph.add(GenericProjectionFactorCal3_S2(points3d{i}.Z{j}, ... - measurementNoise, symbol('x', cameraIdx), symbol('p', points3d{i}.cameraConstraint{j}), K) ); + for j = 1:pointsNum + if points3d{j}.visiblity + points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p',j)); end end - + + cameraPosesCov{i} = marginals.marginalCovariance(symbol('x',i)); end - + %% Print the graph graph.print(sprintf('\nFactor graph:\n')); -%% linearize the graph -% currently throws the Indeterminant linear system exception -marginals = Marginals(graph, initialEstimate); +%% Plot the result +plotFlyingResults(points3d, cameraPoses, cameraPosesCov, cylinders, options); %% get all the points track information for i = 1:pointsNum @@ -101,7 +95,4 @@ for i = 1:pointsNum end end -%plot3DPoints(initialEstimate, [], marginals); -%plot3DTrajectory(initialEstimate, '*', 1, 8, marginals); - end From 34ae976f6ab18f276c7452519247569a6ca4e2fa Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 22 Jan 2015 15:35:28 -0500 Subject: [PATCH 35/38] fix the indeterminant system in stereo case --- matlab/+gtsam/cylinderSampleProjectionStereo.m | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/matlab/+gtsam/cylinderSampleProjectionStereo.m b/matlab/+gtsam/cylinderSampleProjectionStereo.m index 51cda12ac..6512231e8 100644 --- a/matlab/+gtsam/cylinderSampleProjectionStereo.m +++ b/matlab/+gtsam/cylinderSampleProjectionStereo.m @@ -42,6 +42,11 @@ for i = 1:cylinderNum continue; end + % too small disparity may call indeterminant system exception + if Z.du < 0.6 + continue; + end + % ignore points occluded % use a simple math hack to check occlusion: % 1. All points in front of cylinders' surfaces are visible From fd6b377d4b738dd0c58919025f51fcb06baf4559 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 22 Jan 2015 16:53:36 -0500 Subject: [PATCH 36/38] check under constrained measurement in monocular camera setup --- matlab/+gtsam/points2DTrackMonocular.m | 29 +++++++++++++++++++------- 1 file changed, 22 insertions(+), 7 deletions(-) diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 3ac501904..fc48f587e 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -27,7 +27,8 @@ for i = 1:cylinderNum for j = 1:cylinderPointsNum points3d{end+1}.data = cylinders{i}.Points{j}; points3d{end}.Z = cell(0); - points3d{end}.cameraConstraint = cell(0); + points3d{end}.camConstraintIdx = cell(0); + points3d{end}.added = cell(0); points3d{end}.visiblity = false; points3d{end}.cov = cell(cameraPosesNum); end @@ -44,6 +45,7 @@ end pts3d = cell(cameraPosesNum, 1); cameraPosesCov = cell(cameraPosesNum, 1); +marginals = Values; for i = 1:cameraPosesNum cameraPose = cameraPoses{i}; pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders); @@ -52,25 +54,38 @@ for i = 1:cameraPosesNum for j = 1:measurementNum index = pts3d{i}.overallIdx{j}; points3d{index}.Z{end+1} = pts3d{i}.Z{j}; - points3d{index}.cameraConstraint{end+1} = i; - points3d{index}.visiblity = true; + points3d{index}.camConstraintIdx{end+1} = i; + points3d{index}.added{end+1} = false; - graph.add(GenericProjectionFactorCal3_S2(pts3d{i}.Z{j}, ... - measurementNoise, symbol('x', i), symbol('p', index), K) ); + if length(points3d{index}.Z) < 2 + continue; + else + for k = 1:length(points3d{index}.Z) + if ~points3d{index}.added{k} + graph.add(GenericProjectionFactorCal3_S2(points3d{index}.Z{k}, ... + measurementNoise, symbol('x', points3d{index}.camConstraintIdx{k}), ... + symbol('p', index), K) ); + points3d{index}.added{k} = true; + end + end + end + + points3d{index}.visiblity = true; end pose_i = cameraPoses{i}.retract(0.1*randn(6,1)); initialEstimate.insert(symbol('x', i), pose_i); marginals = Marginals(graph, initialEstimate); - + for j = 1:pointsNum if points3d{j}.visiblity points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p',j)); end end - + cameraPosesCov{i} = marginals.marginalCovariance(symbol('x',i)); + end %% Print the graph From cf26dec49ef471576ede3d5fb4a298b0debf5991 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 22 Jan 2015 20:25:01 -0500 Subject: [PATCH 37/38] add noise to incremental poses. more realistic --- matlab/gtsam_examples/CameraFlyingExample.m | 31 +++++++++++++++------ 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 1d779b135..3538f38b6 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -27,7 +27,7 @@ options.cylinder.cylinderNum = 15; % pls be smaller than 20 options.cylinder.radius = 3; % pls be smaller than 5 options.cylinder.height = 10; % point density on cylinder -options.cylinder.pointDensity = 0.05; +options.cylinder.pointDensity = 0.1; %% camera options % parameters set according to the stereo camera: @@ -38,7 +38,7 @@ options.camera.IS_MONO = false; % the field of view of camera options.camera.fov = 120; % fps for image -options.camera.fps = 10; +options.camera.fps = 25; % camera pixel resolution options.camera.resolution = Point2(752, 480); % camera horizon @@ -65,8 +65,8 @@ options.writeVideo = true; 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); +% camera flying height +options.height = 30; %% ploting options % display covariance scaling factor @@ -135,15 +135,28 @@ end %% generate ground truth camera trajectories: a line KMono = Cal3_S2(525,525,0,320,240); -cameraPoses = cell(options.poseNum, 1); +cameraPoses = cell(0); theta = 0; -for i = 1:options.poseNum - t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ... - 15, 10]'); +t = Point3(5, 5, options.height); +i = 0; +while 1 + i = i+1; + distance = options.speed / options.camera.fps; + angle = 0.1*pi*(rand-0.5); + inc_t = Point3(distance * cos(angle), ... + distance * sin(angle), 0); + t = t.compose(inc_t); + + if t.x > options.fieldSize.x - 10 || t.y > options.fieldSize.y - 10; + break; + end + + %t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ... + % 15, 10]'); camera = SimpleCamera.Lookat(t, ... Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... Point3([0,0,1]'), options.camera.monoK); - cameraPoses{i} = camera.pose; + cameraPoses{end+1} = camera.pose; end %% set up camera and get measurements From 0f100e8228a347352e4487b0d89725e94f9a9ab2 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 22 Jan 2015 20:39:02 -0500 Subject: [PATCH 38/38] add the scale of visualization for covariance matrix --- matlab/+gtsam/covarianceEllipse.m | 4 +++- matlab/+gtsam/covarianceEllipse3D.m | 6 ++++- matlab/+gtsam/plotFlyingResults.m | 26 +++++++++++---------- matlab/gtsam_examples/CameraFlyingExample.m | 5 ++-- 4 files changed, 25 insertions(+), 16 deletions(-) diff --git a/matlab/+gtsam/covarianceEllipse.m b/matlab/+gtsam/covarianceEllipse.m index cdc239bb2..829487dc6 100644 --- a/matlab/+gtsam/covarianceEllipse.m +++ b/matlab/+gtsam/covarianceEllipse.m @@ -8,6 +8,8 @@ function h = covarianceEllipse(x,P,color, k) % it is assumed x and y are the first two components of state x % k is scaling for std deviations, defaults to 1 std +hold on + [e,s] = eig(P(1:2,1:2)); s1 = s(1,1); s2 = s(2,2); @@ -32,4 +34,4 @@ h = line(ex,ey,'color',color); y = c(2) + points(2,:); end -end \ No newline at end of file +end diff --git a/matlab/+gtsam/covarianceEllipse3D.m b/matlab/+gtsam/covarianceEllipse3D.m index c99b19117..4364e0fe4 100644 --- a/matlab/+gtsam/covarianceEllipse3D.m +++ b/matlab/+gtsam/covarianceEllipse3D.m @@ -1,4 +1,4 @@ -function sc = covarianceEllipse3D(c,P) +function sc = covarianceEllipse3D(c,P,scale) % covarianceEllipse3D plots a Gaussian as an uncertainty ellipse % Based on Maybeck Vol 1, page 366 % k=2.296 corresponds to 1 std, 68.26% of all probability @@ -12,6 +12,10 @@ hold on k = 11.82; radii = k*sqrt(diag(s)); +if exist('scale', 'var') + radii = radii * scale; +end + % generate data for "unrotated" ellipsoid [xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3),8); diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m index 85042f5a8..5d4a287c4 100644 --- a/matlab/+gtsam/plotFlyingResults.m +++ b/matlab/+gtsam/plotFlyingResults.m @@ -13,11 +13,13 @@ set(gcf, 'Position', [80,1,1800,1000]); %% plot all the cylinders and sampled points axis equal -axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 30]); +axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]); xlabel('X (m)'); ylabel('Y (m)'); zlabel('Height (m)'); +h = cameratoolbar('Show'); + if options.camera.IS_MONO h_title = title('Quadrotor Flight Simulation with Monocular Camera'); else @@ -91,7 +93,7 @@ for i = 1:posesSize pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame - h_pose_cov = gtsam.covarianceEllipse3D(C,gPp); + h_pose_cov = gtsam.covarianceEllipse3D(C, gPp, options.plot.covarianceScale); if exist('h_point', 'var') for j = 1:pointSize @@ -110,13 +112,14 @@ for i = 1:posesSize if ~isempty(pts3d{j}.cov{i}) hold on h_point{j} = plot3(pts3d{j}.data.x, pts3d{j}.data.y, pts3d{j}.data.z); - h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], pts3d{j}.cov{i}); + h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], ... + pts3d{j}.cov{i}, options.plot.covarianceScale); end - end + end axis equal - axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]); - + axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]); + drawnow if options.writeVideo @@ -149,11 +152,15 @@ end %% camera flying through video +camzoom(0.8); for i = 1 : posesSize + + hold on + campos([poses{i}.x, poses{i}.y, poses{i}.z]); camtarget([options.fieldSize.x/2, options.fieldSize.y/2, 0]); camlight(hlight, 'headlight'); - + if options.writeVideo currFrame = getframe(gcf); writeVideo(videoObj, currFrame); @@ -162,11 +169,6 @@ for i = 1 : posesSize drawnow end - -if ~holdstate - hold off -end - %%close video if(options.writeVideo) close(videoObj); diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 3538f38b6..36b7635e2 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -69,8 +69,9 @@ options.speed = 20; options.height = 30; %% ploting options -% display covariance scaling factor -options.plot.scale = 1; +% display covariance scaling factor, default to be 1. +% The covariance visualization default models 99% of all probablity +options.plot.covarianceScale = 1; % plot the trajectory covariance options.plot.DISP_TRAJ_COV = true; % plot points covariance