Unterschiede

Hier werden die Unterschiede zwischen zwei Versionen gezeigt.

--- ws1314:dokumentation [2014/02/13 17:10]
maximilian.obst
+++ ws1314:dokumentation [2016/05/10 14:46] (aktuell)
@@ Zeile 27: / Zeile 27: @@
 == 2.2 Erste Planänderung: Funkwellen ==
-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 - - 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:
+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:
 <code python>
@@ Zeile 72: / Zeile 72: @@
 </code>
-Bald darauf erkannten wir jedoch, dass die Entfernungsmessung viel zu ungenau funktionierte - und daher nicht in Frage kam. Also war auch diese Idee gescheitert.
+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.
 == 2.3 Der endgültige Plan ==
@@ Zeile 78: / Zeile 78: @@
 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.
+{{ :pinhole-camera.png?nolink |}}
+Quelle: http://pille.iwr.uni-heidelberg.de/~kinect01/img/pinhole-camera.png (23.01.2014; 17:25 Uhr)
 === 3 Die Umsetzung ===
@@ Zeile 134: / Zeile 137: @@
 </code>
-Unsere Gruppe wollte sich zuerst mit dem Quadrocopter beschäftigen und dessen Gleichgewichtsstörungen beheben und danach gucken, was man noch alles implementieren könnte: z.B. eigenständig Routen abfliegen, an bestimmten Orten Aufnahmen mit einer Kamera machen oder auch ferngesteuert Fotos machen. Dafür wollten wir GPS und eine Kamera zur Kollisionserkennung benutzen.
+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:
-Dann kam das Treffen mit Felix (Leiter des Robotiklabors). Der hat uns dann erstmal erklärt, dass der große Quadrocopter aufgrund seiner Größe nicht in zu kleinen Innenräumen fliegen darf, weil ihn aufgrund seiner Größe seine eigens verursachte Luftzirkulation selber behindert. Man kann ihn also in Innenräumen nicht ausprobieren. Draußen ist auch eher schlecht. Für Draußen-Flüge braucht man eine Genehmigung und generell ist das Gerät sehr wasseranfällig und sollte deshalb nach Möglichkeit nur sehr sehr wenig bis gar nicht mit ihm in Berührung kommen, was im Winter (sollte der irgendwann noch kommen) generell eher schwierig ist. Außerdem kann es bei Fehlprogrammierung durchaus vorkommen, dass der Quadrocopter ohne Möglichkeit, ihn aufzuhalten, davonfliegt und dabei sich oder anderen Schaden zufügt.
+<code python>
+#!/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.
+#
+#
-Also entfernten wir uns etwas von dem Gedanken mit diesem großen Quadrocopter uns zu beschäftigen und überlegten uns mehr mit kleineren Versionen zu arbeiten, da diese ohne große Probleme drinnen fliegen können.
-Damit hatten wir auch nicht mehr das Problem mit dem Gleichgewicht.
-Wir hatten uns daraufhin auf das Ziel fokussiert das Fluggerät soweit zu programmieren, dass es sich eigenständig im (Innen)Raum bewegen kann und zwar ohne irgendwo gegen zu stoßen. Dafür muss es sowohl wissen, wie der Raum aussieht, als auch wo es sich selbst in diesem Raum befindet.
-Seitdem haben wir uns des zweiten Teils dieses Problems angenommen: Die Ortung von sich selbst.
+import numpy as np
+import cv2
+from thread import start_new_thread
+from time import sleep
-Unser erster Gedankenansatz lief über die Triangulation. D.h. wir haben mehrere festgesetzte Punkte, deren Standort klar ist und die messen den Abstand zu dem Fluggerät, woraus wir dessen genaue Position im Raum bestimmen können. Da wir zeitgleich das Plotten mit Python im Labor kennen gelernt haben, haben wir versucht unsere Idee zumindest im 2-dimensionalen-Raum darzustellen. Dabei hatten wir auch schon ein paar Probleme, da wir nicht genau wussten, wie die Funktion, welche wir aus dem Internet hatten funktioniert. Deshalb haben wir viel rumprobiert, bis uns Stefan eine bessere gegeben hat.
+KAMERA_NR=1
+STOP_FLAG=False
-Am Ende bei rausgekommen ist folgende Funktion:
+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
 </code>
-<code numpy>
+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:
+<code python>
 import numpy as np
-import matplotlib.pyplot as plt
+import cv2
-import matplotlib.path as mpath
+import pickle
-import matplotlib.lines as mlines
+from thread import start_new_thread
-import matplotlib.patches as mpatches
+from time import sleep
+KAMERA1_NR=1
+KAMERA2_NR=2
+STOP_FLAG=False
-W = { }
+def lies_kamera():
-N = 5
+	''' 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
-qx = np.random.randint(30)
+capture1= cv2.VideoCapture(KAMERA1_NR)
-qy = np.random.randint(30)
+capture1.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH,640)
-W["Referenzpunkt X"] =  qx
+capture1.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT,480)
-W["Referenzpunkt Y"] =  qy
+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)
-plt.figure()
+capture2= cv2.VideoCapture(KAMERA2_NR)
-ax=plt.subplot(111)
+capture2.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH,640)
-ax.set_xlim([0,30])
+capture2.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT,480)
-ax.set_ylim([0,30])
+sleep(2)
-ax.add_patch(mpatches.Circle((qx, qy), radius=1,color="red",alpha=0.1))
+#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)
-for i in range(0,N):                             #scatterplot handles data nonlinear, its not legible for triangulation-use
+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)
-	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))
+	h, w = img1.shape[:2]
-	#plt.scatter(x, y, s=area*190 , alpha=0.1)
+	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)
-plt.draw()
+		cv2.drawChessboardCorners(img1, pattern_size, corners1, found1)
-plt.show()
+		cv2.drawChessboardCorners(img2, pattern_size, corners2, found2)
-print W
+		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
 </code>
-<code>
-//Es wird etwas komisch angezeigt, aber wenn ihr auf bearbeiten klickt, könnt ihr, wenn ihr wollt, den Code gut rauskopieren und selbst ausprobieren.//
+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.
-Als wir das Grundkonzept hatten, mussten wir allerdings noch eine Möglichkeit finden das Grundkonzept technisch umzusetzen.
+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.
-Wir haben erst über Lichtsignale nachgedacht, haben dann ein wenig im Internet nach Möglichkeiten der Entfernungsmessung gesucht und sind dann auf WLAN und Bluetooth gestoßen.
+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.
-Bei Bluetooth haben wir auch gleich eine passende Mod mit Funktion gefunden und das ausprobiert
+Hier der Quellcode von Stefan, der zur Erkennung der 2D-Punkte benötigt wird:
+<code python>
+#!/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 bluetooth as bluetooth
+import numpy as np
+import cv2
+from ps3camera import *
-print "performing inquiry..."
+#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
-nearby_devices = bluetooth.discover_devices(lookup_names = True)
+# Folgende Funktionen entsprechen vier Leuchtdioden, verwendet 02/14
+# Conrad 184433 184447  194460 184473
-print "found %d devices" % len(nearby_devices)
+#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))
-for addr, name in nearby_devices:
+def main():
-    print "  %s - %s" % (addr, name)
+	'''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
-find_service(name=None, uuid=None, address=None)
+if __name__ == '__main__':
+	main()
-//Ähnliche Anzeigeprobleme: Deshalb bei Bedarf bitte wie oben. Allerdings müsst ihr vorher bluepy downloaden//
+</code>
-Das haben wir dann mehrfach ausprobiert, aber es war deutlich zu ungenau für unsere Zwecke.
+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:
-Dann haben wir überlegt unsere eigene Funktion zu schreiben, durch die wir die Entfernung messen können über elektromagnetische Wellen. Diese bewegen sich aber viel zu schnell für einen Computer, um daraus vernünftige Ergebnisse raus zuziehen. Außerdem bräuchte man sehr starke Sender für die Distanz, sodass wir uns auch von dieser Idee verabschieden mussten.
+<code python>
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+from __future__ import division
-Die letzte Idee war, über 2 oder mehr Kameras Dioden auf dem Quadrocopter zu erkennen und den Quadrocopter dadurch Koordinaten im von den Kameras überblickten Raum zuzuordnen. Dafür haben wir zunächst versucht, verschiedene Kameras so einzustellen, dass möglichst nur noch die Dioden auf den Bildern zu sehen waren. Dann haben wir die Bilder in den HSV-Farbraum umgewandelt, um das Bild so zu bearbeiten, dass Bildteile bestimmter Helligkeit, Farbe und Sättigung weiß und alle anderen Bildteile schwarz gefüllt werden. Dies gelang uns recht gut, einzig die Lichtbrechung an den Deckenlampen bewirkt teilweise eine Störung.
+import cv
-Die jetzige Aufgabe ist es, die Kameras zu kalibrieren. Dafür wird mithilfe eines Schachbrettes bekannter Größe zunächst für die Kameras jeweils eine Kalibrierungsmatrix erstellt. Mit diesen Matrizen kann wird dann über die Funktion stereoCalibrate die Rotations- und Translationsmatrix und daher dann die Fundamental Matrix bestimmt.
+import pickle
+import cv2
+import numpy as np
+import finde_punkte
-Für alle, die, wie wir anfangs, keine Ahnung von den obigen Themen haben, hier ein kleiner Einstieg:
-Das Problem bei der Bestimung von Koordinaten im Raum durch Kamerabilder ist, dass die Bilder 2-dimensional, der Raum jedoch 3-dimensional ist. Alle Punkte auf dem Kamerabild entsprechen also einer Linie im Raum mit dem Ursprung in den Kamera. Gelöst werden kann das Problem durch verwenden mehrerer Kameras, die einen Punkt auch 3-dimensional erkennen können. Zunächst wird daher ein Koordinatensystem für eine Kamera aufgebaut, bestehend aus einer Kalibrierungsmatrix, die kameraspezifisch ist, und einer um eine Nullspalte erweiterte Einheitsmatrix. Dann wird ein an die erste Kamera angepasstes Koordinatensystem für die zweite Kamera aufgebaut. Dieses Koordinatensystem besteht ebenfalls aus einer Kalibrierungsmatrix, multipliziert mit einer Rotationsmatrix erweitert um eine einspaltige Translationsmatrix. Dafür muss zunächst die Drehung und der Abstand der einen Kamera zu der anderen bestimmt werden. Dafür werden die Rotations- und die Translationsmatrix benötigt. Diese werden durch Kalibrierung bestimmt, wobei die Kameras danach für den Versuch nicht mehr verschoben werden dürfen. Sie müssen also jedes mal kalibriert werden! In diesem Koordinatensystem wird jedem Punkt auf der Bildebene einer Kamera eine Linie auf der Bildebene der anderen Kamera zugeordnet. Dies kann für mehrere Punkte gemacht werden. Jede der entstehenden Linien trifft sich in einem Punkt, dem Epipolar. Dieser Punkt entspricht den Punkten auf der Bildebene der anderen ersten Kamera. Mit diesen einander zugeordneten Punkten kann man dann den 3-dimensionalen Punkt berechnen.
-{{ :pinhole-camera.png?nolink |}}
+from visual import *
-Quelle: http://pille.iwr.uni-heidelberg.de/~kinect01/img/pinhole-camera.png (23.01.2014; 17:25 Uhr)
+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
+</code>
+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.
+=== 4 Fazit ===
+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 vorher bestimmten Matrizen werden nun mit Hilfe der Funktion stereoRectify umgeformt, um sie in der Funktion triangulatePoints endlich so zu nutzen, dass 3D-Punkte bestimmt werden können.
+Die Gruppe Robotik im Labor Mathesis
-Soweit zur Berechnung der 3d-Punkte. Nach diesem Problem mussten wir uns wieder dem Problem der Punkterkennung widmen: Aus einem Kamerabild nötige Punkte herauszufiltern, die man in die Funktion triangulatePoints einsetzen kann.
 [[Robotik]]

Mathesis Wiki

Benutzer-Werkzeuge

Webseiten-Werkzeuge

Unterschiede

Seiten-Werkzeuge