【python】【matlab】【SGBM】双目相机计算车辆前方车辙高度（直行工况）

1.单目相机-计算前方车辙的像素坐标

题目的单目相机指双目相机的左相机（需提前标定）。
直行工况下，当相机标定完成后，相对于左右车轮的距离信息固定，因此直行时可以认为前方车辙是确定的
matlab代码如下，相机参数、车辆参数需提前标定
（假设相机到左轮中心平面的距离为0.75米，到右轮中心平面的距离为0.75米，轮胎宽0.2米，计算车轮前3-30米车辙的像素坐标）

%直线工况车辙预测
%假设相机在车辆坐标系z轴上，仅与车辆坐标系y轴有转角
clc
clear all
close all
%% 原始参数
%相机参数
K=[257.786374731449,0,165.410335513033;...
    0,256.002910359339,236.281905764687;...
    0,0,1];%左相机内参矩阵
pitch = 0/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=1;%单位是米
%车辆参数
x_left_wheel=0.75;%相机到左车轮车辙中心线的距离在车辆坐标系y坐标上的投影长度，米
x_right_wheel=0.75;%相机到右车轮车辙中心线的距离在车辆坐标系y坐标上的投影长度，米
wheel_width=0.2;%车轮宽度，米
front_large=30;%最大探测范围：车辆前方道路30米
front_small=3;%最小探测范围：车辆前方道路3米
PianYi=150;%图片偏移量，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