UbuntuでIntel RealSense T265を動かしてみた

背景

RealSenseとはIntelが開発している深度や位置情報を取得できるカメラのシリーズ名です。
T265とは、2019年4月に発売された、魚眼レンズ付きカメラ2つを使って自己位置推定を行うRealSenseです。

どのような使い方が出来るか気になったので、買って試してみました。
備忘録を兼ねて、試した内容を共有します。

使ったもの

RealSense T265

今回試すカメラです。

ステレオ魚眼カメラとジャイロセンサを利用して自己位置推定をしてくれます。
ステレオカメラですが、深さの計算はしてくれないようです。

Intelのオンラインストアやスイッチサイエンスで購入できます。

今回はIntelのオンラインストアで購入しました。
購入額は 本体$199 + 送料$24 + 税金と手数料約2000円（配達時にDHLから請求されました）、合計約28000円でした。

githubの製品情報ページはこちらです。

IntelRealSense/librealsense/doc/t265.md

Ubuntu18.04をインストールしたPC

UbuntuというLinuxのディストリビューションをインストールしたPCでプログラムを試します。

Ubuntuのダウンロードページはこちらです。

Download Ubuntu Desktop

RealSenseのライブラリをインストール

RealSenseの開発情報は、下記のページから辿れます。

Get Started with Intel RealSense

RealSenseのライブラリlibrealsenseという名前でgithubで公開されています。

IntelRealSense/librealsense

Linux用のインストール手順があるので、それに従ってインストールしました。

IntelRealSense/librealsense/doc/distribution_linux.md

Ubuntu18.04で実行したコマンドはこちらです。
（バージョンが上がると下記のコマンドではインストールできなくなると思うので、インストール手順は上記のページを参考にして、下記のコマンドは雰囲気だけ参考にしてください。）

sudo apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo bionic main" -u
sudo apt install librealsense2-dkms librealsense2-utils librealsense2-dev

サンプルアプリケーションを動かしてみる

librealsense2-utilsをインストールしたらrealsense-viewerが実行できるようになります。

T265をPCに接続して下記のコマンドを実行します。

realsense-viewer

操作画面が表示されます。

3Dモード（画面右上）でトラッキングを有効（画面左側）にすると、T265での自己位置推定結果を表示できます。

T265を移動すると、推定される自己位置も移動します。

2Dモードにすると、T265から取得できる、左右のカメラの白黒画像、加速度、ジャイロ、現在の推定位置と姿勢、それぞれを表示できます。

ROSのラッパーをインストールして、サンプルプログラムを動かしてみる

ROSとはロボットのプログラムを使いやすくしてくれるフレームワークです。
realsenseのROSのラッパーがあるので、それをインストールしてT265用のプログラムを実行してみます。

IntelRealSense/realsense-ros

（ROS2用のラッパー（intel/ros2_intel_realsense）もありますが、自分がまだROS2に不慣れなので、今回はROS1用だけ試します。）

動作確認にはROSのmerodicを利用しました。

readmeではkinetic用と記載されていますが、merodicでもcatkin_makeしてrs_t265.launchを実行できました。
（demo_t265.launchは実行できるものの、エラーが発生してrviz上に情報が表示されませんでした。）

コードを配置してビルドしてROSに読み込ませます。
2019.10.27に試したときはddynamic_reconfigureが無いとビルドが失敗したので、それも一緒に配置します。（aptでインストールするddynamic_reconfigureではビルドできませんでした。）

cd [catkin work space]/src
git clone https://github.com/IntelRealSense/realsense-ros.git
git clone https://github.com/pal-robotics/ddynamic_reconfigure.git
cd ../
catkin_make
source devel/setup.bash

rs_t265.launchを実行します。
（USBハブを挟むと動作が不安定になることがあったので、不安定な場合はPCにT265を直接接続するのが良いと思います。）

roslaunch realsense2_camera rs_t265.launch

上記のコマンド実行後に確認できるtopicはこちらです。

rostopic list

/camera/accel/imu_info
/camera/accel/sample
/camera/fisheye1/camera_info
/camera/fisheye1/image_raw
/camera/fisheye2/camera_info
/camera/fisheye2/image_raw
/camera/gyro/imu_info
/camera/gyro/sample
/camera/odom/sample
/camera/realsense2_camera_manager/bond
/camera/tracking_module/parameter_descriptions
/camera/tracking_module/parameter_updates
/diagnostics
/rosout
/rosout_agg
/tf
/tf_static

別のターミナルで推定された位置を配信している/tfの内容を確認します。

rostopic echo /tf

---
transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1556510463
        nsecs: 115720272
      frame_id: "camera_odom_frame"
    child_frame_id: "camera_pose_frame"
    transform: 
      translation: 
        x: 0.333742678165
        y: 0.438069999218
        z: -0.696598947048
      rotation: 
        x: 0.0623288042843
        y: 0.0751170888543
        z: 0.578197062016
        w: -0.81003755331
---

ROSで推定された位置を確認できました。

pyrealasense2をインストールして、pythonのサンプルプログラムを動かしてみる

pythonとはビルドせずに動かせるプログラミング言語のひとつです。
pyrealsense2というlibrealsense2のpythonラッパーがあるので、それをインストールしてサンプルプログラムを動かしてみます。
2が付いていないpyrealsenseはv1のSDK用なので、2が付いている方を利用します。

IntelRealSense/librealsense/wrappers/python

python3とpython3のパッケージマネージャーであるpipはaptコマンドでインストールできます。

sudo apt install python3-pip

librealsense2-devをインストールしたPCに、下記のコマンドでpyrealsense2をインストールできます。

pip3 install pyrealsense2

T265用のサンプルプログラムがgithubにあるので、tmpディレクトリにダウンロードして動かしてみます。

cd /tmp
wget https://raw.githubusercontent.com/IntelRealSense/librealsense/master/wrappers/python/examples/t265_example.py
python3 t265_example.py

位置、速度、加速度を取得できました。
たまにT265が認識されないことがありましたが、USBを抜き差ししたら認識されました。

Frame #49
Position: x: 0.000111191, y: -0.000166429, z: 0.000447679
Velocity: x: -0.00280558, y: -0.000483674, z: -0.00434761
Acceleration: x: -0.0514255, y: 0.0337114, z: -0.0892147

魚眼ステレオカメラの画像を処理して視差を計算してみる

T265は深さを計算してくれないものの、視差計算に利用できる魚眼カメラ2個からの白黒画像を扱えるので、それをキャリブレーション（画像の歪みを取る処理）無しで利用して大まかに深さを計算するプログラムを作ってみました。

	#!/usr/bin/python
	# -*- coding: utf-8 -*-
	# First import the library
	import pyrealsense2 as rs
	import numpy as np
	import cv2
	# setup opencv stereo
	minDisp = 0
	numDisp = 64 - minDisp
	windowSize = 5
	stereo = cv2.StereoSGBM_create(
	minDisparity = minDisp,
	numDisparities = numDisp,
	blockSize = 16,
	P1 = 8*3*windowSize**2,
	P2 = 32*3*windowSize**2,
	disp12MaxDiff = 1,
	uniquenessRatio = 10,
	speckleWindowSize = 100,
	speckleRange = 32
	)
	# Declare RealSense pipeline, encapsulating the actual device and sensors
	pipe = rs.pipeline()
	# Build config object and request pose data
	cfg = rs.config()
	cfg.enable_stream(rs.stream.fisheye, 1)
	cfg.enable_stream(rs.stream.fisheye, 2)
	# Start streaming with requested config
	pipe.start(cfg)
	print('press q to quit this program')
	try:
	while True:
	# Wait for the next set of frames from the camera
	frames = pipe.wait_for_frames()
	# Get images
	# fisheye 1: left, 2: right
	fisheye_left_frame = frames.get_fisheye_frame(1)
	fisheye_right_frame = frames.get_fisheye_frame(2)
	fisheye_left_image = np.asanyarray(fisheye_left_frame.get_data())
	fisheye_right_image = np.asanyarray(fisheye_right_frame.get_data())
	# Calculate disparity
	width = fisheye_left_frame.get_width()
	height = fisheye_left_frame.get_height()
	x1 = int(width/3 - numDisp / 2)
	x2 = int(width*2/3 + numDisp / 2)
	y1 = int(height/3)
	y2 = int(height*2/3)
	rect_left_image = fisheye_left_image[y1:y2, x1:x2]
	rect_right_image = fisheye_right_image[y1:y2, x1:x2]
	disparity = stereo.compute(rect_left_image, rect_right_image).astype(np.float32)/16
	disparity = (disparity - minDisp) / numDisp
	# Display images
	cv2.rectangle(fisheye_left_image, (x1, y1), (x2, y2), (255,255,255), 5)
	cv2.rectangle(fisheye_right_image, (x1, y1), (x2, y2), (255,255,255), 5)
	cv2.imshow('fisheye target', np.hstack((fisheye_left_image, fisheye_right_image)))
	cv2.imshow('disparity', disparity)
	key = cv2.waitKey(1)
	if key == ord('q'):
	break
	finally:
	pipe.stop()

view raw t265_stereo_depth.py hosted with ❤ by GitHub

OpenCVを利用するので、pipでインストールしてください。

pip3 install opencv-python

OpenCVとpyrealsense2をインストールしたPCで下記ようなコマンドを実行すると、/tmpディレクトリにプログラムをダウンロードして実行できます。

cd /tmp
wget -O t265_stereo_depth.py https://gist.githubusercontent.com/asukiaaa/07508c5242e1bdce57e789574ba26f5f/raw
python3 t265_stereo_depth.py

魚眼カメラの画像の中央の白で囲んだ部分の視差を計算しています。

ステレオカメラとして精度良く作られているためか、キャリブレーションをしなくてもまあまあ視差が取れました。

「q」を入力するとプログラムが終了します。

まとめ

RealSenseT265についてこれらのことが分かりました。

これらの情報が取得できます。

• 加速度
• 角速度
• 左右の魚眼レンズ付きカメラの白黒画像
• 推定した自己位置 <- これがT265の売り

上記の情報はサンプルアプリケーション（realsense-viewer）で確認できます。

Ubuntu18.04上のROSとpythonで動かせることを確認できました。
他のツールとの連携はREADMEのWhat’s included in the SDKを参照してください。

標準機能では視差は計算してくれないものの、ステレオカメラとしての精度が良いため、ステレオカメラの画像から視差を計算することで、まあまあ使えそうな視差情報を取得できました。

共有する情報は以上です。

変更履歴

2019.10.27
インストールコマンドを最新版に更新しました。
ros用のrealsenseのラッパーのurlが変わっていたので、更新しました。
realsense-rosのインストールにddynamic_reconfigureが必要になっていたので、cloneコマンドを追加しました。