110 lines
3.4 KiB
Matlab
110 lines
3.4 KiB
Matlab
function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(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
|
|
% @author: Zhaoyang Lv
|
|
|
|
import gtsam.*
|
|
|
|
%% create graph
|
|
graph = NonlinearFactorGraph;
|
|
|
|
%% create the noise factors
|
|
poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
|
|
posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
|
|
stereoNoise = noiseModel.Isotropic.Sigma(3,0.2);
|
|
|
|
cameraPosesNum = length(cameraPoses);
|
|
|
|
%% add measurements and initial camera & points values
|
|
pointsNum = 0;
|
|
cylinderNum = length(cylinders);
|
|
points3d = cell(0);
|
|
for i = 1:cylinderNum
|
|
cylinderPointsNum = length(cylinders{i}.Points);
|
|
pointsNum = pointsNum + length(cylinders{i}.Points);
|
|
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;
|
|
for i = 1:cameraPosesNum
|
|
pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, imageSize, cylinders);
|
|
|
|
if isempty(pts3d{i}.Z)
|
|
continue;
|
|
end
|
|
|
|
measurementNum = length(pts3d{i}.Z);
|
|
for j = 1:measurementNum
|
|
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
|
|
|
|
for j = 1:length(pts3d{i}.Z)
|
|
graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ...
|
|
stereoNoise, symbol('x', i), symbol('p', pts3d{i}.overallIdx{j}), K));
|
|
end
|
|
end
|
|
|
|
%% 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
|
|
|
|
for i = 1:pointsNum
|
|
point_j = points3d{i}.data.retract(0.1*randn(3,1));
|
|
initialEstimate.insert(symbol('p', i), point_j);
|
|
|
|
if ~points3d{i}.visiblity
|
|
continue;
|
|
end
|
|
|
|
factorNum = length(points3d{i}.Z);
|
|
for j = 1:factorNum
|
|
cameraIdx = points3d{i}.cameraConstraint{j};
|
|
graph.add(GenericStereoFactor3D(StereoPoint2(points3d{i}.Z{j}.uL, points3d{i}.Z{j}.uR, points3d{i}.Z{j}.v), ...
|
|
stereoNoise, symbol('x', cameraIdx), symbol('p', points3d{i}.cameraConstraint{j}), K));
|
|
end
|
|
end
|
|
|
|
|
|
%% Print the graph
|
|
graph.print(sprintf('\nFactor graph:\n'));
|
|
|
|
%% linearize the graph
|
|
marginals = Marginals(graph, initialEstimate);
|
|
|
|
%% get all the 2d points track information
|
|
pts2dTracksStereo.pt3d = cell(0);
|
|
pts2dTracksStereo.Z = cell(0);
|
|
pts2dTracksStereo.cov = cell(0);
|
|
for i = 1:pointsNum
|
|
if ~points3d{i}.visiblity
|
|
continue;
|
|
end
|
|
|
|
pts2dTracksStereo.pt3d{end+1} = points3d{i}.data;
|
|
pts2dTracksStereo.Z{end+1} = points3d{i}.Z;
|
|
pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i));
|
|
|
|
end
|
|
|
|
%% plot the result with covariance ellipses
|
|
plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, initialEstimate, marginals);
|
|
|
|
%plot3DTrajectory(initialEstimate, '*', 1, 8, marginals);
|
|
%view(3);
|
|
|
|
end
|