はじめに
Pepperくんには3Dセンサーが搭載されています。このセンサーの情報にアクセスするには、
NAOqiのALSegmentation3DやALCloseObjectDetectionといったAPIを経由してアクセスすることになるのですが、それぞれセグメンテーションと近接物体検知の用途に限定されており、凝った用途には使いづらいです。
一方で3Dセンサーで取得できる3次元形状を扱うライブラリとして、最近注目を集めているのがPCL（Point Cloud Library）というライブラリです。この記事では、Pepperくんの3Dセンサーで取得するデータをPCLで処理する方法について紹介します。
PCLはKinectなどの3DセンサーにOpenNI経由でアクセスしてPoint Cloudを取得できますが、その他にも自前でPoint Cloudのオブジェクトを構築したり、Point Cloudのファイル(pcdファイル）を直接渡したりもできます。今回はPepperくんの上で直接PCLを動かすのは難しそうだったので、ローカルPCのPythonスクリプトでPepperくんの3DセンサーからDepth Imageを取得し、それをPoint Cloudデータに変換します。その後にローカルPCのPCLで、Point Cloudを表示してみます。
フルのソースコードはGitHubに上げてます。実際に動かすにはNAOqiのPython SDKが必要です。またPCLについてはここでは解説しないので、 DERiVEさんの連載「PCLを触ってみよう！」などを参考にしてください。
Depth Imageの取得
Depth ImageはALVideoDeviceを利用して、他の2Dカメラと同様に取得できます。
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)
3Dセンサーから画像を取得するにはALVideoDevice.subscribeCamera()で指定するCamera Idに2を指定します。これらの定数定義は、Choregraphe付属のドキュメントの「Pepper - 3D Sensor」の章に記載されています。例外として、Color Space=17の定義はドキュメントに記載されていないのですが、17を指定するとGray Scaleで各画素が16ビットで表された画像が取れます。これはRomeo用のROSモジュールのソースコードを参考にしています。
Depth ImageからPoint Cloudへの変換
Depth ImageからPoint Cloud、つまり3次元座標の集合に変換するにはカメラ・パラメータが必要です。カメラ・パラメータはカメラの焦点距離や画像中心といったカメラの特性を表すパラメータです。今回はPepperくんのXtionでカメラは固定されているので、先ほど同様、ROSのソースコードで使われている既知の値を使います。
# Focal length
FX = 525.0 / 2
FY = 525.0 / 2
# Optical center
CX = 319.5 / 2
CY = 239.5 / 2
あとはDepth Imageの各画素を3次元座標に変換してあげるだけです。各画素が上位と下位の2バイトに分かれていることに注意します。
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ファイルの出力
最後に取得したPoint CloudをPCDファイルとして出力してあげます。ここでは単純にPCDファイルのヘッダと実データをテキストとして出力します。
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()
これでPCDファイルとして保存できました。
PCDファイルの表示
これでPCDファイルをPCLのVisualizerなどで表示すると、以下のように3次元形状が再現できていることがわかります。
参考までに同じタイミングでPepperくんの2Dカメラで撮った写真は以下。
まとめ
Pepperくんの3DカメラでPoint Cloudを取得する方法を紹介しました。これをPCLでうまく料理してやれば面白いことができそうですね！