[PYTHON] Nuage de points avec du poivre

introduction

Pepper a un capteur 3D. Pour accéder aux informations sur ce capteur [AL Segmentation 3D] de NAOqi (http://doc.aldebaran.com/2-1/naoqi/vision/alsegmentation3D.html) et ALCloseObjectDetectionといったAPIを経由してアクセスすることになるのですが、それぞれセグメンテーションと近接物体検知の用途に限定されており、凝った用途には使いづらいです。

D'autre part, une bibliothèque appelée PCL (Point Cloud Library) a récemment attiré l'attention en tant que bibliothèque qui gère des formes 3D pouvant être acquises par un capteur 3D. Dans cet article, je vais vous présenter comment traiter les données acquises par le capteur 3D de Pepper avec PCL.

PCL peut accéder à des capteurs 3D tels que Kinect via OpenNI pour obtenir Point Cloud, mais vous pouvez également créer des objets Point Cloud vous-même ou transmettre directement des fichiers Point Cloud (fichiers pcd). Je vais. Cette fois, il semblait difficile d'exécuter PCL directement sur Pepper, donc je vais obtenir l'image de profondeur du capteur 3D de Pepper avec un script Python sur mon PC local et la convertir en données de nuage de points. Après cela, essayez d'afficher Point Cloud sur le PCL du PC local.

Le code source complet est disponible sur GitHub. Le SDK Python de NAOqi est nécessaire pour fonctionner réellement. De plus, PCL ne sera pas expliqué ici, donc la sérialisation de DERiVE ["Let's touch PCL!"](Http://derivecv.tumblr.com/tagged/PCL%E3%82%92%E8%A7%A6 % E3% 81% A3% E3% 81% A6% E3% 81% BF% E3% 82% 88% E3% 81% 86% EF% BC% 81), etc.

Obtenir une image en profondeur

L'image en profondeur peut être obtenue comme les autres caméras 2D en utilisant «AL Video Device».

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)

Pour obtenir l'image du capteur 3D, spécifiez 2 pour l'ID de caméra spécifié par ʻALVideoDevice.subscribeCamera () `. Ces définitions de constantes se trouvent dans le chapitre "Pepper-3D Sensor" de la documentation qui accompagne Choregraphe. Par exception, la définition de l'espace colorimétrique = 17 n'est pas décrite dans le document, mais si vous spécifiez 17, vous pouvez obtenir une image dans laquelle chaque pixel est représenté par 16 bits sur l'échelle de gris. Ceci est basé sur Code source du module ROS pour Romeo.

Conversion d'une image de profondeur en nuage de points

Les paramètres de la caméra sont nécessaires pour convertir une image en profondeur en nuage de points, un ensemble de coordonnées 3D. Les paramètres de la caméra sont des paramètres qui représentent les caractéristiques de la caméra, telles que la distance focale de la caméra et le centre de l'image. Cette fois, la caméra est fixée par Pepper's Xtion, comme avant, code source ROS Utilise la valeur connue utilisée dans.

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

Tout ce que vous avez à faire est de convertir chaque pixel de l'image de profondeur en coordonnées 3D. Notez que chaque pixel est divisé en 2 octets supérieur et inférieur.

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))

Sortie de fichier PCD

Je sortirai le dernier nuage de points acquis sous forme de fichier PCD. Ici, l'en-tête du fichier PCD et les données réelles sont simplement sorties sous forme de texte.

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()

Vous l'avez maintenant enregistré en tant que fichier PCD.

Afficher les fichiers PCD

Maintenant, lorsque vous affichez le fichier PCD avec le visualiseur de PCL, etc., vous pouvez voir que la forme 3D peut être reproduite comme indiqué ci-dessous.

Pour référence, les photos prises avec la caméra 2D de Pepper au même moment sont les suivantes.

Résumé

J'ai présenté comment obtenir Point Cloud avec la caméra 3D de Pepper. Si vous cuisinez bien cela avec PCL, vous pouvez faire quelque chose d'intéressant!