Schätzen Sie die Haltung des AR-Markers mit Python + OpenCV + Drohne
Einführung
Dies ist Qiitas erster Beitrag. Ich möchte eine sehr grobe Methode für Studenten der Informationstechnik schreiben, um AR-Marker mit Python, OpenCV und Drohnen zu generieren und zu erkennen und ihre Haltung abzuschätzen.
Ich habe auf diesen Blog verwiesen. https://qiita.com/ReoNagai/items/a8fdee89b1686ec31d10
Implementierung
ArUco-Markergenerierung
Dieses Mal werden wir ArUco verwenden, eine AR-Bibliothek, die eines der OpenCV-Module ist. Der ArUco-Marker kann mit dem folgenden Code generiert werden.
generate.py
#!/usr/bin/env python
# -*- coding: utf-8 -*
import cv2
aruco = cv2.aruco
dir(aruco)
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
marker = aruco.drawMarker(dictionary, 1, 300)
cv2.imwrite('id-001.png', marker)
Mit aruco.DICT_4X4_50 können 50 Arten von Markern generiert werden.
Wenn ich versuche, eine zu generieren, sieht es vorerst so aus.
Es wird gedruckt und an die Wand geklebt.
Ich habe es benutzt, um die Drohne zu verfolgen, also habe ich es so auf die Rückseite der Drohne geklebt.
ArUco-Markererkennung
Bei der Erkennung von der Kamera der Drohne
Ich benutze AR.Drone 2.0 von Papagei.
Parrot AR. Drone2.0， https://www.parrot.com/jp/doron/parrot-ardrone-20-power-edition (2020.4.22 access)
detect_drone.py
# -*- coding: utf-8 -*-
import numpy as np
import cv2
import sys
from pyardrone import ARDrone
import logging
logging.basicConfig(level=logging.DEBUG)
client = ARDrone()
client.video_ready.wait()
cnt=0
aruco = cv2.aruco #Aruco Bibliothek
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
try:
while True:
cv2.imwrite('drone'+str(cnt)+'.png',client.frame)
cnt+=1
corners, ids, rejectedImgPoints = aruco.detectMarkers(client.frame, dictionary) #Marker erkennen
aruco.drawDetectedMarkers(client.frame, corners, ids, (0,255,0)) #Zeichnen Sie auf den erkannten Marker
if cv2.waitKey(10) == ord(' '):
break
finally:
client.close()
Wenn Sie mit einer allgemeinen Webkamera usw. arbeiten.
detect_camera.py
# -*- coding: utf-8 -*-
import numpy as np
import cv2
import sys
aruco = cv2.aruco #Aruco Bibliothek
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
def arReader():
print(cv2.getBuildInformation())
cap = cv2.VideoCapture(0)
cnt=0
while True:
ret, frame = cap.read()
if frame is None: break
corners, ids, rejectedImgPoints = aruco.detectMarkers(frame, dictionary) #Marker erkennen
print(corners,ids)
aruco.drawDetectedMarkers(frame, corners, ids, (0,255,0)) #Zeichnen Sie auf den erkannten Marker
cv2.imwrite('result'+str(cnt)+'.png',frame)
cnt+=1
cap.release()
cv2.destroyAllWindows()
if __name__ == '__main__':
arReader()
Ausführungsergebnis
Auf diese Weise wurden die Rand-, ID-, Eck- und XYZ-Achsen des Markers gezeichnet und erkannt.
Messen Sie den Abstand und die Neigung zwischen dem Marker und der Kamera
Wenn Sie den Abstand und die Neigung zwischen dem Marker und der Kamera ermitteln möchten, verwenden Sie im Allgemeinen einen Infrarotsensor. Diesmal wird jedoch zusätzlich zu den internen Parametern und dem Verzerrungskoeffizienten der Kamera durch Kamerakalibrierung die Drehung ermittelt Sie wird unter Verwendung einer Matrix, eines Rotationsvektors und eines Translationsvektors berechnet.
Kamerakalibrierung
Führen Sie zunächst eine Kamerakalibrierung durch, um die internen Parameter und den Verzerrungskoeffizienten der Kamera zu erhalten.
calib.py
# -*- coding: utf-8 -*
import numpy as np
import cv2
import glob
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((7*7,3), np.float32)
objp[:,:2] = np.mgrid[0:7,0:7].T.reshape(-1,2)
objpoints = []
imgpoints = []
gray_images=glob.glob('chess*.png')
cnt=0
for fname in gray_images:
img = cv2.imread(fname)
gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (7,7),None)
if ret == True:
objpoints.append(objp)
corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
imgpoints.append(corners2)
img = cv2.drawChessboardCorners(img, (7,7), corners2,ret)
cv2.imwrite('calib'+str(cnt)+'.png',img)
cnt+=1
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)
print(mtx, dist)
cv2.destroyAllWindows()
Ausführungsergebnis
Ausführungsergebnis
Interne Parameter der Kamera
[[9.31357583e+03 0.00000000e+00 1.61931898e+03]
[0.00000000e+00 9.64867367e+03 1.92100899e+03]
[0.00000000e+00 0.00000000e+00 1.00000000e+00]]
Dehnungskoeffizient
[[ 0.22229833 -6.34741982 0.01145082 0.01934784 -8.43093571]]
Berechnet mit Rodriguez
Berechnungen werden unter Verwendung einer Rotationsmatrix, eines Rotationsvektors und eines Translationsvektors durchgeführt. Schreiben Sie camera_matrix mit den Parametern neu, die mit der obigen Methode erhalten wurden.
distance.py
#!/usr/bin/env python
# -*- coding: utf-8 -*
import cv2
import numpy as np
import sys
from pyardrone import ARDrone
import logging
logging.basicConfig(level=logging.DEBUG)
aruco = cv2.aruco #Aruco Bibliothek
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
client = ARDrone()
client.video_ready.wait()
parameters = aruco.DetectorParameters_create()
parameters.cornerRefinementMethod = aruco.CORNER_REFINE_CONTOUR
def main():
cnt=0
marker_length = 0.1 # [m]
camera_matrix = np.array( [[ , , ],
[ , , ],
[ , , ]] )
#Finden Sie Parameter durch Kamerakalibrierung ↑
distortion_coeff = np.array( [[ 0.22229833, -6.34741982, 0.01145082, 0.01934784, -8.43093571]] )
try:
while True:
corners, ids, rejectedImgPoints = aruco.detectMarkers(client.frame, dictionary, parameters=parameters)
aruco.drawDetectedMarkers(client.frame, corners, ids, (0,255,255))
if len(corners) > 0:
for i, corner in enumerate(corners):
# rvec -> rotation vector, tvec -> translation vector
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corner, marker_length, camera_matrix, distortion_coeff)
tvec = np.squeeze(tvec)
rvec = np.squeeze(rvec)
rvec_matrix = cv2.Rodrigues(rvec)
rvec_matrix = rvec_matrix[0]
transpose_tvec = tvec[np.newaxis, :].T
proj_matrix = np.hstack((rvec_matrix, transpose_tvec))
euler_angle = cv2.decomposeProjectionMatrix(proj_matrix)[6] # [deg]
print("x : " + str(tvec[0]))
print("y : " + str(tvec[1]))
print("z : " + str(tvec[2]))
print("roll : " + str(euler_angle[0]))
print("pitch: " + str(euler_angle[1]))
print("yaw : " + str(euler_angle[2]))
draw_pole_length = marker_length/2 #Echte Länge[m]
aruco.drawAxis(client.frame, camera_matrix, distortion_coeff, rvec, tvec, draw_pole_length)
cv2.imwrite('distance'+str(cnt)+'.png',client.frame)
cnt+=1
key = cv2.waitKey(50)
if key == 27: # ESC
break
finally:
client.close()
if __name__ == '__main__':
try:
main()
except KeyboardInterrupt:
pass
Ausführungsergebnis
Um zu überprüfen, ob der Abstand korrekt gemessen werden kann, stellen Sie den Abstand zwischen dem Marker und der Kamera (Drohne) auf 25 cm und 50 cm ein und führen Sie das Programm aus.
Ausführungsergebnis(25cm)
x : -0.5410338091817037
y : -0.713790809892646
z : 3.828983632577233
roll : [173.43049198]
pitch: [-20.42010647]
yaw : [1.58947772]
Ausführungsergebnis(50cm)
x : -1.0633298519994792
y : -1.3476793013004273
z : 7.311742619516305
roll : [-150.91884043]
pitch: [2.45488175]
yaw : [3.61554034]
Der Wert des Ausführungsergebnisses wird gut verdoppelt. Auf diese Weise können die Größe der Bewegung und der Wert des Roll-Pitch-Gierens für jede der XYZ-Achsen erhalten werden.
Bonus
Aus dem erhaltenen Wert wurde die Drohne durch bedingte Verzweigung verfolgt, wie in der folgenden Tabelle gezeigt.
| Marker-Abstand zwischen den Kameras | Drohnenbetrieb |
|---|---|
| ~0.5m | Schweben |
| 0.5m ~ 2.0m | Voraus |
| 2.0m ~ | Landung |
| anders als das | Schweben |
Das Konfigurationsdiagramm sieht folgendermaßen aus.
Da Multi-Hop-Kommunikation (fossil) verwendet wird, wird RasPi, wenn sogar RasPi auf der Drohne platziert werden kann, ein Forschungsterminal an einem Ort sein, an dem Menschen nicht eintreten können.
Aus diesem Grund muss ich eine große Menge an RasPi-WLAN-AP erstellen und dnsmasq und hostapd einstellen, aber dies ist auch zu mühsam, sodass ich es zu einem anderen Zeitpunkt schreiben werde.
abschließend
Dieses Mal habe ich Python und OpenCV verwendet, um AR-Marker zu generieren und zu erkennen, um den Abstand und die Neigung zu ermitteln. Ich denke, dieser ArUco-Marker ist ein ausgezeichneter Marker, der den Abstand zwischen dem Marker und der Kamera, die Richtung des Markers und die jedem zugewiesene Nummer (ID) leicht und genau erkennen kann, aber er ist nicht so berühmt ... ...? Ich werde eines Tages eine Drohnenausgabe schreiben.
Recommended Posts