# 获取已知标定面的三维点

fx，fy，u0，v0

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


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;


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


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(:,3)'*RT(:,4) * (1./(RT(:,3)' * [normp; ones(1,size(normp,2))]));
cpoints=(ones(3,1)*Zc) .* [normp; ones(1,size(normp,2))];


0 个人打赏

©️2019 CSDN 皮肤主题: 编程工作室 设计师: CSDN官方博客

-H-H-

“你的鼓励将是我创作的最大动力”

5C币 10C币 20C币 50C币 100C币 200C币