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