VR地图最短路径分段边界框绘制异常的修复方案咨询

需求说明 #

在虚拟现实（VR）地图的最短路径中，我们需要按转弯分段主路径，以此根据用户导航轨迹（图中蓝线）在最短路径上的行走距离进行打分。

当前问题 #

当前算法可正常绘制第一段路径的边界框，但后续路径段的边界框尺寸与第一段完全相同，且紧邻第一段的x、y线段位置（对应idealPath变量）。

诉求 #

如何修复现有代码，实现正确检测后续路径段并绘制对应的橙色分段边界框？其中idealPath为94666×2的矩阵，第一列为x坐标，第二列为y坐标，构成图中绿色的最短路径。

当前代码 #

function plotSegmentedCheckpoints(trajectory_raw, idealPath, checkpointInfo)
    % Clear and hold the plot
    cla;
    hold on;
    
    % Plot ideal path in green
    plot(idealPath(:,1), idealPath(:,2), 'g.', 'MarkerSize', 5, 'DisplayName', 'Ideal Path');
    
    % Plot trajectory in blue
    scatter(trajectory_raw(:,1), trajectory_raw(:,2), 15, 'b', 'filled', 'DisplayName', 'Trajectory');

    % Get path segments
    [segmentIndices, numSegments] = findPathSegments(idealPath);
    
    % Initialize checkpoint storage
    checkpoint_bounds = zeros(numSegments, 4);  % [x1, x2, y1, y2]
    reached = false(numSegments, 1);
    
    % Constants
    BOX_WIDTH = 86;  % Width of checkpoint boxes
    
    % Process each segment
    for i = 1:numSegments
        % Get current segment
        startIdx = segmentIndices(i);
        endIdx = segmentIndices(i+1);
        segmentPoints = idealPath(startIdx:endIdx, :);
        
        % Calculate segment bounds
        [bounds, orientation] = calculateSegmentBounds(segmentPoints, BOX_WIDTH);
        checkpoint_bounds(i,:) = bounds;
        
        % Check if trajectory reaches this checkpoint
        reached(i) = checkCheckpointReached(trajectory_raw, bounds);
        
        % Draw checkpoint box
        drawCheckpointBox(bounds, reached(i), i);
        
        % Draw direction arrow to next checkpoint if not last segment
        if i < numSegments
            drawDirectionArrow(bounds, idealPath(segmentIndices(i+1),:));
        end
    end
    
    % Calculate and display score
    score = calculateScore(reached, numSegments);
    
    % Configure plot
    configureplot();
    
    % Update title with progress
    title(sprintf('Checkpoint Progress: (%d/%d)\nScore: %.2f', ...
          sum(reached), numSegments, score));
    
    % Store checkpoint information
    checkpointInfo.bounds = checkpoint_bounds;
    checkpointInfo.reached = reached;
    checkpointInfo.score = score;
end

% Function to find path segments based on direction changes
function [segmentIndices, numSegments] = findPathSegments(idealPath)
    % Calculate path directions
    vectors = diff(idealPath);
    angles = atan2(vectors(:,2), vectors(:,1));
    
    % Find significant direction changes (> 30 degrees)
    angleChanges = [true; abs(diff(angles)) > pi/6];
    
    % Combine consecutive changes that are very close
    MIN_SEGMENT_LENGTH = 50;
    for i = 2:length(angleChanges)-1
        if angleChanges(i) && angleChanges(i+1) && ...
           (i - find(angleChanges(1:i-1), 1, 'last') < MIN_SEGMENT_LENGTH)
            angleChanges(i) = false;
        end
    end
    
    % Get indices where direction changes significantly
    segmentIndices = find(angleChanges);
    
    % Ensure first and last points are included
    if segmentIndices(1) ~= 1
        segmentIndices = [1; segmentIndices];
    end
    if segmentIndices(end) ~= size(idealPath,1)
        segmentIndices = [segmentIndices; size(idealPath,1)];
    end
    
    % Number of segments
    numSegments = length(segmentIndices) - 1;
end

% Function to calculate bounds for a segment
function [bounds, orientation] = calculateSegmentBounds(segmentPoints, BOX_WIDTH)
    % Determine if segment is more vertical or horizontal
    endpoint_diff = diff(segmentPoints([1 end],:));
    is_vertical = abs(endpoint_diff(2)) > abs(endpoint_diff(1));
    
    if is_vertical
        % Vertical segment
        x_center = mean(segmentPoints(:,1));
        y_range = [min(segmentPoints(:,2)), max(segmentPoints(:,2))];
        
        bounds = [
            x_center ,  % x1
            x_center + BOX_WIDTH,  % x2
            y_range(1),              % y1
            y_range(2)               % y2
        ];
        orientation = 'vertical';
    else
        % Horizontal segment
        y_center = mean(segmentPoints(:,2));
        x_range = [min(segmentPoints(:,1)), max(segmentPoints(:,1))];
        
        bounds = [
            x_range(1),              % x1
            x_range(2),              % x2
            y_center ,  % y1
            y_center + BOX_WIDTH   % y2
        ];
        orientation = 'horizontal';
    end
end

% Function to check if trajectory reaches checkpoint
function reached = checkCheckpointReached(trajectory, bounds)
    in_x = trajectory(:,1) >= bounds(1) && trajectory(:,1) <= bounds(2);
    in_y = trajectory(:,2) >= bounds(3) && trajectory(:,2) <= bounds(4);
    reached = any(in_x && in_y);
end

% Function to draw checkpoint box
function drawCheckpointBox(bounds, is_reached, checkpoint_number)
    x = [bounds(1), bounds(2), bounds(2), bounds(1), bounds(1)];
    y = [bounds(3), bounds(3), bounds(4), bounds(4), bounds(3)];
    
    if is_reached
        fill(x(1:4), y(1:4), [0.8 1 0.8], 'FaceAlpha', 0.3, 'EdgeColor', 'k', 'LineWidth', 1);
    else
        fill(x(1:4), y(1:4), [1 0.8 0.8], 'FaceAlpha', 0.3, 'EdgeColor', 'k', 'LineWidth', 1);
    end
    
    % Add checkpoint number
    text(mean([bounds(1), bounds(2)]), mean([bounds(3), bounds(4)]), ...
         num2str(checkpoint_number), 'HorizontalAlignment', 'center', ...
         'VerticalAlignment', 'middle', 'FontSize', 12, ...
         'FontWeight', 'bold', 'Color', 'k');
end

% Function to draw direction arrow
function drawDirectionArrow(current_bounds, next_point)
    % Calculate arrow start point (center of current box)
    arrow_start = [
        mean([current_bounds(1), current_bounds(2)]), 
        mean([current_bounds(3), current_bounds(4)])
    ];
    
    % Calculate direction to next checkpoint
    dir_vec = next_point - arrow_start;
    dir_vec = dir_vec / norm(dir_vec) * 50;  % Fixed length arrows
    
    % Draw arrow
    quiver(arrow_start(1), arrow_start(2), dir_vec(1), dir_vec(2), 0, ...
           'Color', [1 0.5 0], 'LineWidth', 2, 'MaxHeadSize', 0.5);
end

% Function to calculate score
function score = calculateScore(reached, numSegments)
    weights = linspace(1, 2, numSegments);
    score = sum(reached .* weights') / sum(weights);
end

% Function to configure plot
function configureplot()
    grid on;
    axis equal;
    set(gca, 'YDir', 'reverse');
    xlabel('X Position (cm)');
    ylabel('Y Position (cm)');
    legend('Location', 'northwest');
    xlim([0 600]);
    ylim([0 600]);
end

修复结果 #

采纳建议后，已成功在图中绘制出分段路径。

内容的提问来源于Stack Exchange，提问作者Michael Liang