ぼくフルスタックエンジニア。

フルにスタックされすぎて積んでる人のブログ

【ハンズオン】動画から歩いている位置を把握!OpenVSLAMを使ってスマホのカメラで自己位置推定を試してみた

みなさん、カメラの映像から空間を把握して自己位置をマッピングしたいと思ったことはありませんか? 自己位置推定ツールのOpenVSLAMを使えば動画から自分のいる場所を自動でマッピングできます。 特殊な器具はなくてOK! なんの変哲もないスマホなどの普通のカメラでできるんです。

f:id:gpioblink:20201104174641g:plain

最終的にこのような画面でマッピングを確認することができます。この記事では、カメラの下準備からこの画面を表示するまでをハンズオン形式で書いていきます。

必要なもの

ツールのインストール

まずはOpenVSLAMの環境を作っていきましょう。まずはDockerで用意していきます。画面出力をX11上で行なうopenvslam-desktopとWebブラウザ上でopenvslam-socketの2つが提供されていますが、今回はX関連のエラーを避けるためにWebブラウザ上で動くopenvslam-socketを選択します。

公式のチュートリアルを参考にしながら、次のコマンドを実行していきます。

OpenVSLAMの入手

まずはOpenVSLAMのリポジトリを落とします。

git clone git@github.com:xdspacelab/openvslam.git
cd openvslam

この場所(openvslamディレクトリ内)のことを以下では「プロジェクトルート」と表現します。

OpenVSLAMのビルド

次にOpenVSLAM本体のopenvslam-socketとそれをWebから見るためのツールopenvslam-serverをビルドします。

docker build -t openvslam-desktop -f Dockerfile.desktop . --build-arg NUM_THREADS=4
cd viewer
docker build -t openvslam-server .

OpenVSLAMの動作確認

ここまでで下準備は完了です。次にOpenVSLAM側が公開しているサンプル動画で動作を確認してみましょう。

検証用ファイルをダウンロード

開発者のGoogleドライブからaist_entrance_hall_1.zipをダウンロードし、プロジェクトルートの下にmyexampleというディレクトリを作りそこに展開してください。

drive.google.com

ボキャブラリファイルorb_vocab.dbow2もダウンロードし、同様に展開してください。

drive.google.com

OpenVSLAMの起動

2つのターミナルを開き、プロジェクトルートに移動してから次の2つのコンテナを起動します。 macOSの方は手順が異なるようなので公式ドキュメントを参照してください

Webサイト表示用

docker run --rm -it --name openvslam-server --net=host openvslam-server

OpenVSLAM処理用

vオプションでmyexample内のファイルをDocker内と同期させています。

docker run --rm -it --name openvslam-socket -v "myexample:/openvslam/myexample" --net=host openvslam-socket

テスト

まず、ブラウザから http://localhost:3001 を開きます。

次に「OpenVSLAM処理用」のコンソールで以下のコマンドを実行します。

./run_video_slam \
    -v ../myexample/orb_vocab/orb_vocab.dbow2 \
    -c ../myexample/aist_entrance_hall_1/config.yaml \
    -m ../myexample/aist_entrance_hall_1/video.mp4 \
    --frame-skip 3 \
    --map-db aist_living_lab_1_map.msg

この操作をして、ブラウザ上の画面にマッピングが表示されたらテストは成功です。 立てた2つのコンテナは終了してください。

f:id:gpioblink:20201105105415p:plain

カメラの準備

早速、手持ちのカメラで撮った動画で試したいですが、その前にカメラの固有値を設定する「キャリブレーション」という処理でカメラの内部パラメータを特定し設定する必要があります。

カメラの内部パラメータとは

内部パラメータとは、画素の大きさやカメラの中央からのズレ幅のことです。2次元の画像から3次元の位置を求める(3次元復元する)ためには、カメラの内部パラメータを知っておく必要があります。これをカメラ毎に正しく設定しないと、正しくマッピングできません。

いろいろ難しい計算が必要ですが、チェッカーボードを使えばOpenCVで自動計算できます。

ここでは、以下のサイトの内容に従ってチェッカーボードでキャリブレーションを行なっていきます。

labs.eecs.tottori-u.ac.jp

チェッカーボードの用意

このサイト中段にあるcheckerboard.pdf(13x9)を印刷して使いました。

A4の紙に印刷して、硬いものの上にピッタリ貼り付けてください。

w.atwiki.jp

キャリブレーション用写真の撮影

キャリブレーションでは最終的に画像を用いますが、写真モードと動画モードではカメラの内部パラメータが変化する可能性があるので、まずは動画を撮影しコマをキャプチャして使うのがおすすめです。

カメラを三脚やスマホスタンドなどで固定して、様々な角度から格子を撮影してください。

私は、撮影した動画ファイルをPCに転送した後、VLCスクリーンショット機能で20枚程度の写真を抽出しました。

f:id:gpioblink:20201105111737p:plain

OpenCVによるキャリブレーション

このサイトのチュートリアルに従って、写真を入れたディレクトリ上で以下のコードを実行しました。

import numpy as np
import cv2
import glob

# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((13*9,3), np.float32)
objp[:,:2] = np.mgrid[0:13,0:9].T.reshape(-1,2)

# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.

images = glob.glob('*.jpg')

for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

    # Find the chess board corners
    ret, corners = cv2.findChessboardCorners(gray, (13,9),None)

    # If found, add object points, image points (after refining them)
    if ret == True:
        objpoints.append(objp)

        corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
        imgpoints.append(corners2)

        # Draw and display the corners
        img = cv2.drawChessboardCorners(img, (7,6), corners2,ret)
        cv2.imshow('img',img)
        cv2.waitKey(500)

ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)
print("ret", ret)
print("mtx", mtx)
print("dist", dist)
print("rvecs", rvecs)
print("tvecs", tvecs)

fs = cv2.FileStorage('calibration_result.yaml', cv2.FILE_STORAGE_WRITE)
fs.write('intrinsic', mtx)
fs.write('distortion', dist)
fs.release()

cv2.destroyAllWindows()

実行中にはウィンドウが開き以下の様な画像がバンバン表示されて、最終的なキャリブレーションの結果がターミナル上に表示されます。

f:id:gpioblink:20201105112755p:plain

以下のようなcalibration_result.yamlが出ればキャリブレーション用の値取得は完了です。

%YAML:1.0
---
intrinsic: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 1.8921551800170096e+03, 0., 9.4035838675468960e+02, 0.,
       1.8900754975516727e+03, 5.3822505221063818e+02, 0., 0., 1. ]
distortion: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [ 2.9967145140423967e-01, -1.5951350392495047e+00,
       -2.4978008305846832e-04, -5.0818139267033513e-03,
       2.8435830856943536e+00 ]

OpenVSLAM用の設定ファイルを作成

このキャリブレーションした値をOpenVSLAM上に読み込ませるにはOpenVSLAM専用のフォーマットにする必要があります。

最終的には以下のようなconfig.yamlファイルを作ります。先程のOpenCVの実行結果でカメラ行列で出力されたデータを、OpenVSLAM用に必要な要素だけ抜き出して指定します。

#==============#
# Camera Model #
#==============#

Camera.name: "Pixel3 (Sony Exmor for mobile IMX363)"
Camera.setup: "monocular"
Camera.model: "perspective"

Camera.fx: 1892.1551800170096
Camera.fy: 1890.0754975516727
Camera.cx: 940.35838675468960
Camera.cy: 538.22505221063818

Camera.k1: 0.29967145140423967
Camera.k2: -1.5951350392495047 
Camera.p1: -0.00024978008305846832
Camera.p2: -0.0050818139267033513
Camera.k3: 2.8435830856943536

Camera.fps: 30.0
Camera.cols: 1920
Camera.rows: 1080

Camera.color_order: "RGB"

#================#
# ORB Parameters #
#================#

Feature.max_num_keypoints: 2000
Feature.scale_factor: 1.2
Feature.num_levels: 8
Feature.ini_fast_threshold: 20
Feature.min_fast_threshold: 7
Feature.mask_rectangles:
  - [0.0, 1.0, 0.0, 0.1]
  - [0.0, 1.0, 0.84, 1.0]

fx、fy、cx、cyの部分には、calibration_result.yamlのintrinsicのカメラ行列の該当する場所の値にします。

 \begin{bmatrix}f_x & 0 & c_x \\0 & f_y & c_y \\0 & 0 & 1 \end{bmatrix}

k1、k2、p1、p2、k3はcalibration_result.yamldistortionの順番でそのまま差し込みます。

他は必要に応じて名前やフレーム数、解像度の設定を行なってください。

設定が完了したら、<プロジェクトルート>/myexample/my_video_slam/config.yamlに保存してください。

カメラでの自己位置推定

それでは、ここまでで用意が整ったので、早速マッピングを行なってみましょう!

まずは適当な建物の中を歩いた動画を撮影してきてください。

その動画を<プロジェクトルート>/myexample/my_video_slam/video.mp4に置いてください。

あとはテストの時と同様にopenvslam-socketopenvslam-serverのコンテナを起動して、次のコマンドを実行すると。。。

./run_video_slam \
    -v ../myexample/orb_vocab/orb_vocab.dbow2 \
    -c ../myexample/my_video_slam/config.yaml \
    -m ../myexample/my_video_slam/video.mp4 \
    --frame-skip 3 \
    --map-db my_video_slam_map.msg

記事冒頭のgifのような任意の動画でのマッピングに成功しました!

f:id:gpioblink:20201104174641g:plain

まとめ

今回は、私が奥山先生とやっている研究からOpenVSLAMの使い方について紹介しました。

次回は、地図作成の方を書いていきたいと思います。

特殊な機材が何もいらない、夢が広がる楽しい技術なので、みなさんもぜひ活用してみてください!