【matlab】【单目相机】世界坐标-像素坐标转换、面积映射

1.世界坐标->像素坐标

（1.1）代码如下

主程序

clc
clear all
close all
%%
focalLength = [800 800];
principalPoint = [320 240];
imageSize = [480 640];
intrinsics = cameraIntrinsics(focalLength,principalPoint,imageSize);
height = 2.18;
pitch = 14;
sensor = monoCamera(intrinsics,height,'Pitch',pitch);
xyVehicleLoc1 = [12 0 0];
xyImageLoc1 = vehicleToImage(sensor,xyVehicleLoc1)

%%
xw=xyVehicleLoc1(1);
yw=xyVehicleLoc1(2);
zw=0;
h=height;
K=intrinsics.IntrinsicMatrix';%左相机内参矩阵
pitch = -1*pitch/180*pi;%单位是弧度，方向原因需要乘-1
Rc=[1 0 0;...
    0 cos(pitch) sin(pitch);...
    0 -sin(pitch) cos(pitch)];
R=[0 -1 0;0 0 -1;1 0 0];
[A1,A2]=VehicleToCamera(xw,yw,zw,h,R,Rc,K);
xyImageLoc2=[A1,A2]

%%
Ioriginal = imread('road.png');
IvehicleToImage = insertMarker(Ioriginal,xyImageLoc1);
figure
imshow(IvehicleToImage)
title('Vehicle-to-Image Point')

（1.2）VehicleToCamera.m函数

function [A1,A2]=VehicleToCamera(xw,yw,zw,h,R,Rc,K)
world=[xw;yw;zw];
A=K*Rc*(R*world+[0;h;0]);
A1=A(1)/A(3);
A2=A(2)/A(3);

2.像素坐标->世界坐标

特例

（2.1）主代码

%由像素坐标反推世界坐标
%特例，相机仅绕相机坐标系x轴（世界坐标y轴）旋转pitch角度
%相机离地高度为height
%相机坐标系位于世界坐标系z轴（Tcx=Tcz=0）
%求地面目标点（即世界坐标系中zw=0的点）的坐标
clc
clear
close all
%% 像素坐标
u=320;
v=1240.6;
%% 内参
focalLength = [800 800];
principalPoint = [320 240];
imageSize = [480 640];
intrinsics = cameraIntrinsics(focalLength,principalPoint,imageSize);
height = 2.18;%相机高度
pitch = 14;%编转角度

h=height;
K=intrinsics.IntrinsicMatrix';%左相机内参矩阵
%% 输出世界坐标
[xw,yw,zw]=CameraToVehicle(pitch,K,u,v,h)

（2.2）CameraToVehicle.m函数

function [xw,yw,zw]=CameraToVehicle(pitch,K,u,v,h)
pitch = -1*pitch/180*pi;%单位是弧度，方向原因需要乘-1
Rc=[1 0 0;...
    0 cos(pitch) sin(pitch);...
    0 -sin(pitch) cos(pitch)];
% R=[0 -1 0;0 0 -1;1 0 0];
%% 由像素坐标反推世界坐标
W0_1=inv(Rc)*inv(K)*[u;v;1];
Tcy=h;
W0=W0_1*Tcy/W0_1(2);
%世界坐标

xw=W0(3);
yw=-W0(1);
zw=0;

3.像素坐标-世界坐标映射

计算各像素块在世界坐标系下的面积（用AS表示）
计算各像素块左上角(xw2,yw2)/右下角(xw,yw)在世界坐标系下的坐标

%由像素坐标反推世界坐标
%特例，相机仅绕相机坐标系x轴（世界坐标y轴）旋转pitch角度
%相机离地高度为height
%相机坐标系位于世界坐标系z轴（Tcx=Tcz=0）
%求地面目标点（即世界坐标系中zw=0的点）的坐标
clc
clear
close all

%% 内参
focalLength = [800 800];%[800 800];
principalPoint = [120 180];%[行坐标 列坐标];
imageSize =[360 240] ;%[行数 列数]
intrinsics = cameraIntrinsics(focalLength,principalPoint,imageSize);
height = 1.04;%相机高度
pitch = 17;%编转角度

h=height;
K=intrinsics.IntrinsicMatrix';%左相机内参矩阵

%% 像素坐标
%xw记录各像素块右下角点的世界坐标x坐标
%yw记录各像素块右下角点的世界坐标y坐标
xw=zeros();
yw=zeros();
for u=1:240
    for v=1:360
        [xw0,yw0,~]=CameraToVehicle(pitch,K,u,v,h);
        xw(v,u)=xw0;
        yw(v,u)=yw0;
    end
end
%xw2记录各像素块左上角点的世界坐标x坐标
%yw2记录各像素块坐上角点的世界坐标y坐标
xw2=zeros();
yw2=zeros();
for u=1:120
    for v=1:360
        [xw0,yw0,~]=CameraToVehicle(pitch,K,u-1,v-1,h);
        xw2(v,u)=xw0;
        yw2(v,u)=yw0;
    end
end
yw3=yw2;
yw3(:,121)=zeros(360,1);
yw3(361,2:121)=yw(360,1:120);
[~,yw000,~]=CameraToVehicle(pitch,K,0,360,h);
yw3(361,1)=yw000;
AreaSquare=zeros();
for u=1:240
    for v=1:360
        %下式计算各像素块的面积
        AreaSquare(v,u)=0.5*(xw2(v,u)-xw(v,u))*(-yw3(v,u+1)+yw3(v,u)-yw3(v+1,u+1)+yw3(v+1,u));
    end
end
%AS记录各像素块在世界坐标下的面积
AS=zeros(360,120);
for i=1:240
    AS(:,i)=AreaSquare(:,1);
end