原创

获取已知标定面的三维点

版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/H_____H/article/details/84592124

本文目的根据已经标定的平面计算二维像素坐标(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(:,3)'*RT(:,4) * (1./(RT(:,3)' * [normp; ones(1,size(normp,2))]));
 cpoints=(ones(3,1)*Zc) .* [normp; ones(1,size(normp,2))];

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

0 个人打赏
文章最后发布于: 2018-11-28 20:45:27
展开阅读全文

没有更多推荐了,返回首页

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

打赏

-H-H-

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

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

分享到微信朋友圈

×

扫一扫,手机浏览