12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

2D monocular track. Testing with random data now throws indeterminant linear system exception

release/4.3a0
lvzhaoyang 2015-01-12 23:27:06 -05:00
parent a8bf2a4da1
commit 39f5aa499e
2 changed files with 52 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
matlab/+gtsam/cylinderSampleProjection.m
View File

@ -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

57
matlab/+gtsam/points2DTrackMonocular.m
View File

@ -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