【matlab】直线行驶车辙预测

matlab代码

通过坐标转换，将汽车直行时的前方车辙标记出

%直线工况车辙预测
%假设相机在车辆坐标系z轴上，仅与车辆坐标系y轴有转角
clc
clear
close all
%% 原始参数
%相机参数
K=[800,0,320;...
    0,800,240;...
    0,0,1];%左相机内参矩阵
pitch = 5/180*pi;%单位是弧度
pitch=-pitch;%方向原因需要乘-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];
h=2.18;%单位是米
%车辆参数，代码核心
x_left_wheel=1.5;%相机到左车轮车辙中心线的距离在车辆坐标系y坐标上的投影长度，米
x_right_wheel=1.5;%相机到右车轮车辙中心线的距离在车辆坐标系y坐标上的投影长度，米
wheel_width=0.2;%车轮宽度，米
front_large=50;%最大探测范围：车辆前方道路10米
front_small=10;%最小探测范围：车辆前方道路5米
PianYi=0;%图片偏移量，150（因为150*2+180=480）
%% 左侧车辙
zw=0;
%左侧左前left_LF
xw=front_large;
yw=x_left_wheel+wheel_width/2;
[u_left_LF,v_left_LF]=VehicleToCamera(xw,yw,zw,h,R,Rc,K);
%左侧右前left_RF
xw=front_large;
yw=x_left_wheel-wheel_width/2;
[u_left_RF,v_left_RF]=VehicleToCamera(xw,yw,zw,h,R,Rc,K);
%左侧左后left_LR
xw=front_small;
yw=x_left_wheel+wheel_width/2;
[u_left_LR,v_left_LR]=VehicleToCamera(xw,yw,zw,h,R,Rc,K);
%左侧右后left_RR
xw=front_small;
yw=x_left_wheel-wheel_width/2;
[u_left_RR,v_left_RR]=VehicleToCamera(xw,yw,zw,h,R,Rc,K);

uuu1=zeros();
iii1=1;
for iii=round(v_left_LF):1:round(v_left_LR)
   y1=(u_left_LR-u_left_LF)/(v_left_LR-v_left_LF)*iii+u_left_LR-(u_left_LR-u_left_LF)/(v_left_LR-v_left_LF)*v_left_LR;
   uuu1(iii1,1)=y1;
   uuu1(iii1,2)=iii-PianYi;
   iii1=iii1+1;
end
uuu2=zeros();
iii2=1;
for iii=round(v_left_RF):1:round(v_left_RR)
   y1=(u_left_RR-u_left_RF)/(v_left_RR-v_left_RF)*iii+u_left_RR-(u_left_RR-u_left_RF)/(v_left_RR-v_left_RF)*v_left_RR;
   uuu2(iii2,1)=y1;
   uuu2(iii2,2)=iii-PianYi;
   iii2=iii2+1;
end
xyI0=[uuu1,uuu2];
[aaa,~]=size(xyI0);
zuobiao_left=zeros();
qqq=1;
for i=1:aaa
    u1=round(uuu1(i,1));
    u2=round(uuu2(i,1));
    for ii=u1:1:u2
        zuobiao_left(qqq,1)=ii;
        zuobiao_left(qqq,2)=uuu1(i,2);
        qqq=qqq+1;
    end
end
%% 右侧车辙
zw=0;
%右侧左前right_LF
xw=front_large;
yw=x_right_wheel+wheel_width/2;
[u_right_LF,v_right_LF]=VehicleToCamera(xw,yw,zw,h,R,Rc,K);
%右侧右前right_RF
xw=front_large;
yw=x_right_wheel-wheel_width/2;
[u_right_RF,v_right_RF]=VehicleToCamera(xw,yw,zw,h,R,Rc,K);
%右侧左后right_LR
xw=front_small;
yw=x_right_wheel+wheel_width/2;
[u_right_LR,v_right_LR]=VehicleToCamera(xw,yw,zw,h,R,Rc,K);
%右侧右后right_RR
xw=front_small;
yw=x_right_wheel-wheel_width/2;
[u_right_RR,v_right_RR]=VehicleToCamera(xw,yw,zw,h,R,Rc,K);

uuu1=zeros();
iii1=1;
for iii=round(v_right_LF):1:round(v_right_LR)
   y1=(u_right_LR-u_right_LF)/(v_right_LR-v_right_LF)*iii+u_right_LR-(u_right_LR-u_right_LF)/(v_right_LR-v_right_LF)*v_right_LR;
   uuu1(iii1,1)=y1;
   uuu1(iii1,2)=iii-PianYi;
   iii1=iii1+1;
end
uuu2=zeros();
iii2=1;
for iii=round(v_right_RF):1:round(v_right_RR)
   y1=(u_right_RR-u_right_RF)/(v_right_RR-v_right_RF)*iii+u_right_RR-(u_right_RR-u_right_RF)/(v_right_RR-v_right_RF)*v_right_RR;
   uuu2(iii2,1)=y1;
   uuu2(iii2,2)=iii-PianYi;
   iii2=iii2+1;
end
xyI0=[uuu1,uuu2];
[aaa,~]=size(xyI0);
zuobiao_right=zeros();
qqq=1;
for i=1:aaa
    u1=round(uuu1(i,1));
    u2=round(uuu2(i,1));
    for ii=u1:1:u2
        zuobiao_right(qqq,1)=ii;
        zuobiao_right(qqq,2)=uuu1(i,2);
        qqq=qqq+1;
    end
end
zuobiao_right(:,1)=round(K(1,3)*2-zuobiao_right(:,1));
%% 绘图
xyI=[zuobiao_left;zuobiao_right];
Ioriginal = imread('road.png');
IvehicleToImage = insertMarker(Ioriginal,xyI);
figure
imshow(IvehicleToImage)

效果如下