ReactNative+Expoでジャイロセンサーと加速度センサーを使って姿勢を推定し、WebSocketでPCに送信して、OpenGLで可視化する

やりたいこと

WebSocketとPyOpenGLは姿勢の精度を視覚的に確認したかったのでやりました。姿勢推定はオイラー角、回転行列、クォータニオンなど色々ありますが、今回はクォータニオンを使います。また、姿勢の補正(ジャイロと加速度の合成？？）はとりあえず一番簡単であろう相補フィルターを使います。OpenGLについてはあまり書かないのでご了承願います。。。

追記(2018/3/6): 姿勢の推定はExpoのDeviceMotionを使うことで簡単に実装することができるようです

f:id:tsupiano:20190304212709p:plain

Pythonによるデータ受信と可視化

ソースコードを貼っておきます。
Gyroscope Tester · GitHub

姿勢推定のクォータニオン周りは出来るだけ視覚的に確認しながら作りたいのでサーバーサイドからやります。

WebSocket

WebSocketは少ないデータ量でリアルタイムに通信するためのプロトコルです。今回はリアルタイムに通信を行いたいのでこれを使います。実装は今回のような一時的に使うような凝ったものでなければ、詳細を知らなくてもできると思いますが、一応WebSocketとはなんぞや的なものが書いてある記事を下に貼っておきます。OpenGLとWebSocketをメインスレッドで同時に行えない（？）ので別スレッドを立てて行いました。（これが正しいかはわかりませんが一応できたので良しとしています...）

WebSocketのインストール

sudo pip3 install websocket-server

サーバー（描画側）のコード

from websocket_server import WebsocketServer
import threading

def new_client(client, server):
   print("connected")

def received_quaternion(client, server, message):
  # クォータニオンから回転軸と回転角を計算し、その値を更新する処理
  # 送られてくるデータはmessageに格納されている


def buildServer():
  # サーバーのIPアドレスとポート番号を引数にしてWebsocketServerクラスを生成
  # IPは各自で調べて入力してください
  server = WebsocketServer(9999, host='192.168.1.6')
  # 新しい接続があったときの関数を設定
  server.set_fn_new_client(new_client) 
  # データを受け取ったときの関数を設定
  server.set_fn_message_received(received_quaternion) 
  # サーバーを立ち上げる
  server.run_forever()

if __name__ == "__main__":
  threadServer = threading.Thread(target=buildServer)
  threadServer.start()

動作確認

wscatを用いることでクライアントやサーバーがない状態でも動作確認を行うことができます。まずはインストールします。

sudo apt-get install npm
sudo npm install -g wscat

次に以下のコマンドで起動します。IPアドレスはプログラム内で使用したものを入力します。

wscat -c ws://192.168.1.6:9999

"connect"で出力されればコネクションの確立が完了しています。
qiita.com
www.raspberrypirulo.net

PyOpenGL

最近はglutではなくglfwを使うのが流行りらしいですが、スマホのセンサーのテスト用に作るプログラムで、学習コストを割きたくないので、glutを使います。

PyOpenGLとGLUTのインストール

次のリンクを頼りにインストールしてください。
tokoik.github.io

プログラム

from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *

rotationValue = 90
rotationVector = [1, 0, 0]

def display():
    glEnable(GL_DEPTH_TEST)
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)

    glPushMatrix()
    
    glRotatef(rotationValue*180/math.pi, rotationVector[0],rotationVector[1],rotationVector[2])

    glMaterialfv(GL_FRONT, GL_DIFFUSE, [1.0, 0.0, 0.0,1.0])
    glColor3f(1.0, 1.0, 1.0)
    glutSolidCube(0.5)
    glColor3f(0.0, 0.0, 0.0)
    glutWireCube(0.5)
    glDisable(GL_LIGHTING)

    # Axis
    glColor3f(0.0, 0.0, 1.0)
    glBegin(GL_LINES)
    glVertex3f( 999.0, 0.0, 0.0)
    glVertex3f(-999.0, 0.0, 0.0)
    glEnd()
    glColor3f(0.0, 1.0, 0.0)
    glBegin(GL_LINES)
    glVertex3f( 0.0,  999.0, 0.0)
    glVertex3f( 0.0, -999.0, 0.0)
    glEnd()
    glColor3f(1.0, 0.0, 0.0)
    glBegin(GL_LINES)
    glVertex3f( 0.0, 0.0,  999.0)
    glVertex3f( 0.0, 0.0, -999.0)
    glEnd()

    glPopMatrix()

    glFlush()

def timer(value):
  glutPostRedisplay()
  glutTimerFunc(10, timer, 0)

def pyMain():  
  glutInit(sys.argv)
  glutInitWindowSize(600, 600)
  glutInitDisplayMode(GLUT_RGBA | GLUT_DEPTH)
  glutCreateWindow(b"OpenGL")
  
  glMatrixMode(GL_PROJECTION)
  glLoadIdentity()
  glMatrixMode(GL_MODELVIEW)
  
  glutDisplayFunc(display)
  glutTimerFunc(10, timer, 0)
  glClearColor(0.0, 0.0, 0.0, 1.0)
  glutMainLoop()

if __name__ == "__main__":
  pyMain()

PyOpenGLに関しては何も書いてないのですが、OpenGLとほとんど同じ（セミコロンがなかったりするだけ）なので、OpenGLで検索して出てきた解説ページなどを見ればなんとなく理解できると思います。

OpenGLによる3次元CGプログラミング

作者: 林武文,加藤清敬
出版社/メーカー: コロナ社
発売日: 2003/04/01
メディア: 単行本
購入: 3人クリック: 38回
この商品を含むブログ (13件) を見る

tadaoyamaoka.hatenablog.com

ReactNativeでクライアント側を作る

加速度と角速度の取得方法

加速度にはAccelerometer、角速度にはGyroscopeというexpoでデフォルトで使えるAPIを使用します。
docs.expo.io
docs.expo.io
AccelerometerとGyroscopeの使い方はほとんど一緒です。

import React from 'react';
import { StyleSheet, Text, TouchableOpacity, View } from 'react-native';
import { Accelerometer, Gyroscope } from 'expo';

export default class App extends React.Component {

  componentDidMount() {
    this._toggle();
  }

  componentWillUnmount() {
    this._unsubscribe();
  }

  _toggle = () => {
    if (this._subscription) {
      this._unsubscribe();
    } else {
      this._subscribe();
    }
  };

  _subscribe = () => {
    // Accelerometerがデータを取得するインターバルと取得した際の処理を設定
    Accelerometer.setUpdateInterval(16); 
    this._accSubscription = Accelerometer.addListener((result) => {
        console.log("Acc:", result); 
　　# 相補フィルターの処理を書く
    }); 
　// Gyroscopeがデータを取得するインターバルと取得した際の処理を設定
    Gyroscope.setUpdateInterval(16); 
    this._gyroSubscription = Gyroscope.addListener((result) => {
        console.log("Gyro:", result); 
        # クォータニオンによる姿勢推定をここで行う
    }); 
  };

  _unsubscribe = () => {
    this._accSubscription && this._accSubscription.remove(); 
    this._accSubscription = null;
    this._gyroSubscription && this._gyroSubscription.remove(); 
    this._gyroSubscription = null;
  };

  render() 
    return (
      <View style={styles.container}>
        <Text>Hello World</Text>
      </View>
    );
  }
}

const styles = StyleSheet.create({
  container: {
    flex: 1,
  },
});

WebSocketでサーバーに送信する

WebSocketもAccelerometerなどと同様にexpoでデフォルトで使うことができます。
docs.expo.io

import React from 'react';
import { StyleSheet, Text, TouchableOpacity, View } from 'react-native';

export default class App extends React.Component {

  componentDidMount() {
    // WebSocketクラスを生成
    this.ws = new WebSocket("ws://192.168.1.6:9999");
  }

  // コネクション確立時に実行される
  this.ws.onopen = () => {
    console.log("connected");
    // データを送信できる
    // 実際にはtimerなどを使ってクォータニオンを送信する
    this.ws.send("Javascript");
  }

  this.ws.onerror = (e) => {
    console.log(e.message);
  }

  render() 
    return (
      <View style={styles.container}>
        <Text>Hello World</Text>
      </View>
    );
  }
}

const styles = StyleSheet.create({
  container: {
    flex: 1,
  },
});

ジャイロセンサーのデータから姿勢推定する

下の動画にあるように角速度を単純に積算するだけでは、姿勢は推定できません。そのため、クォータニオンを用います。
www.youtube.com

クォータニオンについては以下のサイトが参考になるかと思います。特にMSSさんの「クォータニオン計算便利ノート」は非常にまとまっていてわかりやすいです。
・【Unity道場博多スペシャル 2017】クォータニオン完全マスター - YouTube
・http://www.mss.co.jp/technology/report/pdf/18-07.pdf
・Python 3.x - ジャイロセンサーから測定したquaternionによる姿勢推定を用いた加速度の回転方法について｜teratail
・クォータニオンから回転角と回転軸の算出 ( プログラム ) - Color Model：色をプログラムするブログ - Yahoo!ブログ
・quaternion - npm

初期クォータニオンを作って、角速度からクォータニオンの時間変化を作って足しこむことで、次ステップでのクォータニオンを求められます。計算を繰り返していると誤差が累積してしまうので、計算毎に正規化を行います。
$q(t+\Delta t) = q(t) + \frac{1}{2}\omega q(t)\Delta t$

$\frac{d}{dt}\left( \begin{array}{c} q_0 \\ q_1 \\ q_2 \\ q_3 \end{array} \right) = \left( \begin{array}{cccc} 0 & -\omega_1 & -\omega_2 & -\omega_3 \\ \omega_1 & 0 & -\omega_3 & \omega_2 \\ \omega_2 & \omega_3 & 0 & -\omega_1 \\ \omega_3 & -\omega_2 & \omega_1 & 0 \end{array} \right) \left( \begin{array}{c} q_0 \\ q_1 \\ q_2 \\ q_3 \end{array} \right)$

var quat = new Quaternion([1.0, 0.0, 0.0, 0.0]); // グローバル変数

var Quaternion = require('quaternion');


this._gyroSubscription = Gyroscope.addListener((result) => {
  var dt = 0.10

  var wx = -result.x 
  var wy = -result.y
  var wz = result.z

  var dq = new Quaternion([  0*quat.w -wx*quat.x -wy*quat.y -wz*quat.z,
                                              wx*quat.w + 0*quat.x -wz*quat.y +wy*quat.z,
                                              wy*quat.w +wz*quat.x + 0*quat.y -wx*quat.z,
                                              wz*quat.w -wy*quat.x +wx*quat.y + 0*quat.z])
  dq = dq.scale(0.5*dt)
  quat = quat.add(dq)
  quat = quat.normalize();

  this.ws.send("Gyroscope " + "Quaternion:" + " w:"+quat.w.toString() + " x:"+quat.x.toString() + " y:"+quat.y.toString() + " z:"+quat.z.toString()); 
});

加速度で相補フィルターをかける

相補フィルターはざっくりいえばジャイロセンサーと加速度センサーの加重平均のようなものです。以下の記事が非常にわかりやすいです。ベクトル演算をしたかったので、Three.jsをちょっとだけ使いました。以下のコードはまだ係数の設定をしていません。加速度とジャイロだけだとヨー軸を揃えることはできません。
qiita.com

import ExpoTHREE from 'expo-three'
import { THREE } from 'expo-three'

Accelerometer.setUpdateInterval(10); 
this._accSubscription = Accelerometer.addListener((result) => {
  var dt = 0.050
  var acc = new THREE.Vector3(result.x*dt, result.y*dt, result.z*dt)   
  var gravity = new THREE.Vector3(0, 0, 1)

  // Gが大きい時はそもそも加速度計の値を使用しないようにしています。
  if(Math.abs(acc.x)+Math.abs(acc.y)+Math.abs(acc.z) < 1.5){
    var accVectorCalc = quat.inverse().mul(acc).mul(quat).toVector().slice(1,4)
    var accVector = new THREE.Vector3(accVectorCalc[0],accVectorCalc[1],accVectorCalc[2])
    var correctionVector = accVector.cross(gravity)
    var correctionValue = Math.acos(accVector.dot(gravity))
   
    var accQuat = new Quaternion(Math.cos(0.5*correctionValue),
[correctionVector.x*Math.sin(0.5*correctionValue),correctionVector.y*Math.sin(0.5*correctionValue),correctionVector.z*Math.sin(0.5*correctionValue)])
    quat = quat.mul(accQuat)
  } 
});

ちろる

理系大学生が自由気ままに