帮我一行一行的解读代码function [pha_absolute, dif] = m_calc_absolute_phase(files_phaseShift, files_grayCode, IT, B_min, win_size)
[~, N] = size(files_phaseShift);
[pha_wrapped, B] = m_calc_warppred_phase(files_phaseShift, N);
[~, n] = size(files_grayCode);
n = n - 2; % 鎶曞奖浜嗕竴榛戜竴鐧戒袱骞呭浘鐗�
Ks = m_calc_gray_code(files_grayCode, IT, n);
pha_absolute = pha_wrapped + 2 * pi .* Ks;
% 璋冨埗搴︽护娉�
B_mask = B > B_min;
pha_absolute = pha_absolute .* B_mask;
% 杈圭紭璺冲彉璇樊
[pha_absolute, dif] = m_filter2d(pha_absolute, win_size);
% 褰掍竴鍖�
pha_absolute = pha_absolute / (2 * pi * 2^ n);
endfunction [Ks] = m_calc_gray_code(files, IT, n)
% 01 璇诲彇姣忎竴寮犲浘鐗囪繘Is
[~, num] = size(files);
img = imread(files{1});
[h, w] = size(img);
Is = zeros(num, h, w);
for i = 1: num
img = imread(files{i});
Is(i, :, :) = double(img);
end
% 02 璁＄畻Is_Max銆両s_Min锛屽姣忎釜鐐硅繘琛岄槇鍊煎垽鏂�
Is_max = max(Is);
Is_min = min(Is);
Is_std = (Is - Is_min) ./ (Is_max - Is_min);
gcs = Is_std > IT;
% 03 瀵规瘡涓儚绱犵偣锛岃绠楃紪鐮佸�糣
Vs_row = zeros(1, 2 ^ n, 'uint8');
codes = m_gray_code(n);
for i = 1: 2 ^ n
code = str2mat(codes(i)); %#ok<DSTRMT>
V = 0;
for j = 1: n
V = V + str2num(code(j)) * 2^ (4 - j); %#ok<ST2NM>
end
Vs_row(1, i) = V;
end
% 04 寤虹珛 V - > K 鐨勬槧灏勮〃
V2K = containers.Map();
for K = 1: 2 ^ n
V = Vs_row(1, K);
V2K(int2str(V)) = K - 1;
end
Ks = zeros(h, w);
for v = 1: h
%disp(strcat("绗�", int2str(v), "琛�"));
for u = 1: w
% 涓嶉渶瑕佹渶鍚庨粦銆佺櫧涓ゅ箙鍥剧墖鐨勭紪鐮�
gc = gcs(1: n, v, u);
V = 0;
for i = 1: n
V = V + gc(i) * 2 ^ (4 - i);
end
% 涓昏鐨勬�ц兘鐡堕
Ks(v, u) = V2K(int2str(V));
end
end
end
%% 计算包裹相位
function [pha, B] = m_calc_warppred_phase(files, N)
sin_sum = 0;
cos_sum = 0;
for k = 0: N - 1
Ik = m_imread(files{k + 1}); % 读取图片
Ik = m_filter2d(Ik);
sin_sum = sin_sum + Ik * sin(2 * k * pi / N);
cos_sum = cos_sum + Ik * cos(2 * k * pi / N);
end
% 根据计算相位、调制度
pha = atan2(sin_sum, cos_sum);
B = sqrt(sin_sum .^ 2 + cos_sum .^ 2) * 2 / N;
%% 尝试注释掉这段，自己从零实现一遍
% 为了将波折相位转为单个周期内单调递增
pha = - pha;
pha_low_mask = pha <= 0;
pha = pha + pha_low_mask .* 2. * pi;
end
%% 读取图片
function [img] = m_imread(file)
img = imread(file);
img = double(((img(:, :, 1)))); % 转换灰度图
end
%% 高斯滤波
function [img] = m_filter2d(img)
w = 3.;
sigma = 1.;
kernel = fspecial("gaussian", [w, w], sigma);
img = imfilter(img, kernel, "replicate");
end% 滤波
function [pha_new, dif] = m_filter2d(pha, win_size)
% 中值滤波（格雷码边缘处计算出现问题）
pha_new = medfilt2(pha, [win_size, win_size]);
% （剔除未编码区域）
dif = pha - pha_new;
endfunction [code] = m_gray_code(n)
if (n < 1)
disp("鏍奸浄鐮佹暟閲忓繀椤诲ぇ浜�0");
return;
elseif (n == 1)
% 浜х敓0銆�1 涓や釜鏁板瓧
code = ["0", "1"];
% 杩斿洖code
else
code_pre = m_gray_code(n - 1);
[~, num] = size(code_pre);
% 鍒濆鍖栦竴涓暟缁�
code = repmat("", 1, num * 2);
% step1锛氭瘡涓瓧绗︿覆鍓嶉潰閮�+0
idx = 0;
for i = 1: num
idx = idx + 1;
code(idx) = "0" + code_pre(i);
end
% step2锛氱炕杞涓厓绱狅紝鍏朵綑鍙栧绉�
for i = num: -1: 1
idx = idx + 1;
code(idx) = "1" + code_pre(i);
end
end
endfunction [patterns] = m_make_gray_code_patterns(n, W, H)
codes = m_gray_code(n);
% 灏嗗瓧绗︿覆鏍奸浄鐮佽浆鎹负鐭╅樀鏍煎紡
[~, num] = size(codes);
codes_mat = zeros(n, num, 'int8');
for col = 1: num
code_col = str2mat(codes(col)); %#ok<DSTRMT>
for row = 1: n
codes_mat(row, col) = str2num(code_col(row)); %#ok<ST2NM>
end
end
W_per = round(W / num);
% 姣忓紶鍥剧墖
patterns = zeros(n + 2, H, W, "uint8");
for idx = 1: n
% 涓�琛屽浘鐗�
row_one = zeros(1, W, "uint8");
% 姣忎釜鏍煎瓙
for i = 1 : num
gray = codes_mat(idx, i);
% 鏍煎瓙閲屾瘡涓儚绱�
for w = 1: W_per
row_one(1, (i - 1) * W_per + w) = gray;
end
end
row_one = row_one * 255;
pattern = repmat(row_one, H, 1);
patterns(idx, :, :) = pattern;
end
% 鍏ㄩ粦銆佸叏鐧藉浘鐗囷紝鐢ㄤ簬纭畾姣忎釜鍍忕礌闃堝��
patterns(n + 2, :, :) = ones(H, W, 'uint8') * 255;
end% 函数：生成相移条纹
function [Is, Is_img] = m_make_phase_shift_patterns(A, B, T, N, W, H)
Is = cell(N, 1);
Is_img = zeros(N, H, W);
xs = 1: W;
f_2pi = 1. / double(T) * 2. * pi;
for k = 0: N - 1
Is{k + 1} = A + B * cos(f_2pi * xs + 2 * k / N * pi);
Is_img(k + 1, :, :) = repmat(Is{k + 1} / 255., H, 1);
end
end
close all;
clc; clear;
tic;
%% 01 鍙傛暟閰嶇疆
W = 1280;
H = 720;
save_folder = "data/project"; mkdir(save_folder);
save_file_csv = strcat(save_folder, "/","patterns.csv"); % 鐢ㄤ簬鍐欏叆鍒版姇褰变华
n = 4; % 鏍奸浄鐮佷綅鏁�
% 鐩哥Щ鍙傛暟
N = 12; % 鐩哥Щ姝ユ暟
A = 130;
B = 90;
TW = W / (2 ^ n);
TH = H / (2 ^ n);
%% 02 鐢熸垚鐩哥Щ娉曞浘鍍�
[~, patterns_phaseshift_X] = m_make_phase_shift_patterns(A, B, TW, N, W, H);
[~, temp_Y] = m_make_phase_shift_patterns(A, B, TH, N, H, W);
patterns_phaseshift_Y = zeros(N, H, W);
for i = 1: N
patterns_phaseshift_Y(i, :, :) = squeeze(temp_Y(i, :, :))';
end
%% 03 鐢熸垚鏍奸浄鐮佸浘鍍忥細X鏂瑰悜
patterns_graycode_X = m_make_gray_code_patterns(n, W, H);
temp_Y = m_make_gray_code_patterns(n, H, W);
[num, H, W] = size(patterns_graycode_X);
patterns_graycode_Y = zeros(num, H, W);
for i = 1: num
patterns_graycode_Y(i, :, :) = squeeze(temp_Y(i, :, :))';
end
%% 04 鍐欏叆鍥惧儚
idx = 0;
file = fopen(save_file_csv, "w+");
% 鍐欏叆鐩哥Щ鍥炬 X
for i = 1: N
idx = idx + 1;
% 鍐欏叆鍥剧墖
save_file_img = strcat(save_folder, "/", int2str(idx), ".bmp");
disp("鍐欏叆鏂囦欢鍒�:" + save_file_img);
img = squeeze(patterns_phaseshift_X(i, :, :));
imwrite(img, save_file_img);
% 鍐欏叆csv鏂囦欢
img_row = squeeze(img(1, :)) * 255.;
for w = 1: W
fprintf(file, strcat(int2str(round(img_row(w))), ","));
end
fprintf(file, "\n");
end
% 鍐欏叆鏍奸浄鐮佺▼搴� X
for i = 1: num
idx = idx + 1;
% 鍐欏叆鍥惧儚
save_file_img = strcat(save_folder, "/", int2str(idx), ".bmp");
disp("鍐欏叆鏂囦欢鍒�:" + save_file_img);
img = squeeze(patterns_graycode_X(i, :, :));
imwrite(img, save_file_img);
% 鍐欏叆鍥惧儚
img_row = squeeze(img(1, :));
for w = 1: W
fprintf(file, strcat(int2str(round(img_row(w))), ","));
end
fprintf(file, "\n");
end
% 鍐欏叆鐩哥Щ鍥炬 Y
for i = 1: N
idx = idx + 1;
% 鍐欏叆鍥剧墖
save_file_img = strcat(save_folder, "/", int2str(idx), ".bmp");
disp("鍐欏叆鏂囦欢鍒�:" + save_file_img);
img = squeeze(patterns_phaseshift_Y(i, :, :));
imwrite(img, save_file_img);
% 鍐欏叆csv鏂囦欢
img_row = squeeze(img(1, :)) * 255.;
for w = 1: W
fprintf(file, strcat(int2str(round(img_row(w))), ","));
end
fprintf(file, "\n");
end
% 鍐欏叆鏍奸浄鐮佺▼搴� Y
for i = 1: num
idx = idx + 1;
% 鍐欏叆鍥惧儚
save_file_img = strcat(save_folder, "/", int2str(idx), ".bmp");
disp("鍐欏叆鏂囦欢鍒�:" + save_file_img);
img = squeeze(patterns_graycode_Y(i, :, :));
imwrite(img, save_file_img);
% 鍐欏叆鍥惧儚
img_row = squeeze(img(1, :));
for w = 1: W
fprintf(file, strcat(int2str(round(img_row(w))), ","));
end
fprintf(file, "\n");
end
disp("鍐欏叆瀹屾垚");
fclose(file);
toc;close all;
clc; clear;
tic;
%% 01 鍙傛暟閰嶇疆
calib_folder = "data/calib";
N = 12;
n = 4;
num = n + 2;
B_min = 10; % 浣庝簬杩欎釜璋冨埗搴︾殑鎴戜滑灏辫涓哄畠鐨勭浉浣嶄俊鎭笉鍙潬
IT = 0.5; % 鏍奸浄鐮侀槇鍊�
win_size = 7; % 涓�兼护娉㈢獥鍙ｅぇ灏�
W = 1280;
H = 720;
points_per_row = 7;
points_per_col = 6;
w = 2;
load("data/calib/camera_imagePoints.mat");
[~, ~, calib_num] = size(imagePoints);
prjPoints = zeros(size(imagePoints));
%% 02 鏍囧畾鎶曞奖浠�佺浉鏈�
for calib_idx = 1: calib_num
disp(calib_idx);
data_folder = calib_folder + "/" + num2str(calib_idx);
%% 02 杩戜技鏌ョ湅鍥惧儚鍦嗗績
img = 255 - imread(data_folder + "/18.bmp");
for i = 1: points_per_row * points_per_col
xy = imagePoints(i, :, calib_idx);
x = round(xy(1));
y = round(xy(2));
img(y, x) = 255;
end
figure(); mesh(img);
%% 03 瑙\Y鐩镐綅
files_phaseShiftX = cell(1, N);
idx = 1;
for i = 1: N
files_phaseShiftX{i} = strcat(data_folder, "/", int2str(idx), ".bmp");
idx = idx + 1;
end
files_grayCodeX = cell(1, num);
for i = 1: num
files_grayCodeX{i} = strcat(data_folder, "/", int2str(idx), ".bmp");
idx = idx + 1;
end
files_phaseShiftY = cell(1, N);
for i = 1: N
files_phaseShiftY{i} = strcat(data_folder, "/", int2str(idx), ".bmp");
idx = idx + 1;
end
files_grayCodeY = cell(1, num);
for i = 1: num
files_grayCodeY{i} = strcat(data_folder, "/", int2str(idx), ".bmp");
idx = idx + 1;
end
[phaX, difX] = m_calc_absolute_phase(files_phaseShiftX, files_grayCodeX, IT, B_min, win_size);
[phaY, difY] = m_calc_absolute_phase(files_phaseShiftY, files_grayCodeY, IT, B_min, win_size);
phaX = phaX * W;
phaY = phaY * H;
for i = 1: points_per_row * points_per_col
xy = imagePoints(i, :, calib_idx);
x = xy(1); y = xy(2);
x_round = round(x);
y_round = round(y);
% 瀵箈銆亂闄勮繎瀵圭浉浣嶈繘琛屾牱鏉℃洸绾挎彃鍊�
xs = zeros(1, 2 * w + 1);
ys = zeros(1, 2 * w + 1);
phas_x = zeros(1, 2 * w + 1);
phas_y = zeros(1, 2 * w + 1);
ii = 1;
for j = - 1 * w: w
xs(1, ii) = x_round + j;
ys(1, ii) = y_round + j;
phas_x(1, ii) = phaX(y_round, xs(1, ii));
phas_y(1, ii) = phaY(ys(1, ii), x_round);
ii = ii + 1;
end
pha_x = spline(xs, phas_x, x);
pha_y = spline(ys, phas_y, y);
prjPoints(i, :, calib_idx) = [pha_x, pha_y];
end
end
save(calib_folder + "\projector_imagePoints.mat", 'prjPoints');
toc;%% Clear everything existing.
clc; clear;
close all;
data_folder = "data/model";
N = 12;
n = 4;
num = n + 2;
B_min = 10; % 浣庝簬杩欎釜璋冨埗搴︾殑鎴戜滑灏辫涓哄畠鐨勭浉浣嶄俊鎭笉鍙潬
IT = 0.5; % 鏍奸浄鐮侀槇鍊�
win_size = 7; % 涓�兼护娉㈢獥鍙ｅぇ灏�
%% step1: input parameters
width = 1280; % camera width
height = 1024; % camera height
prj_width = 1280; % projector width
%camera: Projection matrix Pc
load('CamCalibResult.mat');
Kc = KK; % 鐩告満鍐呭弬
Ac = Kc * [Rc_1, Tc_1];
%projector: Projection matrix Pp
load('PrjCalibResult.mat');
Kp = KK; % 鎶曞奖浠唴鍙�
Ap = Kp * [Rc_1, Tc_1];
%% step2: 璇诲彇娴嬭瘯鍥剧墖骞朵笖璁＄畻涓夌淮閲嶅缓
% % 鏉＄汗棰戠巼64锛屼篃鏄棿璺濓紙涓�涓懆鏈熺敱64涓儚绱犵粍鎴愶級鐢ㄤ簬璁＄畻缁濆鐩镐綅锛岄鐜�1銆�8鐢ㄤ簬鍖呰９鐩镐綅灞曞紑
% f = 64; % 鏉＄汗棰戠巼锛堝崟涓懆鏈熸潯绾圭殑鍍忕礌涓暟锛夛紝鍗砅
% load('up_test_obj.mat');
% up_test_obj = up_test_obj / f; % 灏嗙浉浣嶅綊涓�鍖栧埌[0, 2pi]涔嬮棿
%
% figure; imshow(up_test_obj / (2 * pi)); colorbar; title("鐩镐綅鍥�, freq=" + num2str(f));
% figure; mesh(up_test_obj); colorbar; title("鐩镐綅鍥�, freq=" + num2str(f));
%
% % 璁＄畻鎶曞奖浠潗鏍�
% x_p = up_test_obj / (2 * pi) * prj_width;
idx = 1;
files_phaseShiftX = cell(1, N);
for i = 1: N
files_phaseShiftX{i} = strcat(data_folder, "/", int2str(idx), ".bmp");
idx = idx + 1;
end
files_grayCodeX = cell(1, num);
for i = 1: num
files_grayCodeX{i} = strcat(data_folder, "/", int2str(idx), ".bmp");
idx = idx + 1;
end
[phaX, difX] = m_calc_absolute_phase(files_phaseShiftX, files_grayCodeX, IT, B_min, win_size);
up_test_obj = phaX * 2 * pi;
x_p = phaX * prj_width;
% 3D閲嶅缓
Xws = nan(height, width);
Yws = nan(height, width);
Zws = nan(height, width);
for y = 1:height
for x = 1:width
if ~(up_test_obj(y, x) == 0)
uc = x - 1;
vc = y - 1;
up = (x_p(y, x) - 1);
% Eq. (32) in the reference paper.
A = [Ac(1,1) - Ac(3,1) * uc, Ac(1,2) - Ac(3,2) * uc, Ac(1,3) - Ac(3,3) * uc;
Ac(2,1) - Ac(3,1) * vc, Ac(2,2) - Ac(3,2) * vc, Ac(2,3) - Ac(3,3) * vc;
Ap(1,1) - Ap(3,1) * up, Ap(1,2) - Ap(3,2) * up, Ap(1,3) - Ap(3,3) * up];
b = [Ac(3,4) * uc - Ac(1,4);
Ac(3,4) * vc - Ac(2,4);
Ap(3,4) * up - Ap(1,4)];
XYZ_w = inv(A) * b;
Xws(y, x) = XYZ_w(1); Yws(y, x) = XYZ_w(2); Zws(y, x) = XYZ_w(3);
end
end
end
% 鐐逛簯鏄剧ず
xyzPoints(:, 1) = Xws(:);
xyzPoints(:, 2) = Yws(:);
xyzPoints(:, 3) = Zws(:);
ptCloud = pointCloud(xyzPoints);
xlimits = [min(Xws(:)), max(Xws(:))];
ylimits = [min(Yws(:)), max(Yws(:))];
zlimits = ptCloud.ZLimits;
player = pcplayer(xlimits, ylimits, zlimits);
xlabel(player.Axes,'X (mm)');
ylabel(player.Axes,'Y (mm)');
zlabel(player.Axes,'Z (mm)');
view(player,ptCloud);
扫一扫
1万+
到【灌水乐园】发言
