以下代码检测的车道线的数据来源是什么？
%% 1. 场景创建
scenario = drivingScenario('SampleTime', 0.1, 'StopTime', 30);
roadCenters = [0 0 0; 50 0 0; 100 20 0; 200 40 0; 250 50 0; 300 40 0];
road(scenario, roadCenters, 'Lanes', lanespec(3));
% 添加车辆
vehicles = [...
vehicle(scenario, 'ClassID', 1, 'Position', [0 0 0], 'Length', 4.7, 'Width', 1.8, 'Height', 1.4),...
vehicle(scenario, 'ClassID', 1, 'Position', [40 1 0], 'Length', 4.7, 'Width', 1.8, 'Height', 1.4),...
vehicle(scenario, 'ClassID', 1, 'Position', [60 -1 0], 'Length', 4.7, 'Width', 1.8, 'Height', 1.4)...
];
egoVehicle = vehicles(1);
sideVehicle_1=vehicles(2);
sideVehicle_2=vehicles(3);
%设置主车轨迹sideVehicle1=vehicles(2);sideVehicle1=vehicles(2);
trajectoryPoints = [...
0 0 0;
50 0 0;
100 20 0;
200 40 0;
250 50 0;
300 40 0];
speedProfile = [10, 12, 15, 18, 15,10]; % 分段速度(m/s)
trajectory(egoVehicle, trajectoryPoints, speedProfile);
%% 2. 数据采集 - 优化初始化
% 预分配存储空间
totalFrames = round(scenario.StopTime/scenario.SampleTime);
cameraFrames = cell(totalFrames, 1);
vehiclePoses = zeros(totalFrames, 3);
% 初始化图形系统
[hFig, hAx, actorPlots] = initializeGraphics();
setappdata(hFig, 'GraphicsActive', true);
%% 3. 场景渲染循环 - 增强健壮性
frameCount = 0;
while advance(scenario) && frameCount < totalFrames
frameCount = frameCount + 1;
% 获取车辆位姿
position = egoVehicle.Position(1:2);
yaw = egoVehicle.Yaw;
vehiclePoses(frameCount, :) = [position, yaw];
% ==== 修复后的图形处理代码 ====
% 检查图形系统是否激活
if ishghandle(hFig) && isvalid(hFig) && getappdata(hFig, 'GraphicsActive')
% 安全更新图形
try
updateRoadGraphics(scenario, hAx);
actorPlots = updateVehicleGraphics(vehicles, actorPlots, hAx);
catch ME
warning('图形更新错误:',' %s', ME.message);
[hFig, hAx, actorPlots] = initializeGraphics();
setappdata(hFig, 'GraphicsActive', true);
end
% 动态调整视图范围
if ishghandle(hAx) && isvalid(hAx)
set(hAx, 'XLim', [position(1)-20, position(1)+50], ...
'YLim', [position(2)-10, position(2)+10]);
end
% 安全帧捕获，获取图像数据
if mod(frameCount, 5) == 0
try
cameraFrames{frameCount} = safeCaptureFrame(hFig);
if ~isempty(cameraFrames{frameCount})
showFrameInSeparateWindow(cameraFrames{frameCount}, frameCount, totalFrames, position, yaw);
end
catch ME
warning('帧捕获失败:',' %s', ME.message);
cameraFrames{frameCount} = [];
end
else
cameraFrames{frameCount} = [];
end
else
cameraFrames{frameCount} = [];
end
% ==== 修复结束 ====
end
% 资源清理
if ishandle(hFig)
close(hFig);
end
fprintf('数据采集完成: 共 %d 帧\n', frameCount);
%% 4. 相机配置与鸟瞰图生成
% 相机内参设置
focalLength = [1000, 1000]; % 焦距 [fx, fy]
principalPoint = [320, 240]; % 主点 [cx, cy]
imageSize = [480, 640]; % 图像尺寸 [高度, 宽度]
% 相机安装参数
height = 1.5; % 离地高度 (米)
pitch = 10; % 俯仰角 (度)
% 创建相机内参对象
camIntrinsics = cameraIntrinsics(focalLength, principalPoint, imageSize);
% 创建 monoCamera 对象
sensor = monoCamera(camIntrinsics, height, 'Pitch', pitch);
% 定义鸟瞰图输出范围 [xmin, xmax, ymin, ymax]
outView = [-5, 50, -5, 5]; % X: -5到50米, Y: -5到5米
% 定义鸟瞰图输出尺寸 [高度, 宽度]
outImageSize = [600, 800]; % 高度600像素, 宽度800像素
% 创建 birdsEyeView 对象
birdsEyeConfig = birdsEyeView(sensor, outView, outImageSize);
% 车道线检测参数
laneDetectionParams = struct(...
'Sensitivity', 0.85, ...
'LaneWidthRange', [2.5 4.5], ...
'MaxNumLanes', 3, ...
'ROI', [1 400; 1 720; 1280 720; 1280 400]);
% 初始化鸟瞰图显示
figure('Name', '车道线鸟瞰图', 'Position', [100 100 1200 600])
hAxes = axes;
hold(hAxes, 'on');
axis(hAxes, 'equal');
grid(hAxes, 'on');
xlabel('X (m)'); ylabel('Y (m)');
title('实时车道线检测与车辆轨迹');
% 处理每一帧图像
for frameIdx = 1:min(100, frameCount)
% 获取当前帧
I = cameraFrames{frameIdx};
if isempty(I)
continue;
end
% 车道线检测
[laneBoundaries, valid] = detectLanes(I, laneDetectionParams);
if valid
% 转换为鸟瞰图
birdsEyeImage = transformImage(birdsEyeConfig, I);
% 显示鸟瞰图
imshow(birdsEyeImage, 'Parent', hAxes);
title(hAxes, sprintf('帧 %d/%d | 检测到 %d 条车道线', frameIdx, frameCount, numel(laneBoundaries)));
% 绘制检测到的车道线
colors = ['r', 'g', 'b', 'c'];
for laneIdx = 1:numel(laneBoundaries)
lanePoints = laneBoundaries(laneIdx).Points;
lanePointsVehicle = imageToVehicle(birdsEyeConfig, lanePoints);
plot(hAxes, lanePointsVehicle(:,1), lanePointsVehicle(:,2), ...
[colors(laneIdx) '-'], 'LineWidth', 2);
end
% 绘制车辆位置
vehiclePos = vehiclePoses(frameIdx, 1:2);
yaw = vehiclePoses(frameIdx, 3);
plot(hAxes, vehiclePos(1), vehiclePos(2), 'yo', ...
'MarkerSize', 10, 'MarkerFaceColor', 'y');
% 绘制车辆方向箭头
arrowLength = 3;
endX = vehiclePos(1) + arrowLength * cosd(yaw);
endY = vehiclePos(2) + arrowLength * sind(yaw);
quiver(hAxes, vehiclePos(1), vehiclePos(2), ...
endX-vehiclePos(1), endY-vehiclePos(2), ...
0, 'Color', 'y', 'LineWidth', 2, 'MaxHeadSize', 2);
% 绘制轨迹线
if frameIdx > 1
plot(hAxes, vehiclePoses(1:frameIdx,1), vehiclePoses(1:frameIdx,2), ...
'g-', 'LineWidth', 2);
end
% 添加图例
legendEntries = arrayfun(@(x) sprintf('车道 %d', x), 1:numel(laneBoundaries), 'UniformOutput', false);
legendEntries = [legendEntries, '车辆位置', '行驶方向', '行驶轨迹'];
legend(hAxes, legendEntries, 'Location', 'northeastoutside');
drawnow;
end
end
%% 5. 完整车道线地图构建（修复属性访问问题）
% 使用正确的属性名称 ImageSize
bevSize = birdsEyeConfig.ImageSize; % ✅ 修复属性名称
finalMap = uint8(zeros(bevSize)); % ✅ 使用正确的尺寸
laneMaps = cell(1, 3);
% 初始化车道掩码
for i = 1:3
laneMaps{i} = false(bevSize(1:2)); % ✅ 使用正确的尺寸
end
% 累积所有车道线信息
for frameIdx = 1:frameCount
I = cameraFrames{frameIdx};
if isempty(I)
continue;
end
[laneBoundaries, valid] = detectLanes(I, laneDetectionParams);
if valid
for laneIdx = 1:min(numel(laneBoundaries), 3)
lanePoints = laneBoundaries(laneIdx).Points;
lanePointsVehicle = imageToVehicle(birdsEyeConfig, lanePoints);
lanePixels = vehicleToImage(birdsEyeConfig, lanePointsVehicle);
% 创建车道线掩码
laneMask = false(bevSize(1:2)); % ✅ 使用正确的尺寸
for j = 1:size(lanePixels, 1)
x = round(lanePixels(j, 1));
y = round(lanePixels(j, 2));
if x >= 1 && x <= bevSize(2) && y >= 1 && y <= bevSize(1)
laneMask(y, x) = true;
end
end
% 膨胀处理
se = strel('disk', 3);
laneMask = imdilate(laneMask, se);
laneMaps{laneIdx} = laneMaps{laneIdx} | laneMask;
end
end
end
% 创建彩色地图
colorMap = repmat(finalMap, [1 1 3]);
colors = uint8([255 0 0; 0 255 0; 0 0 255]); % 红、绿、蓝
for laneIdx = 1:3
mask = laneMaps{laneIdx};
for c = 1:3
channel = colorMap(:, :, c);
channel(mask) = colors(laneIdx, c);
colorMap(:, :, c) = channel;
end
end
% 添加车辆轨迹
trajectoryPixels = vehicleToImage(birdsEyeConfig, vehiclePoses(:, 1:2));
trajectoryPixels = round(trajectoryPixels);
for i = 1:size(trajectoryPixels, 1)
x = trajectoryPixels(i, 1);
y = trajectoryPixels(i, 2);
if x >= 1 && x <= bevSize(2) && y >= 1 && y <= bevSize(1)
colorMap(y-2:y+2, x-2:x+2, 1) = 255; % R
colorMap(y-2:y+2, x-2:x+2, 2) = 255; % G
colorMap(y-2:y+2, x-2:x+2, 3) = 0; % B
end
end
% 显示最终鸟瞰地图
figure('Name', '完整车道线鸟瞰图', 'Position', [100 100 1000 800])
imshow(colorMap)
title('车道线鸟瞰图 (红色: 左车道, 绿色: 中车道, 蓝色: 右车道)');
hold on;
text(50, 50, '↑ 行驶方向', 'Color', 'w', 'FontSize', 14, 'FontWeight', 'bold');
%% ================ 关键辅助函数 ================
function [hFig, hAx, actorPlots] = initializeGraphics()
hFig = figure('Position', [100, 100, 1280, 720], 'Name', '场景预览', ...
'CloseRequestFcn', @closeFigureCallback, ...
'Tag', 'DrivingSimFigure', ...
'Visible', 'on');
hAx = axes('Parent', hFig);
axis(hAx, 'equal');
hold(hAx, 'on');
grid(hAx, 'on');
xlabel(hAx, 'X (m)');
ylabel(hAx, 'Y (m)');
title(hAx, '驾驶场景预览');
set(hAx, 'Tag', 'DrivingSimAxes');
actorPlots = gobjects(0);
updateRoadGraphics([], hAx);
function closeFigureCallback(~,~)
setappdata(hFig, 'GraphicsActive', false);
delete(hFig);
end
end
function updateRoadGraphics(scenario, hAx)
roadBorderPlot = findobj(hAx, 'Tag', 'RoadBorderPlot');
if isempty(roadBorderPlot) || ~isgraphics(roadBorderPlot) || ~isvalid(roadBorderPlot)
roadBorderPlot = plot(hAx, NaN, NaN, 'k-', 'LineWidth', 2);
set(roadBorderPlot, 'Tag', 'RoadBorderPlot');
end
if ~isempty(scenario) && isgraphics(hAx) && isvalid(hAx)
roadBorders = roadBoundaries(scenario);
allX = [];
allY = [];
for i = 1:numel(roadBorders)
border = roadBorders{i};
allX = [allX; border(:,1); NaN];
allY = [allY; border(:,2); NaN];
end
if isgraphics(roadBorderPlot) && isvalid(roadBorderPlot)
set(roadBorderPlot, 'XData', allX, 'YData', allY);
else
roadBorderPlot = plot(hAx, allX, allY, 'k-', 'LineWidth', 2);
set(roadBorderPlot, 'Tag', 'RoadBorderPlot');
end
end
end
function actorPlots = updateVehicleGraphics(vehicles, actorPlots, hAx)
if ~isgraphics(hAx) || ~isvalid(hAx)
return;
end
if isempty(actorPlots) || numel(actorPlots) < numel(vehicles)
actorPlots = gobjects(numel(vehicles), 1);
end
for i = 1:numel(vehicles)
if i <= numel(actorPlots) && isgraphics(actorPlots(i)) && isvalid(actorPlots(i))
else
actorPlots(i) = patch(hAx, 'Vertices', [], 'Faces', 1:4, ...
'FaceColor', 'b', 'EdgeColor', 'k', ...
'Tag', sprintf('VehiclePatch%d', i));
end
actor = vehicles(i);
pos = actor.Position(1:2);
yaw = actor.Yaw;
len = actor.Length;
wid = actor.Width;
% 角点定义（保持不变）
corners = [
-len/2, -wid/2;
len/2, -wid/2;
len/2, wid/2;
-len/2, wid/2;
]';
% 创建旋转矩阵
rotMat = [cosd(yaw) -sind(yaw); sind(yaw) cosd(yaw)];
% 正确旋转计算（左乘）
corners = rotMat * corners; % 旋转
% 转置回行向量并平移
corners = corners';
corners(:,1) = corners(:,1) + pos(1);
corners(:,2) = corners(:,2) + pos(2);
% 更新图形
if isgraphics(actorPlots(i)) && isvalid(actorPlots(i))
set(actorPlots(i), 'Vertices', corners);
else
actorPlots(i) = patch(hAx, corners(:,1), corners(:,2), 'b', ...
'Tag', sprintf('VehiclePatch%d', i));
end
end
end
function frame = safeCaptureFrame(hFig)
frame = [];
if ~isgraphics(hFig) || ~isvalid(hFig)
return;
end
try
tmpFile = [tempname '.png'];
print(hFig, '-dpng', '-r0', tmpFile);
frame = imread(tmpFile);
delete(tmpFile);
return;
catch
end
try
oldVer = get(hFig, 'Renderer');
set(hFig, 'Renderer', 'painters');
frame = hardcopy(hFig, '-dzbuffer', '-r0');
set(hFig, 'Renderer', oldVer);
return;
catch
end
warning('安全捕获方法失败，返回空帧');
end
function showFrameInSeparateWindow(frame, frameCount, totalFrames, position, yaw)
persistent frameFig
if isempty(frameFig) || ~isvalid(frameFig)
frameFig = figure('Name', '帧预览', 'NumberTitle', 'off', ...
'Position', [200, 200, 800, 600]);
else
figure(frameFig);
end
imshow(frame);
title(sprintf('帧 %d/%d | X=%.1fm Y=%.1fm Yaw=%.1f°', ...
frameCount, totalFrames, position(1), position(2), yaw));
drawnow limitrate;
end
%% 辅助函数：车道线检测
function [laneBoundaries, valid] = detectLanes(I, params)
hsv = rgb2hsv(I);
saturation = hsv(:, :, 2);
enhanced = imadjust(saturation);
roiMask = poly2mask(params.ROI(:,1), params.ROI(:,2), size(I,1), size(I,2));
enhanced(~roiMask) = 0;
edges = edge(enhanced, 'Canny', [0.1 0.3]);
[H, T, R] = hough(edges, 'Theta', -30:0.5:30);
P = houghpeaks(H, 10, 'Threshold', 0.2*max(H(:)));
lines = houghlines(edges, T, R, P, 'FillGap', 100, 'MinLength', 50);
validLanes = struct('Points', {}, 'Strength', {});
for k = 1:length(lines)
xy = [lines(k).point1; lines(k).point2];
lineLength = norm(xy(1,:) - xy(2,:));
angle = atan2d(xy(2,2)-xy(1,2), xy(2,1)-xy(1,1));
if abs(angle) < 10 || abs(angle) > 170
continue;
end
lane.Points = interpolateLine(xy(1,:), xy(2,:), 50);
lane.Strength = lineLength;
validLanes(end+1) = lane;
end
if numel(validLanes) < 2
laneBoundaries = [];
valid = false;
return;
end
yMeans = arrayfun(@(x) mean(x.Points(:,2)), validLanes);
[~, sortedIdx] = sort(yMeans, 'descend');
validLanes = validLanes(sortedIdx);
laneBoundaries = validLanes(1:min(numel(validLanes), params.MaxNumLanes));
valid = true;
end
%% 辅助函数：线段插值
function points = interpolateLine(p1, p2, numPoints)
x = linspace(p1(1), p2(1), numPoints);
y = linspace(p1(2), p2(2), numPoints);
points = [x' y'];
end
扫一扫
3万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
