结构光逆相机法重建详解+代码

num_x = 11; %number of circles in the x direction of the calibration board
num_y = 9; %number of circles in the y direction of the calibration board
dist_circ = 25; % 标定板


disp('开始相机标定...');

% 角点的像素坐标系
load('camera_imagePoints.mat'); % load the camera image points of the centers

% 角点的世界坐标系
worldPoints = generateCheckerboardPoints([num_y+1,num_x+1], dist_circ);

% 标定相机
[cameraParams,imagesUsed,estimationErrors] = estimateCameraParameters(imagePoints,worldPoints, 'EstimateTangentialDistortion', true);

figure;showReprojectionErrors(cameraParams);title('相机标定的重投影误差');
figure;showExtrinsics(cameraParams);title('相机标定的外参');

% save Rc_1, Tc_1, KK, inv_KK,
% 相机相对于世界坐标系原点的位姿（旋转+平移=外参）
Rc_1 = cameraParams.RotationMatrices(:,:,1);
Rc_1 = Rc_1';    % 转置
Tc_1 = cameraParams.TranslationVectors(1, :);
Tc_1 = Tc_1';
 % 相机内参
KK = cameraParams.IntrinsicMatrix';
save('CamCalibResult.mat', 'Rc_1', 'Tc_1', 'KK');

%% step2: 标定投影仪
disp('开始投影仪标定...');
% 加载投影仪下圆心的像素坐标
load('projector_imagePoints.mat'); % load the projector image points of the centers
% 圆心的世界坐标
worldPoints = generateCheckerboardPoints([num_y+1,num_x+1], dist_circ);
[prjParams,imagesUsed,estimationErrors] = estimateCameraParameters(prjPoints,worldPoints, 'EstimateTangentialDistortion', true);
figure; showReprojectionErrors(prjParams); title('投影仪的重投影误差');
figure; showExtrinsics(prjParams);title('投影仪的外参');

% 保存参数
Rc_1 = prjParams.RotationMatrices(:,:,1);
Rc_1 = Rc_1';
Tc_1 = prjParams.TranslationVectors(1, :);
Tc_1 = Tc_1';
KK = prjParams.IntrinsicMatrix';
save('PrjCalibResult.mat', 'Rc_1', 'Tc_1', 'KK');

%% step1: input parameters
width = 640; % camera width
height = 480; % camera height
prj_width = 912; % projector width

%camera: Projection matrix Pc
load('CamCalibResult.mat');
Kc = KK;  % 相机内参
Ac = Kc * [Rc_1, Tc_1]; % 计算相机投影矩阵

%projector: Projection matrix Ap
load('PrjCalibResult.mat');
Kp = KK;  % 投影仪内参
Ap = Kp * [Rc_1, Tc_1];  % 计算投影仪投影矩阵

Ac = Kc * [Rc_1, Tc_1];
Ap = Kp * [Rp_1, Rc_2];

% 条纹频率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;

% 3D重建
Xws = nan(height, width);
Yws = nan(height, width);
Zws = nan(height, width);

for y = 1:height
   for x = 1:width
       if ~isnan(up_test_obj(y, x))
           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);