ソフトバンクのPepperで3次元点群の取得（Choregraphe, Python）

2015/03/01 アルデバラン・アトリエ秋葉原　Pepper開発タッチ＆トライの記録

Pepperの目である3Dカメラ（XtionのDepthセンサ）を使って、3次元点群を取得するボックスを作成してみました。
参考記事：thorikawaさんの[PepperくんでPoint Cloud](http://qiita.com/thorikawa/items/a625b4766dcd283b783b#pcd%E3%83%95%E3%82%A1%E3%82%A4%E3%83%AB%E3%81%AE%E5%87%BA%E5%8A%9B)

# getPointCloudボックスの作成
Choregraphe 2.1.2を起動し、Pythonスクリプトのボックスを作成します。右クリック→ボックスの新規作成→Pythonスクリプト、または、ボックスライブラリのstandard→Templates→Python Script。
![p1.png](https://qiita-image-store.s3.amazonaws.com/0/63863/8380222f-3448-93f6-a346-cd4372400d8b.png)

ボックスの名前と概要を記述し、OKボタンを押してください。
![p2.png](https://qiita-image-store.s3.amazonaws.com/0/63863/4ffa4154-eed3-e624-123f-3a2bcfe83a4b.png)

ボックスを右クリックし、Pythonスクリプトを編集します。
![p3.png](https://qiita-image-store.s3.amazonaws.com/0/63863/247a85a2-1f19-ab2d-7a7f-a59ad1acf262.png)

次のように、getPointCloud関数を定義します。

~~~
class MyClass(GeneratedClass):
    def __init__(self):
        GeneratedClass.__init__(self)

    def onLoad(self):
        #put initialization code here
        pass

    def onUnload(self):
        #put clean-up code here
        pass

    def getPointCloud(self):
        import argparse
        import Image
        import time

        # Camera parameters (only tested on Pepper)
        # Focal length
        FX = 525.0 / 2
        FY = 525.0 / 2
        # Optical center
        CX = 319.5 / 2
        CY = 239.5 / 2

        # millimeter to meter
        UNIT_SCALING = 0.001

        NAME = "depth_camera"
        CAMERA_ID = 2 # depth
        RESOLUTION = 1 # 320*240
        FRAMERATE = 15
        COLOR_SPACE = 17 # mono16 Note: this is not documented as of Dec 14, 2014

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

        try:
            image = video.getImageRemote(client)

            if image is None:
                print 'Cannot obtain depth image.'
                exit()

            width = image[0]
            height = image[1]
            array = image[6]
            
            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))
        finally:
            video.unsubscribe(client)

        fileName = '/home/nao/recordings/cameras/cloud%f.ply' % time.time()

        f = open(fileName, 'w')
        num = len(cloud)
        header = '''ply
 format ascii 1.0
 comment Pepper 3D generated
 element vertex %d
 property float x
 property float y
 property float z
end_header
'''

        f.write(header % (width*height))
        f.write("\n")
        for c in cloud:
            f.write('%f %f %f' % (c[0], c[1], c[2]))
            f.write("\n")
        f.close()
        pass

    def onInput_onStart(self):
        #self.onStopped() #activate the output of the box
        self.getPointCloud()
        pass

    def onInput_onStop(self):
        self.onUnload() #it is recommended to reuse the clean-up as the box is stopped
        self.onStopped() #activate the output of the box
~~~

#実行

ロボットにアップロードして再生（F5）し、
3次元点群をply形式でPepperのローカルへ保存します。

線で繋いだものが最初に実行される。
![p4.PNG](https://qiita-image-store.s3.amazonaws.com/0/63863/37e0fa0a-2cf1-02e1-1ad7-9b53640ab91e.png)

線で繋がなくてもロボットにアップロードされたあとは、入力のところ（再生ボタン）をクリックすると、単体で実行できる。
![p5.PNG](https://qiita-image-store.s3.amazonaws.com/0/63863/b0a06eab-6a53-32c0-f959-b9deb7a69ef7.png)

#点群ファイルのダウンロード
メニューバーから接続→上級者向け→ファイルの転送をクリック
ID:nao Password:nao
でPepperのローカルフォルダが見れます。

recordings/cameras/の中に、点群のファイルが保存されているので、ダウンロードしましょう。
![c1.png](https://qiita-image-store.s3.amazonaws.com/0/63863/3b29f2e4-d655-ee04-9b02-7de64792f6b4.png)

![c2.png](https://qiita-image-store.s3.amazonaws.com/0/63863/e83c12ae-f196-c7e0-b771-65d144bba0cb.png)

![c3.png](https://qiita-image-store.s3.amazonaws.com/0/63863/e66a0468-da1b-904f-f37b-a4fce60217b9.png)

![c4.png](https://qiita-image-store.s3.amazonaws.com/0/63863/7d9d9f40-bbbe-c8ec-5a91-dfe29cab83fb.png)

#点群の表示
[Meshlab](http://meshlab.sourceforge.net/)で開くと、3次元点群が確認できます。
![p7.PNG](https://qiita-image-store.s3.amazonaws.com/0/63863/34b07bb1-8274-8f64-b378-c1637f891081.png)

#まとめ
Pepperの3Dカメラを使って、3次元点群をplyで保存、ローカルからダウンロード、Meshlabで表示できました。
次は、Depthの生データ(16bit, png画像)を利用し、地図作成と自己位置推定を同時に行うSLAMをしたい。
目標軌道（move_traj.pmt）を走行時のオドメトリ（加速度）やRGB(カラー)、Depth(深度)のデータセットを作る。

#補足
SLAMは、ロボットをある軌道で自律移動させたいときや走行環境を理解させたいときに用います。エンコーダや加速度センサなどの内界センサよりも精度がよく、信頼できます。

3Dカメラは、平面抽出や物体検出、顔認識などにも応用できます。
Pepperには2台の2Dカメラ（RGBカメラ)があるので、ステレオカメラにすれば点群も得られますし（共通に見えている部分のみ、あるかな？ｗ）、3Dカメラから得られる点群に色づけしたりもできます。

※Pepperさんを使わないときはいつもRestしておきましょう。延命のために。笑