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

to check single measurement constraint.

release/4.3a0
lvzhaoyang 2015-01-14 23:37:10 -05:00
parent 3cb1f96371
commit 7eec7f7b45
1 changed files with 45 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
matlab/+gtsam/points2DTrackMonocular.m
View File

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