Point Cloud with Pepper

Introduction

Pepper has a 3D sensor. To access the information on this sensor NAOqi's AL Segmentation 3D and ALCloseObjectDetectionといったAPIを経由してアクセスすることになるのですが、それぞれセグメンテーションと近接物体検知の用途に限定されており、凝った用途には使いづらいです。

On the other hand, a library called PCL (Point Cloud Library) has recently been attracting attention as a library that handles 3D shapes that can be acquired by 3D sensors. In this article, I will introduce how to process the data acquired by Pepper's 3D sensor with PCL.

PCL can access 3D sensors such as Kinect via OpenNI to get Point Cloud, but you can also build Point Cloud objects yourself or pass Point Cloud files (pcd files) directly. I will. This time it seemed difficult to run PCL directly on Pepper, so I will get a Depth Image from Pepper's 3D sensor with a Python script on my local PC and convert it to Point Cloud data. After that, try to display the Point Cloud on the PCL of the local PC.

The full source code is available on GitHub. NAOqi's Python SDK is required to actually run it. Also, PCL will not be explained here, so DERiVE's serialization ["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.

Get Depth Image

Depth Image can be obtained like other 2D cameras by using ʻ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)

To get the image from the 3D sensor, specify 2 for the Camera Id specified by ʻALVideoDevice.subscribeCamera ()`. These constant definitions can be found in the "Pepper-3D Sensor" chapter of the documentation that accompanies Choregraphe. As an exception, the definition of Color Space = 17 is not described in the document, but if you specify 17, you can get an image in which each pixel is represented by 16 bits in Gray Scale. This is based on the ROS module source code for Romeo.

Converting Depth Image to Point Cloud

Camera parameters are required to convert from a Depth Image to a Point Cloud, a set of 3D coordinates. Camera parameters are parameters that represent the characteristics of the camera, such as the focal length of the camera and the center of the image. This time, the camera is fixed by Pepper's Xtion, so as before, ROS source code Uses the known value used in.

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

All you have to do now is convert each pixel of the Depth Image to 3D coordinates. Notice that each pixel is divided into upper and lower 2 bytes.

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 file output

I will output the last acquired Point Cloud as a PCD file. Here, the header of the PCD file and the actual data are simply output as text.

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

You can now save it as a PCD file.

View PCD files

Now, when you display the PCD file with PCL's Visualizer etc., you can see that the 3D shape can be reproduced as shown below.

Screen Shot 2014-12-14 at 10.01.11 PM.png

For reference, the photos taken with Pepper's 2D camera at the same timing are as follows.

image0.png

Summary

I introduced how to get Point Cloud with Pepper's 3D camera. If you cook this well with PCL, you can do something interesting!

Recommended Posts

Point Cloud with Pepper
Acquisition of 3D point cloud with Softbank Pepper (Choregraphe, Python)
Image matching & point cloud alignment with PatchMatch
Debug with PEPPER python interpreter
Enhanced vocabulary with Cloud Text-to-Speech
[Package cloud] Manage python packages with package cloud
Run XGBoost with Cloud Dataflow (Python)
Change point detection with Kalman filter
Open3D basics and point cloud voxels
Run the IDCF cloud CLI with Docker
Streaming speech recognition with Google Cloud Speech API
Use Tabpy with Cloud Run (on GKE)
Try using Python with Google Cloud Functions
Deploy functions with Cloud Pak for Data
PointNet theory and implementation (point cloud data)
[GCP] Operate Google Cloud Storage with Python
Visualize point P that works with Python
Text mining with Python ② Visualization with Word Cloud
Connect to pepper with PEPPER Mac's python interpreter
Transcribe WAV files with Cloud Speech API
Handle structured logs with GCP Cloud Logging
[Verification] Try to align the point cloud with the optimization function of pytorch Part 1