双目立体视觉对比试验

%%  
% 双目立体视觉  
% 对比实验  
  
%%  
% 清空工作区  
clc;  
clear;  
close all;  
  
%%  
% 导入图像数据  
I1 = imread('pattern_cam1_im1.png');  
I2 = imread('pattern_cam2_im1.png');  
figure  
imshowpair(I1, I2, 'montage');   
title('Original Images');  
% 导入相机参数  
load cameraParams.mat  
  
%%  
% 校正  
I1 = undistortImage(I1, camera1Params);  
I2 = undistortImage(I2, camera1Params);  
figure   
imshowpair(I1, I2, 'montage');  
title('Undistorted Images');  
  
%%  
% 特征点提取  
imagePoints1 = detectMinEigenFeatures(rgb2gray(I1), 'MinQuality', 0.1);  
  
%%  
% 可视化  
figure  
imshow(I1, 'InitialMagnification', 50);  
title('150 Strongest Corners from the First Image');  
hold on  
plot(selectStrongest(imagePoints1, 150));  
  
%%  
% Create the point tracker  
tracker = vision.PointTracker('MaxBidirectionalError', 1, 'NumPyramidLevels', 5);  
imagePoints1 = imagePoints1.Location;  
initialize(tracker, imagePoints1, I1);  
% Track the points  
[imagePoints2, validIdx] = step(tracker, I2);  
matchedPoints1 = imagePoints1(validIdx, :);  
matchedPoints2 = imagePoints2(validIdx, :);  
  
%%  
% 特征点匹配  
figure  
showMatchedFeatures(I1, I2, matchedPoints1, matchedPoints2);  
title('Tracked Features');  
  
%%  
% F矩阵估计  
[fMatrix, epipolarInliers] = estimateFundamentalMatrix(...  
  matchedPoints1, matchedPoints2, 'Method', 'MSAC', 'NumTrials', 10000);  
% 极线  
inlierPoints1 = matchedPoints1(epipolarInliers, :);  
inlierPoints2 = matchedPoints2(epipolarInliers, :);  
% 显示内点  
figure  
showMatchedFeatures(I1, I2, inlierPoints1, inlierPoints2);  
title('Epipolar Inliers');  
  
%%  
% R和T（也可以用RANSAC算法）  
R = [0.9455,-0.0096,0.3253;  
    0.0120,0.9999,-0.0053;  
    -0.3252,0.0090,0.9456];  
t = [98.4069,0.1741,18.9018];  
  
%%  
% 稠密的特征点  
imagePoints1 = detectMinEigenFeatures(rgb2gray(I1), 'MinQuality', 0.001);  
  
%%  
% Create the point tracker  创建一个跟踪点
tracker = vision.PointTracker('MaxBidirectionalError', 1, 'NumPyramidLevels', 5);  
% Initialize the point tracker  
imagePoints1 = imagePoints1.Location;  
initialize(tracker, imagePoints1, I1);  
% Track the points  
[imagePoints2, validIdx] = step(tracker, I2);  
matchedPoints1 = imagePoints1(validIdx, :);  
matchedPoints2 = imagePoints2(validIdx, :);  
  
%%  
% cameraMatrix  
camMatrix1 = cameraMatrix(camera1Params, eye(3), [0 0 0]);  
camMatrix2 = cameraMatrix(camera2Params, R', -t*R');  
  
% 三维点云的计算  
points3D = triangulate(matchedPoints1, matchedPoints2, camMatrix1, camMatrix2);  
  
% 获取颜色信息  
numPixels = size(I1, 1) * size(I1, 2);  
allColors = reshape(I1, [numPixels, 3]);  
colorIdx = sub2ind([size(I1, 1), size(I1, 2)], round(matchedPoints1(:,2)), ...  
    round(matchedPoints1(:, 1)));  
color = allColors(colorIdx, :);  
  
% 创建点云  
ptCloud = pointCloud(points3D, 'Color', color);  
  
%%  
% 可视化  
cameraSize = 0.3;  
figure  
plotCamera('Size', cameraSize, 'Color', 'r', 'Label', '1', 'Opacity', 0);  
hold on  
grid on  
plotCamera('Location', t, 'Orientation', R, 'Size', cameraSize, ...  
    'Color', 'b', 'Label', '2', 'Opacity', 0);  
  
% 点云的可视化  
pcshow(ptCloud, 'VerticalAxis', 'y', 'VerticalAxisDir', 'down', ...  
    'MarkerSize', 45);  
  
% Rotate and zoom the plot  
camorbit(0, -30);  
camzoom(1.5);  
  
% Label the axes  
xlabel('x-axis');  
ylabel('y-axis');  
zlabel('z-axis')  
title('Up to Scale Reconstruction of the Scene');