detect_edges_routine.m

%% Routine to run the edge detection 4 times and filter+group the results
function all_edges = detect_edges_routine(map)
    
    scan_angle = 90;
    identified_edges = [];
    all_edges = [];
    
    % scan 4 times, rotate 90° each time
    disp('Starting scan.')
    for angle = 0:scan_angle:359
        points = get_tf_scan();
        [edges, alpha] = get_edges(points);
        
        % Rotate coordinates to alpha=0 robot-coordinate-system
        for i = 1:size(edges, 1) 
            x = edges(i,1);
            y = edges(i,2);
            edges(i, 1) = x * cos(deg2rad(angle)) - y * sin(deg2rad(angle));
            edges(i, 2) = x * sin(deg2rad(angle)) + y * cos(deg2rad(angle));
        end
        
        % Add the edges, identified from this angle
        identified_edges = [identified_edges; edges];
        
        rotate(scan_angle);
        disp("Scan for edges finished.")
    end
    
    
    %% Group the identifies edges
    % grouping tolerance (all points within this distance are grouped together)
    tolerance = 0.4;

    % Seperate grouping of x and y edges
    x_values = identified_edges(:, 1);
    y_values = identified_edges(:, 2);
    x_groups = group_points(x_values, tolerance);
    y_groups = group_points(y_values, tolerance);
    disp('Groups for x:');
    disp(x_groups);
    disp('Groups for y:');
    disp(y_groups);
 
    % Set edges from grouped points
    all_edges = [
        x_groups(1), y_groups(1);
        x_groups(2), y_groups(1);
        x_groups(1), y_groups(2);
        x_groups(2), y_groups(2)
    ];
        
    disp("Edge detection successfull.")
end