角速度から回転行列を求める - 実装編

# はじめに
「[角速度から回転行列を求める - 理論編](https://qiita.com/yakiimo121/items/9506d404bce8b1a49994)」の続きです。前回は角速度から回転行列を求める方法を説明しました。今回はそれをC++で実装し、Processingで可視化していきます。
<img width="600" alt="システムフロー" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/344018/9160dd7a-4679-7415-71bc-3dece4946447.png">

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

https://github.com/yakiimo121/imu_simple_filter

# 環境
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をインストールします。私は以下の記事を参考にさせていただきました。

https://qiita.com/greatnekochan/items/010237ab3313757d0b0b

また、ProcesssingにUDPのライブラリをインストールする必要があります。
Processingを起動して、`スケッチ` -> `ライブラリをインポート...` -> `ライブラリを追加...`の順で進み、UDPをインストールしてください。
<img width="500" alt="UDPの設定" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/344018/ec985373-42c9-f87e-fe84-0ed30e51329c.png">

スマートフォンにHASC Loggerをインストールします。[Apple Store](https://apps.apple.com/jp/app/hasc-logger-%E3%82%BB%E3%83%B3%E3%82%B5%E6%83%85%E5%A0%B1%E5%8F%8E%E9%9B%86%E3%82%A2%E3%83%97%E3%83%AA/id384897105)からインストールできます。2021/06/15現在、Android版がGoogle Playから消えているのでiOS端末でしか動作確認できません。

HASC Loggerの設定を行います。
HASC Loggerを起動します。New Log > Settingsで設定画面に行きます。
<img width="300" alt="設定" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/344018/36cda850-be56-cd93-3575-2265b7b67453.png">
角速度をセンシングできるように設定します。サンプリングレートは任意ですが、今回は100Hzにします。
<img width="300" alt="角速度の設定" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/344018/38c9e0ec-d1c9-6e1a-6504-23d61fa75e97.png">
UDPでデータを送信する設定にします。
`xxx.xxx.xxx.xxx`はホストのIPです。PORT番号は4001です。Use Hasc Log Formatをオンにしてください。
<img width="300" alt="UDPの設定" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/344018/196842dc-cc18-1e43-79eb-152cbeee6b1c.png">
HASC Loggerの設定は以上です。

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

```
./demo
```

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

HASC Loggerでセンシングを開始します。
<img width="300" alt="UDPの設定" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/344018/f4479ef3-5177-639b-6eeb-bd001ec3c45f.png">

Processingにスマートフォンの姿勢がリアルタイムで表示されます。
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">角速度を用いてスマートフォンの姿勢推定をしてみました。相補フィルターを使わないシンプルなアルゴリズムです。序盤は正確にトラッキングできていますが、時間とともにオフセット誤差が増加します。 <a href="https://t.co/RdN5wB2nEm">pic.twitter.com/RdN5wB2nEm</a></p>&mdash; yakiimo121 (@yakiimo121) <a href="https://twitter.com/yakiimo121/status/1405373485786877955?ref_src=twsrc%5Etfw">June 17, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>


# 参考
- https://qiita.com/greatnekochan/items/010237ab3313757d0b0b
- https://nobita-rx7.hatenablog.com/entry/28514197
- https://github.com/arduino-libraries/MadgwickAHRS
- https://qiita.com/srs/items/c9286b5cff99e741b993
- https://nekodigi.hatenablog.com/entry/2019/10/31/%E3%80%909%E8%BB%B8%E3%82%BB%E3%83%B3%E3%82%B5%E3%83%BC%C3%97Processing%E3%80%91%E5%82%BE%E3%81%8D%E3%82%92Socket%E9%80%9A%E4%BF%A1%E3%81%A7%E9%A3%9B%E3%81%B0%E3%81%97%E3%81%A6%E3%80%81Processing