代码如下

主程序

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')

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);