diff --git a/LaserMeasurement.cpp b/LaserMeasurement.cpp new file mode 100644 index 0000000..6092c2e --- /dev/null +++ b/LaserMeasurement.cpp @@ -0,0 +1,149 @@ +/* +-- LaserMeasurement-KLASSE :: IMPLEMENTIERUNG -- +*/ + + +/* INCLUDES */ + +// Klassenheader +#include "LaserMeasurement.h" +using namespace THOMAS; + +// THOMAS-Exception Klasse +#include "THOMASException.h" + +// IO-Stream +#include + +// OPENCV-Klassen +#include +#include +#include + +// Enthält die pow Funktion für Quadrate +#include + + +/* FUNKTIONEN */ + +LaserMeasurement::LaserMeasurement() +{ + _videoCapture = cv::VideoCapture(CAMERA); + + // Kamera Größenparameter setzten + _videoCapture.set(CV_CAP_PROP_FRAME_WIDTH, CAMERA_WIDTH); + _videoCapture.set(CV_CAP_PROP_FRAME_HEIGHT, CAMERA_HEIGHT); + _videoCapture.set(CV_CAP_PROP_FPS, CAMERA_MAX_FPS); + + cv::Mat frame; + + while(true) + { + _videoCapture >> frame; + GetDistanceFromImage(frame); + } +} + +int LaserMeasurement::GetDistanceFromImage(cv::Mat frame) +{ + // Laserposition abrufen und auf Mittelpunkt beziehen + float laserPosition = CAMERA_WIDTH / 2 - GetLaserPosition(frame, BAR_START, BAR_START + BAR_HEIGHT); + + // Distanz berechnen + float distance = (-LASER_DISTANCE / tan(ALPHA)) / (2 * laserPosition / CAMERA_WIDTH * tan(GAMMA / 2) / tan(ALPHA) - 1); + + // Distanz zurückgeben + return distance; +} + +int LaserMeasurement::GetLaserPosition(cv::Mat frame, int startY, int endY) +{ + // Startpunkt des Suchlaufes + int sectionStart = -1; + + std::vector bar; + + // X-Achte durchlaufen + for(int x = 0; x < frame.cols; x++) + { + // Variable für hellsten Wert + int topBrightness = 0; + + // Spalte durchlaufen + for(int y = startY; y < endY; y++) + { + // BGR Werte abrufen + int b = frame.at(y,x)[0]; + int g = frame.at(y,x)[1]; + int r = frame.at(y,x)[2]; + + // Helligkeit berechnen + int brightness = b + g + r; + + // Helligkeitswerte vergleichen + if(brightness > topBrightness) + { + // Helligkeitswert übernehmen + topBrightness = brightness; + } + } + + // Die größte Helligkeit speichern + bar.push_back(topBrightness); + + // Ist die Helligkeit größer als der Mindestwert + if(topBrightness > MIN_BRIGHTNESS) + { + // Prüfe: + // 1. Es hat noch kein Abschnitt begonnen + // 2. Durchschnittliche Helligkeit vor dem Abschnitt weist einen großen Kontrast auf + if(sectionStart == -1 && GetBrightnessAvg(bar, x - (AVG_CHECK_WIDTH + AVG_CHECK_OFFSET), x - AVG_CHECK_OFFSET) < topBrightness - AVG_CHECK_BRIGHTNESS_OFFSET) + { + // Startpunkt setzen + sectionStart = x; + } + } + else + { + // Prüfe: + // 1. Es hat bereits ein Abschnitt begonnen + // 2. Die Laserbreite entspricht den Mindest- und Maximalwerten + // 3. Die durchschnittliche Helligkeit nach dem Abschnitt weist einen großen Kontrast auf + if(sectionStart != -1 && x - sectionStart > LASER_MIN_WIDTH && x - sectionStart < LASER_MAX_WIDTH && GetBrightnessAvg(bar, x + AVG_CHECK_OFFSET, x + (AVG_CHECK_WIDTH + AVG_CHECK_OFFSET)) < topBrightness - AVG_CHECK_BRIGHTNESS_OFFSET) + { + // Mittelpunkt des Laserpunktes zurückgeben + return sectionStart + (x - sectionStart) / 2; + } + + // Startpunkt zurücksetzen + sectionStart = -1; + + } + } + + // Keinen Laserpunkt gefunden + return -1; +} + +int LaserMeasurement::GetBrightnessAvg(std::vector bar, int avgStart, int avgStop) +{ + // Division durch 0 verhindern + if(avgStart < 0 || avgStop - avgStart <= 0 || avgStop >= bar.size()) + { + // 0 zurückgeben + return 0; + } + + // Summe der Brightness + int sum = 0; + + // Angegebenen Bereich durchlaufen + for(int x = avgStart; x < avgStop; x++) + { + // Werte summieren + sum += bar.at(x); + } + + // Durchschnitt berechnen und zurückgeben + return sum / (avgStop - avgStart); +} diff --git a/LaserMeasurement.h b/LaserMeasurement.h new file mode 100644 index 0000000..38f00de --- /dev/null +++ b/LaserMeasurement.h @@ -0,0 +1,88 @@ +#pragma once +/* +-- LaserMeasurement-Klasse :: HEADER -- +Definiert die LaserMeasurement +Diese Klasse misst die Entfernung zum Laserpunkt +*/ + + +/* INCLUDES */ + +// OpenCV-Klassen +#include +#include +#include + +// Vector-Klasse +#include + + +/* KONSTANTEN */ + +// Verwendete Kamera +#define CAMERA 1 + +// Kamera Einstellungen +#define CAMERA_WIDTH 1280 +#define CAMERA_HEIGHT 720 +#define CAMERA_MAX_FPS 30 + +// Höhe des Erkennungsbalkens +#define BAR_HEIGHT 60 + +// Startpunkt (Y) des Erkennungsbalkens +#define BAR_START 330 + +// Mindest- und Maximalgröße des Laserpunktes +#define LASER_MIN_WIDTH 1 +#define LASER_MAX_WIDTH 80 + +// Mindesthelligkeit des Punktes +#define MIN_BRIGHTNESS 740 + +// Beginn des Kontrastüberprüfungsmittelwertstreifens +#define AVG_CHECK_OFFSET 2 + +// Breite von dem s.o. +#define AVG_CHECK_WIDTH 20 + +// Mindestkontrast +#define AVG_CHECK_BRIGHTNESS_OFFSET 50 + +// PI +#define PI 3.1415926535 + +// Abstand zwischen Laser und Kamera +#define LASER_DISTANCE 11.5 + +// Laserwinkel (siehe Zeichnung) +#define ALPHA 24 * PI / 180 + +// Kamerawinkel (siehe Zeichnung) +#define GAMMA 50.1 * PI / 180 + + +/* KLASSE */ + +namespace THOMAS +{ + class LaserMeasurement + { + private: + // VideoCapture Device + cv::VideoCapture _videoCapture; + + // Gibt die Laserpositon zurück + int GetLaserPosition(cv::Mat frame, int startY, int endY); + + // Ermittelt die Helligkeit für einen bestimmten Abschnitt + int GetBrightnessAvg(std::vector bar, int avgStart, int avgStop); + + public: + // Konstruktor + LaserMeasurement(); + + // Ermittelt die Entfernung in einem Image + int GetDistanceFromImage(cv::Mat frame); + }; +} diff --git a/Makefile b/Makefile index 1433e06..7ff5abc 100644 --- a/Makefile +++ b/Makefile @@ -2,8 +2,8 @@ CPPARGS= -std=c++11 -pthread OPENCVARGS= `pkg-config --cflags opencv` LINKERARGS= -pthread -g -O0 -v -da -Q `pkg-config --libs opencv` -all: main.o RS232.o ArduinoCom.o ArduinoProtocol.o TCPServer.o MotorControl.o THOMASException.o TelemetryReceiver.o UDPClient.o StatusInformation.o CollisionDetection.o RelayProtocol.o - g++ $(OPENCVARGS) THOMASException.o RS232.o ArduinoCom.o ArduinoProtocol.o TCPServer.o MotorControl.o TelemetryReceiver.o UDPClient.o StatusInformation.o main.o CollisionDetection.o RelayProtocol.o -o thomas $(LINKERARGS) +all: main.o RS232.o ArduinoCom.o ArduinoProtocol.o TCPServer.o MotorControl.o THOMASException.o LaserMeasurement.o TelemetryReceiver.o UDPClient.o StatusInformation.o CollisionDetection.o RelayProtocol.o + g++ $(OPENCVARGS) THOMASException.o RS232.o ArduinoCom.o ArduinoProtocol.o TCPServer.o MotorControl.o LaserMeasurement.o TelemetryReceiver.o UDPClient.o StatusInformation.o main.o CollisionDetection.o RelayProtocol.o -o thomas $(LINKERARGS) main.o: main.cpp g++ -c main.cpp $(CPPARGS) @@ -35,6 +35,9 @@ MotorControl.o: MotorControl.cpp MotorControl.h THOMASException.o: THOMASException.cpp THOMASException.h g++ -c THOMASException.cpp $(CPPARGS) +LaserMeasurement.o: LaserMeasurement.cpp LaserMeasurement.h + g++ -c LaserMeasurement.cpp $(CPPARGS) + TelemetryReceiver.o: TelemetryReceiver.cpp TelemetryReceiver.h g++ -c TelemetryReceiver.cpp $(CPPARGS)