From 39f5aa499e6f9009d5acadf5b953b65abf4b803d Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Mon, 12 Jan 2015 23:27:06 -0500 Subject: [PATCH] 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