1 Vorstellung und Grundidee
2 Verschiedene Wege zum Ziel
  2.1 Der Anfangsplan: GPS
  2.2 Erste Planänderung: Funkwellen
  2.3 Der endgültige Plan: Kameras
3 Die Umsetzung
4 Fazit

Einen Quadrocopter Fotos aus der Luft schießen lassen - das war die ursprüngliche Idee, unter der sich die Gruppe „Robotik“ in Mathesis zusammengefunden hat. Die Gruppe Robotik, das sind/waren Maximilian O., Konstantin D. und Robin N.. Natürlich gehörten zu der Idee noch viele andere Dinge: Das Gleichgewicht des Quadrocopters programmieren, feste Punkte bestimmen, die der Quadrocopter selbstständig ansteuert und - natürlich - dabei auch noch Hindernissen ausweicht. Im Endeffekt kam es doch etwas anders.

Zunächst einmal: Die Idee, das Gleichgewicht des Quadrocopters auch zu programmieren, wurde uns von Stefan und Felix relativ früh wieder ausgetrieben: Das Projekt wäre auch so schon schwierig genug, wir sollten das vorprogrammierte Gleichgewicht nehmen, war die Aussage. Es hat sich herausgestellt, dass dies ein sehr guter Rat war, denn auch so hatten wir genug zu tun.

Unsere erste Idee beinhaltete die Benutzung von GPS: Dem Quadrocopter sollten feste Koordinaten mitgegeben werden, die er mit Hilfe von GPS anfliegt. Dabei sollte eine Kamera an Bord Hindernisse erkennen und der Quadrocopter diese umfliegen. An speziellen Punkten sollten dann Bilder gemacht werden, und schließlich sollte der Quadrocopter wieder zum Start zurückkehren. Das erste Problem an diesen Plan war ein technisches: Quadrocopter sind energietechnisch sehr ungünstige Flugobjekte, die nur sehr wenig Gewicht tragen können. Von daher war es, außer mit einem sehr großen Quadrocopter, faktisch unmöglich, sowohl ein GPS-Modul als auch eine Kamera UND einen Computer auf den Quadrocopter zu tun. Für den großen Quadrocopter, der durchaus in der Uni vorhanden war, ergaben sich aber andere Schwierigkeiten: Zunächst ist dieser Quadrocopter äußerst gefährlich: Seine Rotoren sind durchaus in der Lage, Finger abzuschneiden, wenn sie dem Quadrocopter in die Quere kommen. Außerdem ist er sehr teuer - und eine Fehlfunktion, die ihn zerstört, schlicht nicht leistbar. Das nächste Problem galt für alle Quadrocopter: Für Draußen-Flüge sind Genehmigungen vonnöten. Gerade für den großen Quadrocopter werden solche Genehmigungen aber nur selten erteilt. Auch sind Quadrocopter sehr wasseranfällig und gerade im Winter daher nur schwer nutz- oder testbar. Damit war ein Draußen-Fliegen faktisch unmöglich - und daher auch die Idee mit GPS hinfällig. Eine neue Idee musste her.

Die nächste Idee war es, mit Funkwellen zu arbeiten. Das funktioniert - in einem kleinen Bereich - sowohl drinnen als auch draußen und kann somit auch im Winter mit Quadrocoptern gemacht werden. Zwei Möglichkeiten waren vorhanden: WLAN und Bluetooth. Für Bluetooth entdeckten wir bald ein Programm - BlueProximity - so dass wir Hoffnungen hatten, dass diese Idee funktioniert. Eine Triangulation wäre mit drei Bluetooth-Sendern möglich, ein Einblick liefert dieses kleine Programm zum Plotten:

import numpy as np
import matplotlib.pyplot as plt
import matplotlib.path as mpath
import matplotlib.lines as mlines
import matplotlib.patches as mpatches
 
 
W = { }
N = 5
 
qx = np.random.randint(30)
qy = np.random.randint(30)
W["Referenzpunkt X"] =  qx
W["Referenzpunkt Y"] =  qy
 
plt.figure()
ax=plt.subplot(111)
ax.set_xlim([0,30])
ax.set_ylim([0,30])
ax.add_patch(mpatches.Circle((qx, qy), radius=1,color="red",alpha=0.1))
 
for i in range(0,N):                             #scatterplot handles data nonlinear, its not legible for triangulation-use
 
	x = np.random.randint(30)
	y = np.random.randint(30)
	ankathete = float(x - qx)
	gegenkathete = float(y - qy)
	radius = np.sqrt((ankathete**2) + (gegenkathete**2))
	kreisumfang = 2*np.pi*radius
	area = np.pi * (radius)**2 
	W[i] =  x,y,radius,area
 
	ax.add_patch(mpatches.Circle((x,y),radius=radius,color='blue',alpha=0.1))
	#plt.scatter(x, y, s=area*190 , alpha=0.1)
 
plt.draw()
plt.show()
print W

Bald darauf erkannten wir jedoch, dass die Entfernungsmessung viel zu ungenau funktionierte - und daher nicht in Frage kam. Die Funkwellen waren zu schnell, al dass der Computer sie hätte auswerten können. Also war auch diese Idee gescheitert.

An diesem Punkt erinnerten wir uns an eine Idee, die uns Stefan anfangs vermitteln wollte: Den Quadrocopter mit Hilfe von Kameras im Raum erkennen und dadurch steuern. Also vertieften wir uns in die Theorie dahinter: Wie man mit Hilfe von 2 Kameras aus 2-D Punkten 3-D Punkte zu konstruiert. Dies funktioniert zum Beispiel mit dem Lochkamera-Modell. Dieses baut darauf auf, dass Bildpunkte immer aus Linien im dreidimensionalen Raum besteht (siehe Bild). Im Schnitt zweier solcher Linien liegt also ein Punkt im dreidimensionalen. Wie man diesen Punkt bekommt - das war die Frage, die wir uns ab dann gestellt haben. Quelle: http://pille.iwr.uni-heidelberg.de/~kinect01/img/pinhole-camera.png (23.01.2014; 17:25 Uhr)

Zunächst einmal stellte sich die Frage, welche Punkte von Kameras besonders gut erkannt werden können. Nach ein wenig überlegen und den ersten (fruchtlosen) Versuchen erkannten wir, dass farbige Dioden am besten geeignet sind: Sie senden Licht in einer relativ konstanten Wellenlänge aus, die gute aus Bildern herausgefiltert werden kann. Dafür wandelt man das Bild aus dem ursprünglichen BGR-Farbraum in den HSV-Farbraum um. Der HSV-Farbraum bildet sich aus der Farbe, der Sättigung und der Helligkeit, es ist also leicht, nur Punkte mit bestimmten Werten in diesen drei Bereichen zu betrachten. Hier der Programmcode, der aus diesen Überlegungen sowie viel herumprobieren entstand. Dabei muss beachtet werden, dass für unterschiedliche Dioden und unterschiedliche Kameras unterschiedliche Ergebnisse für diese Werte erreicht werden!

import numpy as np
import cv2
import pylab
from scipy.signal import convolve2d
import threading
import time
 
#Zunächst wird mit der folgenden Definition und dem Code bis zum nächsten Kommentar ein Video hervorgerufen, bei dem jedes Kamerabild 
#den Programmcode durchläuft, angezeigt wird und danach das nächste Bild hochgeladen wird
 
def lies_kamera():
 
	global bild_aktuell
	while not stop_flag:
		print "Bild gelesen"
		res,bild_aktuell=capture.read()			
 
capture= cv2.VideoCapture(1)
stop_flag=False		
kamera_thread=threading.Thread(target=lies_kamera)
kamera_thread.start()
time.sleep(2)
 
while True:	
	bild=bild_aktuell.copy()
	cv2.namedWindow("Fenster 1")
	cv2.namedWindow("Fenster Rot")	
	cv2.namedWindow("Fenster Blau")
	cv2.imshow("Fenster 1",bild)
	cv2.waitKey(50)	
 
	#Nun wird aus dem BGR-Bild ein HSV-Bild geformt. In diesem Bild werden nun die Punkte weiß gefärbt, die zwischen den angegebenen Werten 
        #liegen, alle anderen Punkte werden schwarz gefärbt. Mit filterSpeckles werden kleine weiße Flecken bis zu einer bestimmten, selbst
        #definierten Größe, schwarz gefärbt
 
	bildhsv= cv2.cvtColor(bild,cv2.COLOR_BGR2HSV)
	bild_blau=cv2.inRange(bildhsv,np.array([100,0,200]),np.array( [110,255,255]))    #fur PS-Move
	bild_rot=cv2.inRange(bildhsv,np.array([24,0,200]),np.array( [24,255,255]))     #fur PS-Move
	cv2.filterSpeckles(bild_blau, 00, 1, 1)
 
	#Nun wird das Bild angezeigt
 
	cv2.imshow("Fenster Rot", bild_rot)													
	cv2.imshow("Fenster Blau", bild_blau)
	cv2.waitKey(30)																					
capture.release()

Nach dieser kleinen, noch unvollständigen Arbeit, beschäftigten wir uns mit der Frage, wie man aus einem gegebenen 2-D Punkt einen 3-D Punkt berechnet. Wie schon gesagt, wird dafür das Lochkamera-Modell verwendet. Dieses lässt sich auf Aufgaben aus der Linearen Algebra herunterbrechen. Im Endeffekt wird aus verschiedenen zusammengerechneten Matrizen eine finale Bildmatrix erstellt, die aus zwei 2-D Punkten in zwei Bildern einen 3-D Punkt bildet. Dafür müssen zunächst einmal die Kameras zueinander und zu sich selbst kalibriert werden. Zunächst werden Kameramatrizen K1 und K2 gesucht, die die Brennweite (Diagonale) sowie die Bildmitte (letzte Spalte) der Kamera beinhalten. Hier der Code dazu:

#!/usr/bin/env python
# -*- coding: utf-8 -*-
#
#  unbenannt.py
#  
#  Copyright 2013 Stefan Born <born@math.tu-berlin.de>
#  
#  This program is free software; you can redistribute it and/or modify
#  it under the terms of the GNU General Public License as published by
#  the Free Software Foundation; either version 2 of the License, or
#  (at your option) any later version.
#  
#  This program is distributed in the hope that it will be useful,
#  but WITHOUT ANY WARRANTY; without even the implied warranty of
#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#  GNU General Public License for more details.
#  
#  You should have received a copy of the GNU General Public License
#  along with this program; if not, write to the Free Software
#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
#  MA 02110-1301, USA.
#  
#  
 
 
 
import numpy as np
import cv2
from thread import start_new_thread
from time import sleep
 
KAMERA_NR=1
STOP_FLAG=False
 
def lies_kamera():
	''' Endloschleife um Kamera auslesen;
	das zuletzt gelesene Bild befindet sich in der globalen Variable bild_aktuell'''
	global bild_aktuell
	n=0
	while not STOP_FLAG:
		n+=1
		#print "Frame " ,n
 
	# Ein Bild auslesen
		res,bild=capture.read()
		bild_aktuell=bild.copy()
 
 
 
## Kamera initialisieren
 
capture= cv2.VideoCapture(KAMERA_NR)
capture.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH,640)
capture.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT,480)
sleep(5)
#capture.set(cv2.cv.CV_CAP_PROP_FPS,60.)
capture.set(cv2.cv.CV_CAP_PROP_SATURATION,0.1)
capture.set(cv2.cv.CV_CAP_PROP_BRIGHTNESS,0.0)
#capture.set(cv2.cv.CV_CAP_PROP_EXPOSURE,0)
start_new_thread(lies_kamera,())
sleep(2)
 
cv2.namedWindow("Bild")
 
######  Schleife, die das Schachbrettmuster im Bild sucht,
######  und die gefundenen Punkte in img_points speichert.
######  
 
abbruch=False
 
pattern_size = (9, 6)
pattern_points = np.zeros( (np.prod(pattern_size), 3), np.float32 )
pattern_points[:,:2] = np.indices(pattern_size).T.reshape(-1, 2)*2.54    
# das Kalibrierungsbrett hat 10x 7  1 inch * 1 inch-Felderpyth
 
obj_points = []
img_points = []
h, w = 0, 0
 
 
 
while not abbruch:
 
#durch ein Schachbrett bekannter Größe werden nun die vorher genannten Matrizen berechnet. Dabei werden die Abstände zwischen den Ecken der Quadrate
#gesucht und in Relation zum wahren wert gesetzt
 
	img=bild_aktuell.copy()
	gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
 
	h, w = img.shape[:2]
	found, corners = cv2.findChessboardCorners(img, pattern_size)
	if found:
		term = ( cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1 )
		cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), term)
 
		cv2.drawChessboardCorners(img, pattern_size, corners, found)
 
 
		print 'ok'
		cv2.imshow('Bild',img)
		key=cv2.waitKey(0)
		if not(key==ord('u')):
			img_points.append(corners.reshape(-1, 2))
			obj_points.append(pattern_points)	
 
		if key==ord('q'):
			abbruch=True
 
	cv2.imshow('Bild',img)	
	key=cv2.waitKey(20)
	if key==ord('q'): 
	   abbruch=True
 
camera_matrix = np.zeros((3, 3))
dist_coefs = np.zeros(4)
img_n = len(img_points)
rvecs = np.array([np.zeros(3) for i in xrange(img_n)])
tvecs = np.array([np.zeros(3) for i in xrange(img_n)])
rms, camera_matrix,dist_coefs,rvecs,tvecs = cv2.calibrateCamera(obj_points, img_points, (w, h), camera_matrix, dist_coefs , rvecs, tvecs)
print rms
print "Kamera Matrix ", camera_matrix
print "Distortion Coefficients ", dist_coefs
print "Rotation ", rvecs
print "Translation ", tvecs
cv2.destroyAllWindows()
STOP_FLAG=True

Nun müssen die Kameras in Relation zueinander gesetzt werden. Dafür muss man wissen, dass jede Kamera ein eigenes Koordinatensystem aufbaut, mit sich selbst im Ursprung. Um nun aber aus zwei Bildern einen Punkt zu bestimmen, müssen die Koordinatensysteme aneinander angeglichen werden. Dafür werden die Translationsmatrix T, die den Abstand der beiden Koordinatenursprünge ausgleicht, und die Rotationsmatrix R, die die Drehung der Koordinatensysteme zueinander ausgleicht, gesucht. Hier der zugehörige Code:

import numpy as np
import cv2
import pickle
from thread import start_new_thread
from time import sleep
 
KAMERA1_NR=1
KAMERA2_NR=2
STOP_FLAG=False
 
def lies_kamera():
	''' Endloschleife um Kamera auslesen;
	das zuletzt gelesene Bild befindet sich in der globalen Variable bild_aktuell'''
	global bild1_aktuell
	global bild2_aktuell
	n=0
	while not STOP_FLAG:
		n+=1
		#print "Frame " ,n
 
	# Ein Bild auslesen
		res,bild=capture1.read()
		bild1_aktuell=bild.copy()
		res,bild=capture2.read()
		bild2_aktuell=bild.copy()
 
 
 
## Kamera initialisieren
 
capture1= cv2.VideoCapture(KAMERA1_NR)
capture1.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH,640)
capture1.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT,480)
sleep(2)
#capture1.set(cv2.cv.CV_CAP_PROP_FPS,60.)
capture1.set(cv2.cv.CV_CAP_PROP_CONTRAST,0.1)
capture1.set(cv2.cv.CV_CAP_PROP_SATURATION,0.1)
capture1.set(cv2.cv.CV_CAP_PROP_BRIGHTNESS,0.0)
capture1.set(cv2.cv.CV_CAP_PROP_GAIN,0.1)
 
capture2= cv2.VideoCapture(KAMERA2_NR)
capture2.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH,640)
capture2.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT,480)
sleep(2)
#capture2.set(cv2.cv.CV_CAP_PROP_FPS,60.)
capture2.set(cv2.cv.CV_CAP_PROP_CONTRAST,0.1)
capture2.set(cv2.cv.CV_CAP_PROP_SATURATION,0.1)
capture2.set(cv2.cv.CV_CAP_PROP_BRIGHTNESS,0.0)
capture2.set(cv2.cv.CV_CAP_PROP_GAIN,0.1)
#capture.set(cv2.cv.CV_CAP_PROP_EXPOSURE,0)
 
 
start_new_thread(lies_kamera,())
sleep(4)
 
cv2.namedWindow("Bild1")
cv2.namedWindow("Bild2")
 
######  Schleife, die das Schachbrettmuster im Bild sucht,
######  und die gefundenen Punkte in img_points speichert.
######  
 
abbruch=False
 
pattern_size = (9, 6)
pattern_points = np.zeros( (np.prod(pattern_size), 3), np.float32 )
pattern_points[:,:2] = np.indices(pattern_size).T.reshape(-1, 2)*2.54    
# das Kalibrierungsbrett hat 10x 7  1 inch * 1 inch-Felderpyth
 
obj_points1 = []
img_points1 = []
obj_points2 = []
img_points2 = []
 
h, w = 0, 0
 
 
 
while not abbruch:
 
	img1=bild1_aktuell.copy()
	gray1 = cv2.cvtColor(img1,cv2.COLOR_BGR2GRAY)
	img2=bild2_aktuell.copy()
	gray2 = cv2.cvtColor(img2,cv2.COLOR_BGR2GRAY)
 
 
	h, w = img1.shape[:2]
	found1, corners1 = cv2.findChessboardCorners(img1, pattern_size)
	found2, corners2 = cv2.findChessboardCorners(img2, pattern_size)
 
	if found1 and found2:
		term = ( cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1 )
		cv2.cornerSubPix(gray1, corners1, (11, 11), (-1, -1), term)
		cv2.cornerSubPix(gray2, corners2, (11, 11), (-1, -1), term)
 
		cv2.drawChessboardCorners(img1, pattern_size, corners1, found1)
		cv2.drawChessboardCorners(img2, pattern_size, corners2, found2)
 
 
 
		print 'ok'
		cv2.imshow('Bild1',img1)
		cv2.imshow('Bild2',img2)
 
		key=cv2.waitKey(0)
		if not(key==ord('u')):
			img_points1.append(corners1.reshape(-1, 2))
			obj_points1.append(pattern_points)
			img_points2.append(corners2.reshape(-1, 2))
			obj_points2.append(pattern_points)	
 
		if key==ord('q'):
			abbruch=True
 
	cv2.imshow('Bild1',img1)
	cv2.imshow('Bild2',img2)	
	key=cv2.waitKey(20)
	if key==ord('q'): 
	   abbruch=True
 
#Hier werden die vorher gefundenen Matrizen aufgerufen
 
K1=file("Kameramatrix1","r")
p=pickle.Unpickler(K1)
 
camera_matrix1 = p.load()#np.array([[552.718,0.,318.714],\
#[   0. ,552.567,208.716],\
#[   0. ,0.,1.        ]])
K1.close()
K2=file("Kameramatrix2","r")
p=pickle.Unpickler(K2)
camera_matrix2= p.load()#np.array([[568.1268,0.,299.0498],\
#[   0. ,569.2292,236.8351],\
#[   0. ,0.,1.        ]])
K2.close()
 
dist_coefs1 = np.array([-0.1384 , 0.3435 , -0.0055 , 0.0060 , -0.3011])
#camera_matrix2 = np.zeros((3, 3))
dist_coefs2 = np.array([-0.1208 , 0.3704 , -0.0079 , 0.0010 , -0.3450])
#R=np.zeros((3,3))
#T=np.zeros(3)
#E=np.zeros((3,3))
#F=np.zeros((3,3))
 
img_n = len(img_points1)
#rvecs = np.array([np.zeros(3) for i in xrange(img_n)])
#tvecs = np.array([np.zeros(3) for i in xrange(img_n)])
flags = 0
flags |= cv2.CALIB_FIX_INTRINSIC
#flags |= cv2.CALIB_USE_INTRINSIC_GUESS
#flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
#flags |= cv2.CALIB_FIX_FOCAL_LENGTH
flags |= cv2.CALIB_FIX_ASPECT_RATIO
flags |= cv2.CALIB_ZERO_TANGENT_DIST
flags |= cv2.CALIB_SAME_FOCAL_LENGTH
flags |= cv2.CALIB_RATIONAL_MODEL
flags |= cv2.CALIB_FIX_K3
flags |= cv2.CALIB_FIX_K4
flags |= cv2.CALIB_FIX_K5
term_crit = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 100, 1e-5)
 
#Die Funktion stereoCalibrate ist hier die "Wunderfunktion": Sie zieht alle bisher berechneten Werte heran und gibt am Ende alle benötigten 
#Matrizen aus. Sie müssen nur noch leicht verändert werden
 
rms, camera_matrix1,dist_coefs1,camera_matrix2, distcoefs2,R,T,E,F =\
cv2.stereoCalibrate(obj_points1, img_points1, img_points2,(w,h),cameraMatrix1=camera_matrix1,distCoeffs1=dist_coefs1, cameraMatrix2=camera_matrix2,distCoeffs2=dist_coefs2,\
criteria=term_crit,flags=flags)#,camera_matrix1,dist_coefs1,camera_matrix2,dist_coefs2,\
#R,T,E,F)
 
# Nun werden alle gefunden Matrizen mit der Funktion Pickler gespeichert
 
K1=file("Kameramatrix1","w")
p=pickle.Pickler(K1)
p.dump(camera_matrix1)
K1.close()
K2=file("Kameramatrix2","w")
p=pickle.Pickler(K2)
p.dump(camera_matrix2)
K2.close()
Rot=file("Rotationsmatrix","w")
p=pickle.Pickler(Rot)
p.dump(R)
Rot.close()
Trans=file("Translationsmatrix","w")
p=pickle.Pickler(Trans)
p.dump(T)
Trans.close()
EM=file("E","w")
p=pickle.Pickler(EM)
p.dump(E)
EM.close()
FM=file("F","w")
p=pickle.Pickler(FM)
p.dump(F)
FM.close()
Coef1=file("Coef 1","w")
p=pickle.Pickler(Coef1)
p.dump(dist_coefs1)
Coef1.close()
Coef2=file("Coef 2","w")
p=pickle.Pickler(Coef2)
p.dump(dist_coefs2)
Coef2.close()
 
#Nun werden die Matrizen ausgegeben
 
print rms
print "Kamera Matrix 1", camera_matrix1
print "Kamera Matrix 2", camera_matrix2
print "Rotation ", R
print "Translation ", T
print "E ", E
print "F ", F
cv2.destroyAllWindows()
STOP_FLAG=True

Nun besteht noch die Möglichkeit, die Bilder zu normen, indem man alle geraden Linien aneinander angleicht. Soll heißen: Die Bilder werden so gedreht, dass alle Linien in beiden Bildern die gleiche Ausrichtung haben. Dies ist zur Berechnung von dreidimensionalen Bildern wichtig, für einzelne Punkte jedoch nicht weiter von Bedeutung, daher wird es hier weggelassen.

Nun war es soweit, dass das Blockseminar angefangen hatte, die Vorlesungsfreie Zeit hatte also begonnen. Damit wir in der restlichen Zeit ein vorzeigbares Programm erstellen konnten, wurde uns ein schon weiter fortgeschrittenes Programm übergeben: Das Erkennen der Punkte durch eine Kamera im zweidimensionalen funktionierte. Auch bekamen wir bessere Dioden gestellt, deren Helligkeit gleichmäßiger und stärker war. Um nun bis zum Ende etwas Vorzeigbares zu erstellen, einigten wir uns in der Gruppe darauf, keinen Versuch der Lenkung eines Quadrocopters mehr zu starten, sondern uns darauf zu beschränken, ein mit Dioden bestücktes Objekt im Raum zu verfolgen und aus den gewonnenen Daten die Geschwindigkeit des Objektes zu berechnen. Hier der Quellcode von Stefan, der zur Erkennung der 2D-Punkte benötigt wird:

#!/usr/bin/env python
# -*- coding: utf-8 -*-
#
#  finde_punkte.py
#  
#  Copyright 2014 Stefan Born <born@math.tu-berlin.de>
#  
#  This program is free software; you can redistribute it and/or modify
#  it under the terms of the GNU General Public License as published by
#  the Free Software Foundation; either version 2 of the License, or
#  (at your option) any later version.
#  
#  This program is distributed in the hope that it will be useful,
#  but WITHOUT ANY WARRANTY; without even the implied warranty of
#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#  GNU General Public License for more details.
#  
#  You should have received a copy of the GNU General Public License
#  along with this program; if not, write to the Free Software
#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
#  MA 02110-1301, USA.
#  
#  
 
import numpy as np
import cv2
from ps3camera import *
 
#wichtigste methode, findet in einem hsvbild farbbereiche, die einem bestimmten kriterium unterliegen
def finde_zentrum_farbbereich(hsvbild,HSV1,HSV2):
	'''Erwartet ein HSV-Bild, gibt den Schwerpunkt des größten
	Flecks im Farbbereich zwischen HSV1 und HSV2 zurück'''
	hsv1=np.array(HSV1,dtype=np.uint8)
	hsv2=np.array(HSV2,dtype=np.uint8)
	thresh = cv2.inRange(hsvbild, hsv1,hsv2)
	threshc = thresh.copy()  # Beim Bestimmen der Kontouren wird 'thresh' verändert, deswegen eine Kopie
 
	# Kontouren im Schwellenwertbild
	contours,hierarchy = cv2.findContours(thresh,cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)
 
 
	# Finde Kontour mit maximaler Fläche
	max_area = -1
	for cnt in contours:
		area = cv2.contourArea(cnt)
		if area > max_area:
			max_area = area
			best_cnt = cnt
 
	if contours and max_area>1:
		# Finde Schwerpunkt (mit Hilfe der 'Momente')
		M = cv2.moments(best_cnt)
		cx,cy = int(M['m10']/M['m00']), int(M['m01']/M['m00'])
		ort=np.array([cx,cy])
		return True, threshc, ort 
	else:
		return False,threshc,None	
 
# Folgende Funktionen entsprechen vier Leuchtdioden, verwendet 02/14
# Conrad 184433 184447  194460 184473
 
#abwandlung von finde zentrum für die vier farben
def finde_zentrum_rot(hsvbild):
	return finde_zentrum_farbbereich(hsvbild,(5,70,100),(12,255,255))
 
def finde_zentrum_blau(hsvbild):
	return finde_zentrum_farbbereich(hsvbild,(90,100,70),(120,255,255))
 
def finde_zentrum_gruen(hsvbild):
	return finde_zentrum_farbbereich(hsvbild,(55,70,70),(65,255,255))
 
def finde_zentrum_orange(hsvbild):
	return finde_zentrum_farbbereich(hsvbild,(15,70,70),(25,255,255))
 
def main():
	'''Testet die Farbbunktermittlung durch Schwellenwert'''
	KAMERA_NR=1
	capture=initialize_camera_cv(KAMERA_NR)
	sleep(2)
	set_camera_properties_dark(KAMERA_NR)
	_,frame = capture.read()
	frame2=np.zeros_like(frame)
 
	rotgefunden=False  # Diese Variablen werden True, sobald der erste Punkt gefunden
	blaugefunden=False
	gruengefunden=False
	orangegefunden=False
 
	while(1):
		#print "Hund"
		# read the frames
 
		_,frame = capture.read()
 
		frame = cv2.blur(frame,(3,3))
 
		#convert to hsv and find range of colors
 
		hsv = cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)
 
		resrot, threshrot, ortrot = finde_zentrum_rot(hsv)
		resblau, threshblau, ortblau= finde_zentrum_blau(hsv)
		resorange,threshorange,ortorange=finde_zentrum_orange(hsv)
		resgruen,threshgruen, ortgruen=finde_zentrum_gruen(hsv)
 
		if resrot:
			cv2.circle(frame,(ortrot[0],ortrot[1]),6,(0,0,0))
			cv2.circle(frame,(ortrot[0],ortrot[1]),5,(0,0,255),-1)
			cv2.circle(frame2,(ortrot[0],ortrot[1]),2,(0,0,255),-1)
			if rotgefunden:
				cv2.line(frame2,(ortrot[0],ortrot[1]),(ortrotalt[0],ortrotalt[1]),(0,0,255),2)
			rotgefunden=True
			ortrotalt=ortrot.copy()
		if resblau:
			cv2.circle(frame,(ortblau[0],ortblau[1]),6,(0,0,0))
			cv2.circle(frame,(ortblau[0],ortblau[1]),5,(255,0,0),-1)
			cv2.circle(frame2,(ortblau[0],ortblau[1]),2,(255,0,0),-1)
			if blaugefunden:
				cv2.line(frame2,(ortblau[0],ortblau[1]),(ortblaualt[0],ortblaualt[1]),(255,0,0),2)
 
			blaugefunden=True
			ortblaualt=ortblau.copy()
		if resgruen:
			cv2.circle(frame,(ortgruen[0],ortgruen[1]),6,(0,0,0))
			cv2.circle(frame,(ortgruen[0],ortgruen[1]),5,(0,255,0),-1)
			cv2.circle(frame2,(ortgruen[0],ortgruen[1]),2,(0,255,0),-1)
			if gruengefunden:
				cv2.line(frame2,(ortgruen[0],ortgruen[1]),(ortgruenalt[0],ortgruenalt[1]),(0,255,0),2)
 
			gruengefunden=True
			ortgruenalt=ortgruen.copy()
 
		if resorange:
			cv2.circle(frame,(ortorange[0],ortorange[1]),6,(0,0,0))
			cv2.circle(frame,(ortorange[0],ortorange[1]),5,(0,255,255),-1)
			cv2.circle(frame2,(ortorange[0],ortorange[1]),2,(0,255,255),-1)
			if orangegefunden:
				cv2.line(frame2,(ortorange[0],ortorange[1]),(ortorangealt[0],ortorangealt[1]),(0,255,255),2)
 
			orangegefunden=True
			ortorangealt=ortorange.copy()
 
		# Show it, if key pressed is 'Esc', exit the loop
		cv2.imshow('Bild',frame)
		cv2.imshow('Rotanteil',threshrot)
		cv2.imshow('Blauanteil', threshblau)
		cv2.imshow('Spuren', frame2)
		if cv2.waitKey(20)== 27:
			ende=True
			break
 
		#pass
 
	return 0
 
if __name__ == '__main__':
 
	main()

Mit dieser neuen Idee der Geschwindigkeitsberechnungvor Augen machten wir uns an die Arbeit. Zunächst programmierten wir die Erstellung der 3D-Koordinaten aus den 2D-Punkten. Ausnahmsweise verwendeten wir hierfür nicht die Python-Funktion (triangulate_Points), sondern schrieben unsere eigene. Zusätzlich plottet die Funktion die Bahn der Dioden im Raum, sodass ein 3D-Bild der Bewegung der Dioden ensteht. Hier der Quellcode:

#!/usr/bin/env python
# -*- coding: utf-8 -*-
from __future__ import division
 
import cv
import pickle
import cv2
import numpy as np
import finde_punkte
 
 
from visual import *
from scipy import linalg
from thread import start_new_thread
from ps3camera import *
 
def lies_kamera():
	''' Endloschleife um Kamera auslesen;
	das zuletzt gelesene Bild befindet sich in der globalen Variable bild_aktuell'''
	global bild1_aktuell
	global bild2_aktuell
	n=0
	while not STOP_FLAG:
		n+=1
		#print "Frame " ,n
 
	# Ein Bild auslesen
		res,bild=capture1.read()
		bild1_aktuell=bild.copy()
		res,bild=capture2.read()
		bild2_aktuell=bild.copy()
 
#berechnet aus zwei gegebenen Punkten einen 3D-Punkt
def berechne_3d_Punkte (a,b):
 
	#Lädt die gespeicherten Matrizen aus den Kalibrierungsprogrammen
	K1=file("Kameramatrix1","r")
	p=pickle.Unpickler(K1)
	camera_matrix1=p.load()
	K1.close()
 
	K2=file("Kameramatrix2","r")
	p=pickle.Unpickler(K2)
	camera_matrix2=p.load()
	K2.close()
 
	Rot=file("Rotationsmatrix","r")
	p=pickle.Unpickler(Rot)
	R=p.load()
	Rot.close()
 
	Trans=file("Translationsmatrix","r")
	p=pickle.Unpickler(Trans)
	T=p.load()
	Trans.close()
 
	bild_matrix1=np.append(camera_matrix1,np.array([[0],[0],[0]]), axis=1)
	bild_matrix2=np.dot(camera_matrix2,np.append(R,T,axis=1))
 
	if a != None and b != None :
 
		a=a*(-1)
		b=b*(-1)
 
		#erstellt die Matrix M aus K1,K2 und den beiden gegebenen Punkten
		M1=np.concatenate((bild_matrix1,bild_matrix2), axis=0)
		M2=np.concatenate((M1,np.append(a,np.array([[0],[0],[0]]), axis=0)), axis=1)
		M=np.concatenate((M2,np.append(np.array([[0],[0],[0]]),b, axis=0)), axis=1)
		U, D, V=linalg.svd(M)
		L=V[5,0:4]
		x=L[0]
		y=L[1]
		z=L[2]
		f=L[3]
		x=x/float(f)
		y=y/float(f)
		z=z/float(f)
 
		return{'X':x,'Y':y,'Z':z}
	return None
 
#sucht die Dioden in der ersten Kamera
def CamOneDots():
 
	frame=bild1_aktuell.copy()
 
	frame = cv2.blur(frame,(3,3))
	#rgb zu hsv umwandlung	
	hsvbild = cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)
 
	#transformation der ersten kamerapunkte
	resrot,threshrot,rot1 = finde_punkte.finde_zentrum_rot(hsvbild)
	resblau,threshblau,blau1 = finde_punkte.finde_zentrum_blau(hsvbild)
	resgruen,threshgruen,gruen1 = finde_punkte.finde_zentrum_gruen(hsvbild)
	resorange,threshorange,orange1 = finde_punkte.finde_zentrum_orange(hsvbild)
 
	#erstellt die 3x1 Matrix für alle Punkte
	if resrot:
		rot1 = rot1.reshape((2,1))
		rot1=np.concatenate((rot1,np.array([[1]])), axis=0)
	if resblau:
		blau1 =blau1.reshape((2,1))
		blau1=np.concatenate((blau1,np.array([[1]])), axis=0)
	if resgruen:
		gruen1 =gruen1.reshape((2,1))
		gruen1=np.concatenate((gruen1,np.array([[1]])), axis=0)
	if resorange:
		orange1 =orange1.reshape((2,1))
		orange1=np.concatenate((orange1,np.array([[1]])), axis=0)
 
	return{'rot':rot1,'blau':blau1,'gruen':gruen1,'orange':orange1}
 
#sucht die Dioden in Kamera 2
def CamTwoDots():
 
	frame=bild2_aktuell.copy()
 
	frame = cv2.blur(frame,(3,3))	
	hsvbild = cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)
 
	#transformation der zweiten kamerapunkte
	resrot,threshrot,rot2 = finde_punkte.finde_zentrum_rot(hsvbild)
	resblau,threshblau,blau2 = finde_punkte.finde_zentrum_blau(hsvbild)
	resgruen,threshgruen,gruen2 = finde_punkte.finde_zentrum_gruen(hsvbild)
	resorange,threshorange,orange2 = finde_punkte.finde_zentrum_orange(hsvbild)
 
	#Umwandlung in 3x1 Matrix
	if resrot:
		rot2=rot2.reshape((2,1))
		rot2=np.concatenate((rot2,np.array([[1]])), axis=0)	
	if resblau:
		blau2=blau2.reshape((2,1))
		blau2=np.concatenate((blau2,np.array([[1]])), axis=0)
	if resgruen:
		gruen2=gruen2.reshape((2,1))
		gruen2=np.concatenate((gruen2,np.array([[1]])), axis=0)
	if resorange:
		orange2=orange2.reshape((2,1))
		orange2=np.concatenate((orange2,np.array([[1]])), axis=0)
 
	return{'rot':rot2,'blau':blau2,'gruen':gruen2,'orange':orange2}
 
#funktion,die die vektorenlänge zwischen zwei 3D-Punkten berechnet
def vektorcalc(alt,neu):
	altX = alt['X']
	altY = alt['Y']
	altZ = alt['Z']
 
	neuX = neu['X']
	neuY = neu['Y']
	neuZ = neu['Z']
 
	vektorX = neuX - altX
	vektorY = neuY - altY
	vektorZ = neuZ - altZ
 
	vektorlen = sqrt((vektorX**2) + (vektorY**2) + (vektorZ**2))
	return vektorlen
 
#init von kamera 1 und 2 durch ps3camera	
KAMERA_NR1=1
capture1=initialize_camera_cv(KAMERA_NR1)
sleep(2)	
set_camera_properties_dark(KAMERA_NR1)
 
KAMERA_NR2=2
capture2=initialize_camera_cv(KAMERA_NR2)
sleep(2)
set_camera_properties_dark(KAMERA_NR2)
 
sleep(1)
STOP_FLAG=False
 
start_new_thread(lies_kamera,())
sleep(4)
 
#starten der trails für alle dioden
rottrail = curve(color=color.red)
blautrail = curve(color=color.blue)
gruentrail = curve(color=color.green)
orangetrail = curve(color=color.yellow)
 
rotalt = {'X':0,'Y':0,'Z':0}
blaualt = {'X':0,'Y':0,'Z':0}
gruenalt = {'X':0,'Y':0,'Z':0}
orangealt = {'X':0,'Y':0,'Z':0}
 
#hauptmethode
while True:
	X1 = CamOneDots()
	X2 = CamTwoDots()
 
	#3D-Punkt berechnung
	rot3d = berechne_3d_Punkte(X1['rot'],X2['rot'])
	if rot3d != None:
		#rotvektor
		if vektorcalc(rotalt,rot3d) < 20:
			#diorot = sphere(pos=(rot3d["X"],rot3d["Y"],rot3d["Z"]),radius= 2,color=color.red) ausweichcode
			rottrail.append(pos=(rot3d["X"],rot3d["Y"],rot3d["Z"]))
			rotalt = rot3d
 
	blau3d = berechne_3d_Punkte(X1['blau'],X2['blau'])
	if blau3d != None:
		#blauvektor
		if vektorcalc(blaualt,blau3d) < 20:
			#dioblau = sphere(pos=(blau3d["X"],blau3d["Y"],blau3d["Z"]),radius= 2,color=color.blue) ausweichcode
			blautrail.append(pos=(blau3d["X"],blau3d["Y"],blau3d["Z"]))
			blaualt = blau3d
 
	gruen3d = berechne_3d_Punkte(X1['gruen'],X2['gruen'])
	if gruen3d != None:
		#gruenvektor
		if vektorcalc(gruenalt,gruen3d) < 20:
			#diogruen = sphere(pos=(gruen3d["X"],gruen3d["Y"],gruen3d["Z"]),radius= 2,color=color.green) ausweichcode
			gruentrail.append(pos=(gruen3d["X"],gruen3d["Y"],gruen3d["Z"]))
			gruenalt = gruen3d
 
	orange3d = berechne_3d_Punkte(X1['orange'],X2['orange'])
	if orange3d != None:
		#orangevektor
		if vektorcalc(orangealt,orange3d) < 20:
			#dioorange = sphere(pos=(orange3d["X"],orange3d["Y"],orange3d["Z"]),radius= 2,color=color.yellow) ausweichcode
			orangetrail.append(pos=(orange3d["X"],orange3d["Y"],orange3d["Z"]))
			orangealt = orange3d
 
	#abbruchkey q für visual-python
	if scene.kb.keys:
		ch=scene.kb.getkey()
		if ch == "q":
			break
			STOP_FLAG = True

Nun wäre als letzter Punkt die Geschwindigkeitsbestimmung gekommen. Auch die Bewegung von Robotern, also Quadrocoptern oder anderen beweglichen Objekten, wäre mit dieser Grundlage möglich, da nun deren Position im Raum bekannt ist. Wegen Zeitmangel und dadurch, dass das Programm nicht sehr performant ist, haben wir aber an dieser Stelle abgebrochen. Die Bewegung war viel zu aufwendig, um sie in der verbliebenen Zeit zu schaffen und die Geschwindigkeitsbestimmung wurde durch die Langsamkeit des Programms unmöglich.

Es ist wohl vor allem ein Fazit zu ziehen: Auch wenn etwas am Anfang sehr leicht zu programmieren aussieht: Es kann verdammt schwierig sein und noch viel länger dauern! Mit viel Frusttoleranz ist man in jedem Fall gut beraten. Wenn aber dann doch mal etwas funktioniert, macht es auch viel Freude, dass man bis dorthin gekommen ist. Des Weiteren bringt dieses Projekt natürlich ein vergrößertes Verständnis der Verarbeitung von 3D-Bildern in der Informatik mit sich. Wenn man sich durch die vielen, häufig sehr komplizierten Erklärungen hindurchkämpft, kann man am Ende auf eine doch erstaunliche Ansammlung von Wissen zurückblicken. Selbst, wenn wir es später nicht mehr brauchen sollten: Es war ein sehr interessantes Projekt über ein Thema, was man häufig für selbstverständlich nimmt (und daher unterschätzt) und mit dem man sich ohne dieses Labor vermutlich nie so intensiv beschäftigt hätte. Danke dafür! Leider mussten wir unsere Erwartungen und Ziele immer weiter herabsetzen, weil das Anfangsprojekt einfach zu umfangreich war. Aber auch hierraus kann man sicher etwas lernen: Nämlich, dass man Ziele eben manchmal auch nicht erreichen kann und Abstriche machen muss.

Die Gruppe Robotik im Labor Mathesis

Robotik