获取已知标定面的三维点

本文目的根据已经标定的平面计算二维像素坐标（u，v）所对应的三维坐标（Xc，Yc，Zc），分别列出几种计算方式：

相机内参数mtx，畸变dist，外参RT（4X4）；
fx，fy，u0，v0
首先进行畸变校正

normp=normalize2(uv,[fx,fy],[u0,v0],dist,0);%像素归一化（带畸变矫正）
uv_und= [normp(1,:)*fx+u0; normp(2,:)*fy+v0];%矫正后的像素点

或者调用函数undistortPoint进行校正

方法一:
利用两个平面之间单应性矩阵

H=mtx*[RT(1:3,1),RT(1:3,2),RT(1:3,4)];
H=H/H(9);
wpointsH=inv(H)*cat(1,uv_und,ones(1,size(uv_und,2)));
wpoints=[wpointsH(1,:);wpointsH(2,:);zeros(1,size(wpointsH,2));ones(1,size(wpointsH,2))];
cpoints=RT*wpoints;

方法二：
逐点求取，利用线面求交，平面方程为Av·X+Bv·Y+Cv·Z=1
根据相机的射影矩阵建立方程求解

cpoints=ones(4,imgrows*imgcols);
for i = 1:size(uv_und,2)
       A=[fx,0,u0-uv_und(1,i);0,fy,v0-uv_und(2,i);Av,Bv,Cv];
       b=[0 0 1]'; 
       tempc=A\b;
       cpoints(1:3,i)=tempc;
end

方法三：obtain3Dpoints_from_knownPlane

cpoints = obtain3Dpoints_from_knownPlane(mtx,eye(4),RT(1:3,1:3),RT(1:3,4), uv_und);


%sub function
function points3D_cameraFrame = obtain3Dpoints_from_knownPlane(Camera_intrinsics,...
                                                               Camera_extrinsics, ...
                                                               PlaneRotation_CameraFrame,...
                                                               PlaneTranslation_cameraFrame,...
                                                               points2D_CameraFrame)
%Input：
%Camera_extrinsics=eye(4)
%points2D_CameraFrame:size 2XN


% coordinate sistem of the screen plane
ScreenPlane_Normal_vect = PlaneRotation_CameraFrame(:,3); %is the Z coordinate of the rotation matrix
ScreenPlane_Origin = PlaneTranslation_cameraFrame;

% Normalize(augmented) the 2d points
ProjectedGrid_2dpoints_cameraPlane = cat(1,points2D_CameraFrame, ...
                                           ones(1,size(points2D_CameraFrame,2)));
                                           
% obtain the rays corresponding to the corners
A_camera = [Camera_intrinsics [0 0 0]']* Camera_extrinsics; % Camrea transformation Kin*wKc
ProjectedGrid_3drays_cameraFrame = pinv(A_camera)*ProjectedGrid_2dpoints_cameraPlane;

% obtain the scale for the ray that cross the screen
ScreenZDistance =(-1)*dot(ScreenPlane_Origin, ScreenPlane_Normal_vect); %Obtain the Z distance, Projecting the Screen origin over its Z coordinate
RayZprojection = dot( ScreenPlane_Normal_vect(:)* ones(1,size( ProjectedGrid_3drays_cameraFrame, 2)), ...
                      ProjectedGrid_3drays_cameraFrame(1:3,:));
RayScale = (-1*ScreenZDistance) ./ RayZprojection;

% obtain the 3D points of the projected Grid
points3D_cameraFrame = (ones(3,1)*RayScale) .* ProjectedGrid_3drays_cameraFrame(1:3,:);

end

方法四：方法三的简化版

normp=normalize2(uv,[fx,fy],[u0,v0],dist,0);%像素归一化（带畸变矫正）
 Zc = RT(1:3,3)'*RT(1:3,4) * (1./(RT(1:3,3)' * [normp; ones(1,size(normp,2))]));
 cpoints=(ones(3,1)*Zc) .* [normp; ones(1,size(normp,2))];

以上方法都经本人亲测，希望对朋友们有所帮助！
有啥问题欢迎批评指正！0.0