以下代码出现了以下错误:
%% 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 0 0]),...
vehicle(scenario, 'ClassID', 1, 'Position', [60 -3 0])...
];
egoVehicle = vehicles(1);
%% 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 isgraphics(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 isgraphics(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 % 每5帧捕获一次
try
cameraFrames{frameCount} = safeCaptureFrame(hFig);
% 在独立窗口显示捕获的帧
if ~isempty(cameraFrames{frameCount})
showFrameInSeparateWindow(cameraFrames{frameCount}, frameCount, totalFrames, position, yaw); % 传递totalFrames
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. 车道线检测与鸟瞰图生成
% 配置鸟瞰图参数
birdsEyeConfig = birdsEyeView(...
'VehicleWidth', egoVehicle.Width, ...
'VehicleFront', 2.0, ... % 车辆前部到相机距离
'VehicleRear', 1.0, ... % 车辆后部到相机距离
'OutputImageSize', [600, 800], ... % 鸟瞰图尺寸 [高度, 宽度]
'WorldUnitsPerPixel', 0.1); % 每像素0.1米
% 车道线检测参数
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) % 限制处理前100帧
% 获取当前帧
I = cameraFrames{frameIdx};
% 车道线检测
[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. 完整车道线地图构建
% 创建最终鸟瞰地图
finalMap = uint8(zeros(birdsEyeConfig.OutputImageSize));
laneMaps = cell(1, 3); % 存储各车道地图
for i = 1:3
laneMaps{i} = false(birdsEyeConfig.OutputImageSize(1:2));
end
% 累积所有车道线信息
for frameIdx = 1:frameCount
I = cameraFrames{frameIdx};
[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(size(finalMap, 1), size(finalMap, 2));
for j = 1:size(lanePixels, 1)
x = round(lanePixels(j, 1));
y = round(lanePixels(j, 2));
if x >= 1 && x <= size(finalMap, 2) && y >= 1 && y <= size(finalMap, 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]); % 转换为RGB
% 为不同车道上色
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 <= size(colorMap, 2) && y >= 1 && y <= size(colorMap, 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)
% 确保hAx有效
if ~isgraphics(hAx) || ~isvalid(hAx)
return;
end
% 确保actorPlots是有效图形对象数组
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 = corners * rotMat;
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
% 方法1:使用print函数捕获(最稳定)
try
tmpFile = [tempname '.png'];
print(hFig, '-dpng', '-r0', tmpFile);
frame = imread(tmpFile);
delete(tmpFile);
return;
catch
end
% 方法2:使用硬拷贝作为备选
try
oldVer = get(hFig, 'Renderer');
set(hFig, 'Renderer', 'painters');
frame = hardcopy(hFig, '-dzbuffer', '-r0');
set(hFig, 'Renderer', oldVer);
return;
catch
end
% 方法3:避免使用getframe(问题根源)
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空间处理不同光照
hsv = rgb2hsv(I);
saturation = hsv(:, :, 2);
enhanced = imadjust(saturation);
% 应用ROI掩码
roiMask = poly2mask(params.ROI(:,1), params.ROI(:,2), size(I,1), size(I,2));
enhanced(~roiMask) = 0;
% 使用Canny边缘检测
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
% 按Y坐标排序 (从下到上)
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
test3
数据采集完成: 共 298 帧
错误使用 birdsEyeView
输入参数太多。
出错 test3 (第 81 行)
最新发布