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

check under constrained measurement in monocular camera setup

release/4.3a0
lvzhaoyang 2015-01-22 16:53:36 -05:00
parent 34ae976f6a
commit fd6b377d4b
1 changed files with 22 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
matlab/+gtsam/points2DTrackMonocular.m
View File

@ -27,7 +27,8 @@ for i = 1:cylinderNum
for j = 1:cylinderPointsNum for j = 1:cylinderPointsNum
points3d{end+1}.data = cylinders{i}.Points{j}; points3d{end+1}.data = cylinders{i}.Points{j};
points3d{end}.Z = cell(0); 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}.visiblity = false;
points3d{end}.cov = cell(cameraPosesNum); points3d{end}.cov = cell(cameraPosesNum);
end end
@ -44,6 +45,7 @@ end
pts3d = cell(cameraPosesNum, 1); pts3d = cell(cameraPosesNum, 1);
cameraPosesCov = cell(cameraPosesNum, 1); cameraPosesCov = cell(cameraPosesNum, 1);
marginals = Values;
for i = 1:cameraPosesNum for i = 1:cameraPosesNum
cameraPose = cameraPoses{i}; cameraPose = cameraPoses{i};
pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders); pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders);
@ -52,25 +54,38 @@ for i = 1:cameraPosesNum
for j = 1:measurementNum for j = 1:measurementNum
index = pts3d{i}.overallIdx{j}; index = pts3d{i}.overallIdx{j};
points3d{index}.Z{end+1} = pts3d{i}.Z{j}; points3d{index}.Z{end+1} = pts3d{i}.Z{j};
points3d{index}.cameraConstraint{end+1} = i; points3d{index}.camConstraintIdx{end+1} = i;
points3d{index}.visiblity = true; points3d{index}.added{end+1} = false;
graph.add(GenericProjectionFactorCal3_S2(pts3d{i}.Z{j}, ... if length(points3d{index}.Z) < 2
measurementNoise, symbol('x', i), symbol('p', index), K) ); 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 end
pose_i = cameraPoses{i}.retract(0.1*randn(6,1)); pose_i = cameraPoses{i}.retract(0.1*randn(6,1));
initialEstimate.insert(symbol('x', i), pose_i); initialEstimate.insert(symbol('x', i), pose_i);
marginals = Marginals(graph, initialEstimate); marginals = Marginals(graph, initialEstimate);
for j = 1:pointsNum for j = 1:pointsNum
if points3d{j}.visiblity if points3d{j}.visiblity
points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p',j)); points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p',j));
end end
end end
cameraPosesCov{i} = marginals.marginalCovariance(symbol('x',i)); cameraPosesCov{i} = marginals.marginalCovariance(symbol('x',i));
end end
%% Print the graph %% Print the graph