Ermittlung Spurpolynom

Berechnung der Punkte für die Fahrbahnmarkierung

function [punkteL, punkteM, punkteR] = punkteFinden2 (frame)
% PUNKTEFINDEN2 ermittelt zu einem Bild mit einer Fahrban jeweils 5 Punkte zu
% den einzelnen Fahrspuren
%
% Syntax:
% [punkteL, punkteM, punkteR] = punkteFinden2(frame)
%
% Beschreibung: Zu dem gegebenen Bild werden von rechts aus die Flanken von
% schwarz auf weiss bestimmt. So werden zu jeder der 3 Fahrspurmarkierungen 
% 5 Punkte zurückgegeben.  
% 
%
% Eingangswerte:
% frame: Bild mit 3 Fahrbahnmarkierungen
%
% Rueckgabewerte:
% punkteL:  5 Punkte der linken Fahrbahnmarkierung in dem Format [x1, y1;x2 y2; ...]
% punkteM:  5 Punkte der mittleren Fahrbahnmarkierung in dem Format [x1, y1;x2 y2; ...] 
% punkteR:  5 Punkte der rechten Fahrbahnmarkierung in dem Format [x1, y1;x2 y2; ...]
%
% Beispiel:
% [punkteL, punkteM, punkteR] = punkteFinden2 (bild1)
%************************************************************\
%
% Modul : punkteFinden2.m
%
% Datum : 17. Juni 2021
%
% Entwicklungsumg : MATLAB R2020a
%
% Toolbox : -
%
% Author :  SDE Team 2021/2022
%
% Bemerkung : 
%
% Letzte ?Anderung : 17. Juni 2021
%
% ***********************************************************/

%% Initalisierung der Variablen bei ersten Durchlauf
persistent zweiterP % zweiter Punkt des vorherigen Bildes als ausgangspunkt für neues Bild 
persistent first
if isempty(first)
  first = false;
   zweiterP = 440;
  
end

% Bestimmung der Startwerte
maxXr = zweiterP+40;
minXr =  maxXr -100;

pixelAlt = 0;
pixelNeu = 0;

xyl = zeros(1,2);   % Speicherplatz für Punkte der linken Spur
xym= zeros(1,2);    % Speicherplatz für Punkte der mittlere Spur
xyr= zeros(1,2);    % Speicherplatz für Punkte der rechte Spur
counter = [1 , 1 , 1]; % Counter für die Speicherplaetze


for i=532:-1:200        
    %% rechte Spur finden
    saveR = true;
    
    % Flanke von schwarz auf Weiss erkennen
    while pixelAlt ~= 0 || pixelNeu ~=1
        % von rechts nach links durchsuchen
        maxXr = maxXr-1;        
        % Nur wenn wirklich ein Punkt gefunden wurde auch speichern
        if maxXr < minXr || minXr<1
            saveR = false;
            break;
        end
        
        pixelAlt = pixelNeu;
        pixelNeu = frame(i,maxXr);
    end

    pixelAlt = 0;
    pixelNeu = 0;
    
    % Speichern des gefundenen Punkts
    if saveR == true
        xyr(counter(1),:) = [maxXr,i];
        if counter(1) == 2
            zweiterP = maxXr;
        end
        counter(1) = counter(1)+1;    
    else 
        % Falls kein Punkt gefunden wurde, Startwert zuruecksetzen
        maxXr = 400;
    end
        
        
    %% mittlere Spur finden
    %Bereich zum Suchen festlegen
    maxXm = maxXr-20;
    minXm = maxXm-80;
    
    saveM = true;
    % Flanke von schwarz auf Weiss erkennen
    while pixelAlt ~= 0 || pixelNeu ~=1
        % von rechts nach links durchsuchen
        maxXm = maxXm-1;
        % Nur wenn wirklich ein Punkt gefunden wurde auch speichern
        if maxXm < minXm || minXm<1
            saveM = false;
            break;    
        end
        pixelAlt = pixelNeu;
        pixelNeu = frame(i,maxXm);
    end
    pixelAlt = 0;
    pixelNeu = 0;
    % Speichern des gefundenen Punkts
    if saveM == true
     xym(counter(2),:) = [maxXm,i];
     counter(2) = counter(2) +1; 
    end 
         
    %% linke Spur finden
    %Bereich zum Suchen festlegen
    maxXl = maxXm-20;
    minXl = maxXl-80;
    
    saveL = true;
    % Flanke von schwarz auf Weiss erkennen
    while pixelAlt ~= 0 || pixelNeu ~=1
        % von rechts nach links durchsuchen
        maxXl = maxXl-1;
        % Nur wenn wirklich ein Punkt gefunden wurde auch speichern
        if maxXl <minXl || minXl<1
            saveL = false;
            break;    
        end
        
        pixelAlt = pixelNeu;
        pixelNeu = frame(i,maxXl);
    end
    pixelAlt = 0;
    pixelNeu = 0;
    % Speichern des gefundenen Punkts
    if saveL == true
     xyl(counter(3),:) = [maxXl,i];
     counter(3) = counter(3) +1; 
    end 
    
    % Fuer neue Bildzeile Startwerte festlegen
    maxXr = maxXr+30;
    minXr = maxXr-100;
 
end

%% Punkte fuer einzelne Fahrbahnen festlegen
% 5 Punkte im gleichmaeßigem Abstand auswählen, um Datenmenge zu reduzieren
sizeX = [size(xyl,1), size(xym,1),size(xyr,1)];
valuesl = round(linspace(1,sizeX(1),5));
valuesm = round(linspace(1,sizeX(2),5));
valuesr = round(linspace(1,sizeX(3),5));

% Ausgabewerte festlegen
punkteL = [xyl(valuesl,1), xyl(valuesl,2)];
punkteM = [xym(valuesm,1), xym(valuesm,2)];
punkteR = [xyr(valuesr,1), xyr(valuesr,2)];

end

Darstellung Spurpolynom

% PUNKTEANZEIGEN darstellung der durch punkteFinden Ermittelten Punkte der
% Fahrspur
% ***********************************************************\
%
% Modul : punkteAnzeigen.m
%
% Datum : 17. Juni 2021
%
% Implementierung : MATLAB R2020a
%
% Toolbox : Image Processing Toolbox
%
% Autor :  SDE Team 2021/2022
%
% Bemerkung : Code-Review noch ausstehend
%
% Letzte ?Änderung : 17. Juni 2021
%
%************************************************************/

%% MALTAB initisalisieren
clear all; close all; clc

%% Video laden
% Bildpfad temporär hinzufügen
bildPfad = '[...]\MTR_SDE_Praktikum\trunk\Software\Simulation_Bildverarbeitung_und_Spurerkennung\Videos';
addpath(bildPfad);

% Videowriter und -reader oeffnen
video = VideoReader("Vogelperspektive.mpeg");
wVideo = VideoWriter("Spurerkennung"); 
open(wVideo);

figure

%% Auswertung der einzelnen Frames
while hasFrame(video)
    
    % Bild extrahieren und filtern
    frame = readFrame(video);
    frame = rgb2gray(frame);
    binaerBild=im2bw(frame);
    KantenBild=edge(binaerBild, 'Sobel');
    
    % Ermitteln der Punkte der Polynome
    [punkteL, punkteM, punkteR] = punkteFinden2(KantenBild);
    
    % Polynome ermitteln
    ypL = polyfit(punkteL(:,2),punkteL(:,1),2);
    ypM = polyfit(punkteM(:,2),punkteM(:,1),2);
    ypR = polyfit(punkteR(:,2),punkteR(:,1),2);
    
    % Darstellung
    imshow(KantenBild);
    hold on
    xNeu = 200:532;    
    plot(polyval(ypL,xNeu),xNeu,'r-',"lineWidth", 5);
    plot(polyval(ypM,xNeu),xNeu,'b-',"lineWidth", 5);
    plot(polyval(ypR,xNeu),xNeu,'g-',"lineWidth", 5);
    
    % Darstelung ohne Polynome mit verbundenen Punkten
%     plot (punkteL(:,1),punkteL(:,2), "ro-", "lineWidth", 5);
%     plot (punkteM(:,1),punkteM(:,2), "bo-", "lineWidth", 5);
%     plot (punkteR(:,1),punkteR(:,2), "go-", "lineWidth", 5);
    hold off
    
    % Speichern des Videoframes
    vidFrame = getframe(gcf);
    writeVideo(wVideo,vidFrame)
end

% Beenden des Videoschreibens
close (wVideo);
disp("File closed");