はじめに

「角速度から回転行列を求める - 理論編」の続きです。前回は角速度から回転行列を求める方法を説明しました。今回はそれをC++で実装し、Processingで可視化していきます。

コード

スマートフォンの姿勢推定を行い、可視化するサンプルコードです。角速度から回転行列を求めるプログラムをC++で実装し、可視化をProcessingで行っています。角速度の収集はHASC Loggerというセンシングアプリを使う前提で実装してあります。スマートフォンとC++とProcessingはソケット通信を用いてデータをやりとりしています。

環境

macOS Catalina 10.15.7
Apple clang version 12.0.0
HASC Logger 1.5.7
Processingライブラリ UDP 0.1

準備

はじめにリポジトリをクローンします。

git clone https://github.com/yakiimo121/imu_simple_filter

demo.cppをコンパイルします。

cd imu_simple_filter/src/
g++ demo.cpp -o demo  -std=c++11

Processingをインストールします。私は以下の記事を参考にさせていただきました。

また、ProcesssingにUDPのライブラリをインストールする必要があります。
Processingを起動して、スケッチ -> ライブラリをインポート... -> ライブラリを追加...の順で進み、UDPをインストールしてください。

スマートフォンにHASC Loggerをインストールします。Apple Storeからインストールできます。2021/06/15現在、Android版がGoogle Playから消えているのでiOS端末でしか動作確認できません。

HASC Loggerの設定を行います。
HASC Loggerを起動します。New Log > Settingsで設定画面に行きます。

角速度をセンシングできるように設定します。サンプリングレートは任意ですが、今回は100Hzにします。

UDPでデータを送信する設定にします。
xxx.xxx.xxx.xxxはホストのIPです。PORT番号は4001です。Use Hasc Log Formatをオンにしてください。

HASC Loggerの設定は以上です。

実行

スマートフォンから角速度を受信して、角速度から回転行列を求め、プロセッシングに回転行列を送信するデモ用のプログラムを実行します。

./demo

Processingを起動します。imu_simple_filter/visualize/visualize.pdeを実行してください。

HASC Loggerでセンシングを開始します。

Processingにスマートフォンの姿勢がリアルタイムで表示されます。

角速度を用いてスマートフォンの姿勢推定をしてみました。相補フィルターを使わないシンプルなアルゴリズムです。序盤は正確にトラッキングできていますが、時間とともにオフセット誤差が増加します。 pic.twitter.com/RdN5wB2nEm
— yakiimo121 (@yakiimo121) June 17, 2021

参考