解释代码clc;
close all;
clear all;
load ('F:\镜面实验数据\20240830\Calib_Results.mat');
data=load('F:\镜面实验数据\20240830\pmxs.txt');
R = load('F:\镜面实验数据\20240830\R.txt');
Point = load('F:\镜面实验数据\20240830\P.txt');
tic
phase_shift_num=4;
l_imgs = cell(1,phase_shift_num);
for i=1
for j=1:phase_shift_num
l_imgs{i,j} = double(imread(['F:\镜面实验数据\20240830\wuti1\' num2str(phase_shift_num*(i-1)+j),'.bmp']));
end
end
%%求极线参数
a=data(1,1);b=data(1,2);c=data(1,3);d=data(1,4);
D=abs(d)/(sqrt(a*a+b*b+c*c));
n=[a,b,c]';
I=[1,0,0;0,1,0;0,0,1];
D1=I-2*n*n';
D2=2*D*n;
N0 = [-1,0,0;0,1,0;0,0,1];
I0=[-1;1;1];
R=R;T=Point*R;
fx=fc(1,1);fy=fc(2,1);u0=cc(1,1);v0=cc(2,1); %内参
M=KK*[R,T'];
M0=M(1:3,1:3);m0=M(:,4);
% m=M0*D2+m0-M0*D1*inv(M0)*m0;
% mx=[0,-m(3,1),m(2,1);m(3,1),0,-m(1,1);-m(2,1),m(1,1),0];
% F=mx*M0*D1*inv(M0);
M01 = inv(M0);
m=m0-M0*N0*M01*m0;
mx=[0,-m(3,1),m(2,1);m(3,1),0,-m(1,1);-m(2,1),m(1,1),0];
F = mx*M0*N0*M01;
c1 = -inv(M0)*m0 ; c2 = [-1,0,0;0,1,0;0,0,1]*c1;
K = R*[0;0;1];
V = c1 - c2;
% 将向量 V 单位化
r1 = V / norm(V);
% r2 = R*[0;1;0];
r2 = cross(K,r1);
r3 = cross(r1,r2);
R1 = [r1';r2';r3']';
R2 = -R1*c2;
M2 = KK*[R1 R2];
left_M2 = M2(:,1:3);
Ma = left_M2*inv(M0);
Ma = double(Ma);
% 假设 Ma 是每张图像对应的 3x3 仿射变换矩阵,这里只是一个示例
Mz = cell(1, phase_shift_num); % 创建一个单元数组来存储每个相移图像的变换矩阵
for j = 1:phase_shift_num
Mz{j} = Ma;% 计算或加载 Ma{j}
end
% 初始化校正后的图像数组
l_imgs_corrected = cell(1, phase_shift_num);
% 对每张图像进行校正
for j = 1:phase_shift_num
% 读取原始图像
img = l_imgs{1, j};
% 计算校正后图像的边界框
s1 = size(img);
corners = [0, 0, s1(2), s1(2); 0, s1(1), 0, s1(1)];
corners_x = p2t(Mz{j}, corners);
minx = floor(min(corners_x(1,:)));
maxx = ceil(max(corners_x(1,:)));
miny = floor(min(corners_x(2,:)));
maxy = ceil(max(corners_x(2,:)));
bb1 = [minx; miny; maxx; maxy];
% 应用校正变换
[img_corrected, alpha] = imwarp(img, Mz{j}, 'Bilinear', bb1);
% 存储校正后的图像
l_imgs_corrected{j} = double(img_corrected);
end
ROIL=[];
imgL=[];
for i=1:phase_shift_num
imgL(:,:,i)= l_imgs_corrected{1,i};
end
ROILg=max(imgL,[],3);
figure,imshow(ROILg,[])
%闭运算
freq = 64;
ROIL=imclose(ROILg,strel('disk',10));
ROIL = ROIL<40;
ROIL = ~ROIL;
figure,imshow(ROIL,[])
l_uph1=calculatePhase(l_imgs_corrected,phase_shift_num,freq,10);
l_uph1 = double(l_uph1);
figure,imshow(l_uph1,[]),title('相机截断相位');
l_uph1ROI=l_uph1.*ROIL;
figure,imshow(l_uph1ROI,[]),title('相机截断相位');
[LPx,~]=gradient(l_uph1ROI); %求梯度
figure,imshow(LPx,[]),title('左相机截断相位x梯度');
% 显示变换后的图像
figure, imshow(l_uph1ROI, []);
[X_coord,Y_coord] = ginput(1);
pointArray1 = [];
pointArray2 = [];
[rows, cols] = size(l_uph1ROI);
[X, Y] = meshgrid(1:cols, 1:rows);
% 根据 X 坐标将点坐标保存到两个矩阵中
X_coords = X(:); % 将 X 坐标展开为列向量
Y_coords = Y(:); % 将 Y 坐标展开为列向量
% 将坐标保存到两个矩阵中
coords1 = [X_coords(X_coords < X_coord), Y_coords(X_coords < X_coord)]; % 小于 X 坐标的坐标
coords2 = [X_coords(X_coords >= X_coord), Y_coords(X_coords >= X_coord)]; % 大于等于 X 坐标的坐标
for i= 1:size(coords1,1)
Xi = coords1(i,1);
Yi = coords1(i,2);
if l_uph1ROI(Yi,Xi) ~=0
pointArray1 = [pointArray1; [Xi, Yi]];
end
end
for j= 1:size(coords2,1)
Xj = coords2(j,1);
Yj = coords2(j,2);
if l_uph1ROI(Yj,Xj) ~=0
pointArray2 = [pointArray2; [Xj, Yj]];
end
end
point_x = 385;
point_y = 814;
refPoint = [point_x,point_y];
% 假设 pointArray2 是另一个视野中的点的数组
matchedPoints = [];
% 计算参考点的上一行和下一行索引
row = point_y; % 参考点的行索引
upper_row = max(1, row - 1); % 上一行,确保不会小于1
lower_row = min(size(l_uph1ROI, 1), row + 1); % 下一行,确保不会超出图像边界
for i = 1:size(pointArray2, 1)
x = pointArray2(i, 1);
y = pointArray2(i, 2);
% 检查点是否在参考点所在的行,或者上一行或下一行
% 注意:这里使用 y 来检查行索引
if y == row
% 如果点在这些行之一,检查相位差异
% 注意:这里使用 x, y 来索引图像,即 l_uph1ROI(x, y)
if abs(l_uph1ROI(y, x) - l_uph1ROI(point_y, point_x)) < 0.3
matchedPoints = [matchedPoints; y, x];
end
end
end
% point0 = [point_x,point_y,1]';
% l0 = F * point0;
% % 2. 变换极线
% l_prime = Ma * l0;
% % 3. 将变换后的极线转换为图像坐标
% % 假设变换后的图像大小为 [height, width]
% [height, width] = size(l_uph1ROI); % J1 是变换后的图像
% x = 1:size(l_uph1ROI, 2); % 极线上的x范围
% y = (-l_prime(3) - l_prime(1)*x) / l_prime(2); % 计算极线上的y坐标
figure;
imshow(mat2gray(l_uph1ROI));
hold on;
scatter(matchedPoints(:, 2), matchedPoints(:, 1), 'g', 'filled');
plot(point_x, point_y, 'r.', 'MarkerSize', 20); % 标注红色点
plot([0, cols], [point_y, point_y], 'r', 'LineWidth', 1)
title('相位图像及其极线');
% 显示同一行上每个点的灰度值
gray_value = l_uph1ROI(point_y, :);
% 绘制灰度值图形
figure;
plot(gray_value, 'LineWidth', 2);
title('同一行的灰度值');
xlabel('图像列索引');
ylabel('灰度值');
hold on;
% 提取 X_cood 坐标左边和右边的数据段
left_gray_values = gray_value(1:X_coord-1);
right_gray_values = gray_value(X_coord:end);
% 找到左侧部分的数据中的波峰和波谷
[peaks_left, peak_locs_left] = findpeaks(left_gray_values, 'MinPeakProminence', 0.5, 'MinPeakDistance', 5);
[valleys_left, valley_locs_left] = findpeaks(-left_gray_values, 'MinPeakProminence', 0.5, 'MinPeakDistance', 5);
% 找到右侧部分的数据中的波峰和波谷
[peaks_right, peak_locs_right] = findpeaks(right_gray_values, 'MinPeakProminence', 0.5, 'MinPeakDistance',5);
[valleys_right, valley_locs_right] = findpeaks(-right_gray_values,'MinPeakProminence', 0.5, 'MinPeakDistance', 5);
% 转换为全局索引
peak_locs_right = X_coord + peak_locs_right - 1;
valley_locs_right = X_coord + valley_locs_right - 1;
% 合并右侧的波峰和波谷
right_features = [peak_locs_right valley_locs_right];
right_feature_values = [peaks_right -valleys_right];
% 对右侧的波峰和波谷进行排序
[sorted_right_features, sortIndex_right] = sort(right_features);
sorted_right_feature_values = right_feature_values(sortIndex_right);
% 合并左侧的波峰和波谷
left_features = [peak_locs_left valley_locs_left];
left_feature_values = [peaks_left -valleys_left];
% 对左侧的波峰和波谷进行降序排序
[sorted_left_feature, sortIndex_left] = sort(left_features, 'descend');
sorted_left_feature_values = left_feature_values(sortIndex_left);
%%寻找波形对称的波峰和波谷
if sorted_right_feature_values(1)<0
firstNonPositiveIndex = find(sorted_left_feature_values <= 0, 1);
% 根据找到的索引删除矩阵中索引之前的所有元素
if ~isempty(firstNonPositiveIndex)
% 删除索引之前的所有元素
sorted_left_feature_values=sorted_left_feature_values(firstNonPositiveIndex:end);
sorted_left_feature = sorted_left_feature(firstNonPositiveIndex:end);
else
disp('没有找到不小于0的数,矩阵未改变');
end
end
if sorted_right_feature_values(1)>0
firstNonPositiveIndex = find(sorted_left_feature_values >= 0, 1);
% 根据找到的索引删除矩阵中索引之前的所有元素
if ~isempty(firstNonPositiveIndex)
% 删除索引之前的所有元素
sorted_left_feature_values=sorted_left_feature_values(firstNonPositiveIndex:end);
sorted_left_feature = sorted_left_feature(firstNonPositiveIndex:end);
else
disp('没有找到不大于0的数,矩阵未改变');
end
end
% % 标注左侧的波峰和波谷
% for i = 1:length(sorted_left_feature)
% text(sorted_left_feature(i), sorted_left_feature_values(i), num2str(i), ...
% 'VerticalAlignment', 'bottom', 'HorizontalAlignment', 'right', 'Color', 'r');
% end
% % 标注右侧的波峰和波谷
% for i = 1:length(sorted_right_features)
% text(sorted_right_features(i), sorted_right_feature_values(i), num2str(i), ...
% 'VerticalAlignment', 'top', 'HorizontalAlignment', 'right', 'Color', 'b');
% end
% 标注左侧的波峰和波谷
for i = 1:length(sorted_left_feature)
text(sorted_left_feature(i), sorted_left_feature_values(i), num2str(i), ...
'VerticalAlignment', 'bottom', 'HorizontalAlignment', 'right', 'Color', 'r');
line([sorted_left_feature(i) sorted_left_feature(i)], [min(gray_value) max(gray_value)], 'Color', 'r', 'LineStyle', '--');
end
% 标注右侧的波峰和波谷
for i = 1:length(sorted_right_features)
text(sorted_right_features(i), sorted_right_feature_values(i), num2str(i), ...
'VerticalAlignment', 'top', 'HorizontalAlignment', 'right', 'Color', 'b');
line([sorted_right_features(i) sorted_right_features(i)], [min(gray_value) max(gray_value)], 'Color', 'b', 'LineStyle', '--');
end
% 画图显示
figure
plot(gray_value);
hold on; % 保持当前图像,以便在上面添加竖线
% % 画左侧的竖线
% for i = 1:length(sorted_left_feature)
% line([sorted_left_feature(i) sorted_left_feature(i)], [min(gray_value) max(gray_value)], 'Color', 'r', 'LineStyle', '--');
% end
% % 画右侧的竖线
% for i = 1:length(sorted_right_features)
% line([sorted_right_features(i) sorted_right_features(i)], [min(gray_value) max(gray_value)], 'Color', 'b', 'LineStyle', '--');
% end
% hold off; % 释放图像
% 在灰度值图形上标记匹配点和参考点
scatter(matchedPoints(:, 2), gray_value(matchedPoints(:, 2)), 'g', 'filled', 'MarkerEdgeColor', 'k');
scatter(point_x, gray_value(point_x), 20, 'r', 'filled'); % 标注红色点
hold off;
% 步骤1: 确定参考点是在波峰之间还是波谷之间
ref_point_y = gray_value(point_x);
min_peak_left = min(sorted_left_feature);
max_peak_left = max(sorted_left_feature);
% 初始化距离数组
distances1 = abs(point_x - sorted_left_feature);
% 参考点在最左侧或最右侧,选择最近的波峰或波谷
if point_x <= min_peak_left
[~, nearest_index] = min(distances1);
nearest_peak_indices = nearest_index;
if nearest_peak_indices > length(sorted_right_features)
best_match = [];
return;
else
right_nearest_loc = sorted_right_features(nearest_peak_indices);
end
elseif point_x >= max_peak_left
[~, nearest_index] = min(distances1);
nearest_peak_indices = nearest_index;
if nearest_peak_indices > length(sorted_right_features)
best_match = [];
return;
else
right_nearest_loc = sorted_right_features(nearest_peak_indices);
end
else
% 寻找最近的两个波峰
[~, nearest_peak_indices] = sort(distances1);
nearest_peak_indices = nearest_peak_indices(1); % 取最近的波峰
if point_x < sorted_left_feature(nearest_peak_indices(1))
% 参考点在左侧,往下取一个波峰
next_peak_index = nearest_peak_indices(1) + 1;
else
% 参考点在右侧,往上取一个波峰
next_peak_index = nearest_peak_indices(1) - 1;
end
nearest_peak_indices = [nearest_peak_indices next_peak_index];
end
% 步骤3: 在右侧区域的待匹配点中寻找匹配点
matching_point_index = [];
for i = 1:size(matchedPoints, 1)
current_point = matchedPoints(i, 2); % 假设匹配点的x坐标在第二列
if point_x <= min_peak_left
if current_point >= right_nearest_loc
matching_point_index = [matching_point_index; i];
end
elseif point_x >= max_peak_left
if current_point <= right_nearest_loc
matching_point_index = [matching_point_index; i];
end
else
if max(nearest_peak_indices) > length(sorted_right_features)
best_match = [];
return;
else
right_nearest_peak_locs = sorted_right_features(nearest_peak_indices);
right_nearest_peak_locs = round(right_nearest_peak_locs);
if current_point>=min(right_nearest_peak_locs)&¤t_point<=max(right_nearest_peak_locs)
matching_point_index = [matching_point_index,i];
end
end
end
end
% 过滤掉任何无效的索引(例如 -1)
valid_indices = matching_point_index(matching_point_index > 0);
% 如果过滤后没有有效的索引
if isempty(valid_indices)
best_match = [];
return;
else
% 计算每个匹配点与参考点y坐标的距离
distances = abs(gray_value(matchedPoints(valid_indices, 2)) - ref_point_y);
% 找到距离最小的匹配点
[min_distance, min_index] = min(distances);
% 获取最接近的匹配点的索引
best_match_index = valid_indices(min_index);
% 获取最接近的匹配点的坐标
best_match = matchedPoints(best_match_index, :);
disp('找到最佳匹配点:');
disp(best_match);
end
toc
% 绘制灰度值图形
figure;
plot(gray_value);
hold on; % 保持当前图像,以便在上面添加竖线
% 画左侧的竖线
for i = 1:length(sorted_left_feature)
line([sorted_left_feature(i) sorted_left_feature(i)], [min(gray_value) max(gray_value)], 'Color', 'r', 'LineStyle', '--');
end
% 画右侧的竖线
for i = 1:length(sorted_right_features)
line([sorted_right_features(i) sorted_right_features(i)], [min(gray_value) max(gray_value)], 'Color', 'b', 'LineStyle', '--');
end
hold on; % 保持当前图形,以便在上面添加更多的绘图元素
% 标记参考点
ref_point = [point_x, ref_point_y]; % 创建参考点坐标数组
scatter(ref_point(1), ref_point(2), 'ro', 'filled'); % 以红色圆圈标记参考点
% 检查是否找到了匹配点并标记
if ~isempty(best_match)
scatter(best_match(2), gray_value(best_match(2)), 'go', 'filled'); % 以绿色圆圈标记匹配点
end
% 设置图形的标签和标题
xlabel('x坐标');
ylabel('灰度值');
title('原始信号及参考点与匹配点');
legend('原始信号', '参考点', '匹配点');
hold off; % 释放图形保持状态
figure;imshow(l_uph1ROI, []);
hold on ;
scatter(point_x, point_y, 'ro', 'filled');
scatter(best_match(2), best_match(1), 'go', 'filled');
hold off;
% 将参考点和最佳匹配点组合成一个数组
matchedPair = [refPoint, best_match];
left_x = matchedPair(1);
left_x1 = cols-left_x;
right_x1 = matchedPair(4);
disparity = left_x1-right_x1;
B = 2*T(1);
% 计算深度
XY=[];
depth = (B * fx) ./ disparity;
% 查找坐标 (x, y) 是否存在于矩阵中
indices = find(pointArray1(:, 1) == point_x & pointArray1(:, 2) == point_y);
if ~isempty(indices)
disp(['坐标 (', num2str(x), ', ', num2str(y), ') 存在于矩阵中,索引为:', num2str(indices)]);
else
disp(['坐标 (', num2str(x), ', ', num2str(y), ') 不存在于矩阵中。']);
end