소개

Pepper-kun에는 3D 센서가 탑재되어 있습니다. 이 센서의 정보에 액세스하려면,
NAOqi의 ALSegmentation3D이나 ALCloseObjectDetection 등의 API를 경유해 액세스하게 됩니다만, 각각 세그멘테이션과 근접 물체 검지의 용도에 한정되어 있어, 정교한 용도에는 사용하기 어렵습니다.

한편 3D 센서로 얻을 수 있는 3차원 형상을 취급하는 라이브러리로서 최근 주목을 받고 있는 것이 PCL(Point Cloud Library)이라는 라이브러리입니다. 이 기사에서는 Pepper-kun의 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-kun의 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-kun의 3D 카메라로 Point Cloud를 얻는 방법을 소개했습니다. 이것을 PCL로 잘 요리해 주면 재미있을 수 있을 것 같네요!