Stereo Vision wird vielen Leuten kein umgänglicher Begriff sein, daher will ich erst einmal erklären, was es damit auf sich hat. Mit Hilfe unserer Augen können wir Abstände schätzen, unser Gehirn kennt den Abstand unserer Augen und vergleicht die erkannten Bilder, woraus abgeschätzt werden kann, ob ein Objekt nah oder fern ist, denn nahe Objekte verschieben sich (in beiden Bildern) mehr als weit entfernte Objekte. Dieses Prinzip lässt sich auch mit Algorithmen nachstellen; alles was man dazu braucht sind 2 Kameras und einen Raspberry Pi.
Um z.B. Tiefen anhand Stereo Kameras zu schätzen, müssen zu erst die sog. „Intrinsic“ und „Extrinsic“ Parameter der Kameras bestimmt werden, was man Kalibrierung nennt. Dazu bietet OpenCV alle nötigen Funktionen, welche in diesem Tutorial gezeigt werden. Im anschließenden Teil werden wir dann mit Hilfe dieser Kameras Tiefen berechnen.
Zubehör
Alles was für Stereo Vision nötig ist, ist folgendes:
- Raspberry Pi (Modell mit 2 oder mehr USB Anschlüssen)
- 2x (identische) Webcams
- Doppelseitiges Klebeband oder Panzertape
- Hartpappe / Karton
Es müssen nicht unbedingt zwei identische Kameras sein, allerdings empfiehlt sich das sehr. Sollten die Kameras eine unterschiedliche Auflösung haben, müssten die Bilder runter skaliert werden, worauf ich aber in diesem Tutorial nicht näher eingehe.
Vorkenntnisse
Zuallererst kann ich nur wärmstens empfehlen sich ein wenig in Stereo Vision einzulesen, falls man komplett neu einsteigt. Zwei nützliche Links dazu sind folgende:
- Allgemeines Verständnis zu Tiefenkarten: http://docs.opencv.org/master/dd/d53/tutorial_py_depthmap.html
- Ausführliche Erklärung der verwendeten Funktionen: http://docs.opencv.org/2.4/doc/tutorials/calib3d/camera_calibration/camera_calibration.html
Außerdem wirst du gleich ein Schachbrettmuster zum Kalibrieren benötigen. Ich habe eines als PDF hoch geladen, welches du ausdrucken und auf einen Karton/gerade Fläche kleben kannst. Die Größe der Felder ist dabei genau 2.5cm. Die PDF mit Checkerboard (Schachbrett) gibt es hier.
OpenCV installieren und testen
Bevor es losgehen kann, muss OpenCV erst einmal installiert werden. Die Installation kann (abhängig von deinem Raspberry Pi Model) etwas länger dauern. Eine ausführliche Anleitung zum Installieren von OpenCV3, welches auch in diesem und den kommenden Tutorials verwendet wird, gibt es hier:
OpenCV auf dem Raspberry Pi installieren
Kameras verbinden und Bilder aufnehmen
Die beiden Kameras werden ganz normal per USB an das Raspberry angeschlossen und sollten direkt erkannt werden (du kannst es mittels lsusb prüfen). Anschließend erstellen wir ein Verzeichnis und darin unsere Skripte.
mkdir stereoCamera && cd stereoCamera mkdir images sudo nano captureImages.py
Dieses Skript zum Aufnehmen der Stereo-Bilder bekommt folgenden Inhalt (STRG+O und STRG+X zum speichern und beenden):
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 |
import cv2 import numpy as np import time import sys imageCount = 15 interval = 5 # seconds outputPath = "" # "images/" filenameL = "left%02d.jpg" filenameR = "right%02d.jpg" if __name__ == '__main__': assert len(sys.argv) >= 3 leftCamera = int(sys.argv[1]) # 0 for "/dev/video0" rightCamera = int(sys.argv[2]) if len(sys.argv) > 3: outputPath = sys.argv[3] if len(sys.argv) > 4: imageCount = int(sys.argv[4]) if len(sys.argv) > 5: interval = float(sys.argv[5]) try: capL = cv2.VideoCapture(leftCamera) capR = cv2.VideoCapture(rightCamera) time.sleep(5) # wait some seconds for i in range(imageCount): print "Taking image %d:" % i ret, frame = capL.read() cv2.imwrite(outputPath + filenameL % i, frame) print " Left image done." if ret else " Error while taking left image..." ret, frame = capR.read() cv2.imwrite(outputPath + filenameR % i, frame) print " Right image done." if ret else " Error while taking right image..." time.sleep(interval) finally: capL.release() capR.release() cv2.destroyAllWindows() |
Anschließend können wir die beiden Geräte IDs (ls /dev/video*, z.B. 0 für „/dev/video0“ ) dem Skript übergeben und es erstellt uns die Anzahl der Bilder. Achte darauf, dass als erstes die linke und danach die Rechte Kamera übergeben wird. Die zusätzlichen Parameter für den Ausgabepfad, die Anzahl der Bilder und die Zeit zwischen den Aufnahmen ist optimal (ansonsten werden die im Skript definierten Werte genommen).
Dabei muss noch gesagt werden, dass unterschiedliche Kameras verschieden lange brauchen um ein Bild aufzunehmen. Da beide Bilder hintereinander aufgenommen werden (minimal verzögert), solltest du das Schachbrettmuster still halten.
sudo python captureImages.py 0 1 "images/"
Nach einigen Sekunden werden mit beiden Kameras Bilder aufgenommen. Halte das ausgedruckte Checkerboard in jedem Bild in einer etwas anderen Lage. Ich empfehle zwischen 15 (voreingestellt) und 20 Bildern, da manchmal ein paar Frames fehlerhaft sind. Diese sollten im Nachhinein gelöscht werden (inkl. dem entsprechenden Frame der linken/rechten Kamera).
Ein Hinweis: Falls du einen Fehler wie „VIDIOC_STREAMON: Broken pipe“ angezeigt bekommst, liegt das höchstwahrscheinlich daran, dass deine Kamera USB1.1 verwendet. Vereinzelt findet man Hinweise, dass das resetten der USB Verbindung nach jedem Bild helfen würde, was ich aber nicht bestätigen kann. Ich habe dazu bisher keine Lösung gefunden, daher würde ich dringend min. USB2.0 Kameras empfehlen.
Kalibrierung
Sind alle Bilder gemacht, können wir die Kameras einzeln und zusammen kalibrieren. Dazu verwenden wir die eben gemachten Bilder und ermitteln Intrinsics und Extrinsics. Ich habe dazu ein Skript erstellt, welches du am besten kopieren solltest:
sudo nano stereoCalibrate.py
Der Inhalt ist folgender:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 |
#!/usr/bin/env python import numpy as np import cv2 import json # local modules from common import splitfn # built-in modules import os USAGE = ''' USAGE: calib.py [--save <filename>] [--debug <output path>] [--square_size] [<left image mask>] [<right image mask>] ''' if __name__ == '__main__': import sys import getopt from glob import glob args, img_mask = getopt.getopt(sys.argv[1:], '', ['save=', 'debug=', 'square_size=']) args = dict(args) try: img_mask_left = img_mask[0] img_mask_right = img_mask[1] except: img_mask_left = '../data/left*.jpg' img_mask_right = '../data/right*.jpg' img_names = [glob(img_mask_left), glob(img_mask_right)] img_names[0].sort() img_names[1].sort() debug_dir = args.get('--debug') save_name = args.get('--save') square_size = float(args.get('--square_size', 1.0)) print "Square Size:", square_size, "cm" 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) pattern_points *= square_size obj_points = [[], []] img_points = [[], []] h = [0, 0] w = [0, 0] for i in range(2): for fn in img_names[i]: print 'processing %s...' % fn, img = cv2.imread(fn, 0) #gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) if img is None: print "Failed to load", fn continue h[i], w[i] = 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(img, corners, (5, 5), (-1, -1), term) if debug_dir: vis = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) cv2.drawChessboardCorners(vis, pattern_size, corners, found) path, name, ext = splitfn(fn) cv2.imwrite('%s/%s_chess.bmp' % (debug_dir, name), vis) if not found: print 'chessboard not found' continue img_points[i].append(corners.reshape(-1, 2)) obj_points[i].append(pattern_points) print 'ok' assert (len(img_points[0]) == len(img_points[1])) assert (len(obj_points[0]) == len(obj_points[1])) obj_points = obj_points[0] # run monocular calibration on each camera to get intrinsic parameters rms_l, camera_matrix_l, dist_coeffs_l, _, _ = cv2.calibrateCamera(obj_points, img_points[0], (w[0], h[0]), None, None) rms_r, camera_matrix_r, dist_coeffs_r, _, _ = cv2.calibrateCamera(obj_points, img_points[1], (w[1], h[1]), None, None) print "---------- Camera Left ------------" print "RMS:", rms_l print "camera matrix:\n", camera_matrix_l print "distortion coefficients: ", dist_coeffs_l.ravel() print "\n---------- Camera Right -----------" print "RMS:", rms_r print "camera matrix:\n", camera_matrix_r print "distortion coefficients: ", dist_coeffs_r.ravel() # set stereo flags stereo_flags = 0 stereo_flags |= cv2.CALIB_USE_INTRINSIC_GUESS stereo_flags |= cv2.CALIB_FIX_INTRINSIC # More availabe flags... # stereo_flags |= cv2.CALIB_USE_INTRINSIC_GUESS \ # Refine intrinsic parameters # stereo_flags |= cv2.CALIB_FIX_PRINCIPAL_POINT \ # Fix the principal points during the optimization. # stereo_flags |= cv2.CALIB_FIX_FOCAL_LENGTH \ # Fix focal length # stereo_flags |= cv2.CALIB_FIX_ASPECT_RATIO \ # fix aspect ratio # stereo_flags |= cv2.CALIB_SAME_FOCAL_LENGTH \ # Use same focal length # stereo_flags |= cv2.CALIB_ZERO_TANGENT_DIST \ # Set tangential distortion to zero # stereo_flags |= cv2.CALIB_RATIONAL_MODEL \ # Use 8 param # run stereo calibration rms_stereo, camera_matrix_l, dist_coeffs_l, camera_matrix_r, dist_coeffs_r, R, T, E, F = cv2.stereoCalibrate( obj_points, img_points[0], img_points[1], camera_matrix_l, dist_coeffs_l, camera_matrix_r, dist_coeffs_r, (w[0], h[0]), flags=stereo_flags) # run stereo rectification rectification_matrix_l, rectification_matrix_r, projection_matrix_l, projection_matrix_r, _, _, _ = cv2.stereoRectify(camera_matrix_l, dist_coeffs_l, camera_matrix_r, dist_coeffs_r, (w[0], h[0]), R, T) print "\n---------- Camera Stereo ----------" print "RMS:", rms_stereo print "camera matrix left:\n", camera_matrix_l print "distortion coefficients left: ", dist_coeffs_l.ravel() print "camera matrix right:\n", camera_matrix_r print "distortion coefficients right: ", dist_coeffs_r.ravel() print "R:\n", R #print "T:\n", T #print "E:\n", E #print "F:\n", F print "Rectification matrix Left :\n", rectification_matrix_l print "Rectification matrix Right :\n", rectification_matrix_r print "Projection Matrix Left :\n", projection_matrix_l print "Projection Matrix Right :\n", projection_matrix_r if save_name: data = {"camera_matrix_l": camera_matrix_l.tolist(), "camera_matrix_r": camera_matrix_r.tolist(), "dist_coeffs_l": dist_coeffs_l.tolist(), "dist_coeffs_r": dist_coeffs_r.tolist(), "rectification_matrix_l": rectification_matrix_l.tolist(), "rectification_matrix_r": rectification_matrix_r.tolist(), "projection_matrix_l": projection_matrix_l.tolist(), "projection_matrix_r": projection_matrix_r.tolist(), "w": w, "h": h} with open(save_name, "w") as f: json.dump(data, f) cv2.destroyAllWindows() |
Anschließend können wir die Kalibrierung starten. Dazu geben wir noch einen Datei-Namen an, unter dem die Kamera Matrizen gespeichert werden, damit wir sie später einfach laden können, ohne die Kalibrierung erneut durchführen zu müssen:
sudo python stereoCalibrate.py --square_size=2.5 --save "camera_matrices.json" "images/left*.jpg" "images/right*.jpg"
Hat alles geklappt, solltest du alle Matrizen in der Kommandozeile angezeigt bekommen und in einer JSON Datei gespeichert haben.
Test
Um nun noch zu evaluieren, ob die Matrizen auch richtig sind, wenden wir sie auf die Bilder an. Dabei zeige ich folgend kurz die Anwendung:
sudo nano test_remap.py
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 |
import numpy as np import cv2 import json with open('camera_matrices.json') as data_file: data = json.load(data_file) camera_matrix_l = np.array(data['camera_matrix_l']) dist_coeffs_l = np.array(data['dist_coeffs_l']) rectification_matrix_l = np.array(data['rectification_matrix_l']) projection_matrix_l = np.array(data['projection_matrix_l']) w = data['w'] h = data['h'] map1_l, map2_l = cv2.initUndistortRectifyMap(camera_matrix_l, dist_coeffs_l, rectification_matrix_l, projection_matrix_l, (w[0], h[0]), cv2.CV_16SC2) img = cv2.imread("images/left01.jpg", 0) i = cv2.remap(img, map1_l, map2_l, cv2.INTER_LINEAR) cv2.imshow('image', i) cv2.waitKey(0) #cv2.imwrite('test.bmp', i) # alternativ cv2.destroyAllWindows() |
sudo python test_remap.py
Achja, falls du es über SSH ausführst und kein Bild angezeigt bekommst, solltest du X11 (Linux) bzw. Xming (Windows) aktivieren. Ansonsten würde ein Zugriff per Remote auch funktionieren. Die auskommentierte, vorletzte Zeile ist als Alternative bzw. wenn du das Bild speichern willst.
Um die rechten Bilder zu remappen, musst du natürlich alle Matrizen für die rechte Kamera nehmen. Um ein Beispiel zu geben, habe ich „right03.jpg“ (Titel-Bild) mit entsprechenden Matrizen rektifiziert, was folgendermaßen aussieht:
Ich verwende ähnlichen Code zur Umrechnung der Bilder, bevor ich Stereo Matching darauf anwende.