[PYTHON] Punktwolke mit Pfeffer

Einführung

Pfeffer hat einen 3D-Sensor. Zugriff auf die Informationen zu diesem Sensor NAOqis AL Segmentation 3D und ALCloseObjectDetectionといったAPIを経由してアクセスすることになるのですが、それぞれセグメンテーションと近接物体検知の用途に限定されており、凝った用途には使いづらいです。

Andererseits hat eine Bibliothek namens PCL (Point Cloud Library) kürzlich als Bibliothek Aufmerksamkeit erregt, die 3D-Formen verarbeitet, die von einem 3D-Sensor erfasst werden können. In diesem Artikel werde ich vorstellen, wie die vom 3D-Sensor von Pepper mit PCL erfassten Daten verarbeitet werden.

PCL kann über OpenNI auf 3D-Sensoren wie Kinect zugreifen, um Point Cloud zu erhalten. Sie können jedoch auch Point Cloud-Objekte selbst erstellen oder Point Cloud-Dateien (PCD-Dateien) direkt übergeben. Ich werde. Dieses Mal schien es schwierig zu sein, PCL direkt auf Pepper auszuführen, daher werde ich das Tiefenbild vom Pepper-3D-Sensor mit einem Python-Skript auf meinem lokalen PC abrufen und in Point Cloud-Daten konvertieren. Versuchen Sie danach, Point Cloud auf der PCL des lokalen PCs anzuzeigen.

Der vollständige Quellcode ist auf [GitHub] verfügbar (https://github.com/thorikawa/pcl-pepper/blob/master/get_pcd.py). Das Python SDK von NAOqi ist erforderlich, um tatsächlich zu funktionieren. Außerdem wird PCL hier nicht erklärt, daher die DERiVE-Serialisierung "Lassen Sie uns PCL berühren!" % E3% 81% A3% E3% 81% A6% E3% 81% BF% E3% 82% 88% E3% 81% 86% EF% BC% 81) usw.

Tiefenbild abrufen

Das Tiefenbild kann wie andere 2D-Kameras mit "ALVideoDevice" aufgenommen werden.

NAME = "depth_camera"
CAMERA_ID = 2 # depth
RESOLUTION = 1 # 320*240
FRAMERATE = 15
COLOR_SPACE = 17 # mono16

video = ALProxy('ALVideoDevice', IP, PORT)
client = video.subscribeCamera(NAME, CAMERA_ID, RESOLUTION, COLOR_SPACE, FRAMERATE)

Um das Bild vom 3D-Sensor abzurufen, geben Sie 2 für die in ALVideoDevice.subscribeCamera () angegebene Kamera-ID an. Diese konstanten Definitionen finden Sie im Kapitel "Pepper-3D-Sensor" der Dokumentation zu Choregraphe. Ausnahmsweise wird die Definition von Farbraum = 17 im Dokument nicht beschrieben. Wenn Sie jedoch 17 angeben, können Sie ein Bild erhalten, in dem jedes Pixel auf der Graustufe durch 16 Bit dargestellt wird. Dies basiert auf Quellcode für ROS-Modul für Romeo.

Vom Tiefenbild in Punktwolke konvertieren

Kameraparameter sind erforderlich, um ein Set von 3D-Koordinaten von Tiefenbild in Punktwolke zu konvertieren. Kameraparameter sind Parameter, die die Eigenschaften der Kamera darstellen, z. B. die Brennweite der Kamera und die Bildmitte. Dieses Mal wird die Kamera von Peppers Xtion repariert, also wie zuvor ROS-Quellcode ) Verwendet den bekannten Wert, der in verwendet wird.

# Focal length
FX = 525.0 / 2
FY = 525.0 / 2
# Optical center
CX = 319.5 / 2
CY = 239.5 / 2

Sie müssen lediglich jedes Pixel des Tiefenbilds in 3D-Koordinaten konvertieren. Beachten Sie, dass jedes Pixel in obere und untere 2 Bytes unterteilt ist.

cloud = []
for v in range(height):
	for u in range(width):
		offset = (v * width + u) * 2
		depth = ord(array[offset]) + ord(array[offset+1]) * 256 

		x = (u - CX) * depth * UNIT_SCALING / FX
  		y = (v - CY) * depth * UNIT_SCALING / FY
  		z = depth * UNIT_SCALING

  		cloud.append((x, y, z))

PCD-Dateiausgabe

Ich werde die zuletzt erworbene Point Cloud als PCD-Datei ausgeben. Hier werden der Header der PCD-Datei und die eigentlichen Daten einfach als Text ausgegeben.

header = '''# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH %d
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS %d
DATA ascii'''

f = open(OUTPUT_FILE, 'w')
num = len(cloud)
f.write(header % (num, num))
f.write("\n")
for c in cloud:
	f.write('%f %f %f' % (c[0], c[1], c[2]))
	f.write("\n")
f.close()

Sie haben es jetzt als PCD-Datei gespeichert.

PCD-Dateien anzeigen

Wenn Sie nun die PCD-Datei mit dem Visualizer von PCL usw. anzeigen, können Sie sehen, dass die 3D-Form wie unten gezeigt reproduziert werden kann.

Als Referenz sind die Fotos, die mit Peppers 2D-Kamera zum gleichen Zeitpunkt aufgenommen wurden, wie folgt.

Zusammenfassung

Ich habe vorgestellt, wie man mit Peppers 3D-Kamera Point Cloud erhält. Wenn Sie so gut mit PCL kochen, können Sie etwas Interessantes tun!