ソフトバンクのPepperで3次元点群の取得（Choregraphe, Python） ref: http://qiita.com/SatoshiRobatoFujimoto/items/1b9a3f3e6ec672cce89f

file0.txt

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