LaserMeasurement-Klasse zur Laserbasierten Abstandsmessung hinzugefügt

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 <iostream>
+
+// OPENCV-Klassen
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+// Enthält die pow Funktion für Quadrate
+#include <math.h>
+
+
+/* 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<int> 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<cv::Vec3b>(y,x)[0];
+			int g = frame.at<cv::Vec3b>(y,x)[1];
+			int r = frame.at<cv::Vec3b>(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<int> 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);
+}

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 <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+// Vector-Klasse
+#include <vector>
+
+
+/* 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<int> bar, int avgStart, int avgStop);
+
+	public:
+		// Konstruktor
+		LaserMeasurement();
+
+		// Ermittelt die Entfernung in einem Image
+		int GetDistanceFromImage(cv::Mat frame);
+	};
+}

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)