Numerical Recipes in Biomechanics #1  ~回転行列のキャリブレーション（平均化）~

# Numerical Recipes in Biomechanicsについて
### いかに問うべきか？
前職で，「[動かして学ぶバイオメカニクス](https://note.com/ss_sports_lab/m/m5878c85564ab)」や「[モーションにおける３次元回転](https://note.com/ss_sports_lab/n/n8437f5319f55)」のような記事を書いてきましたが，ここでは，そこで述べきれなかったこと，補足・訂正などを示していきたいと考えています．なお「モーションにおける３次元回転」はグループにすることを忘れていたようで，検索しにくかもしれません．お許しください．

また，「動かして学ぶバイオメカニクス」ではPythonコードも示しましたが，ここでは懐かしい[Numerical Recipes in C](https://gihyo.jp/book/1993/4-87408-560-1)という有名な書籍のタイトルをオマージュし，記事を時々書いていきます．"Numerical Recipes in C"はとても良い本でしたが，Cでコードを書くことも減り，若い世代の方はご存じない方も多いでしょうが，数値計算の名著です．

いまやコードはChaGPTで変換すれば，Matlabでも何でも変換してくれますので，自分が使用する言語に変換することは可能でしょう．試しにここで取り上げた例をMathematicaのコードに変換することもできました．

問題の数理的な背景を理解していれば，AIにお願いすると大抵のコードを書くことができます．正しいとは限らないですが，問えばそれなりに答えてくれ，簡単な数学的な問題やアルゴリズムなら（多くの場合）適切に答えてくれます．それを頼りに勉強していけばアルゴリズムや数理的な理解も飛躍的に進みます．AIはプログラムもさることながら，（数学の凡人にはですが）数学的な理解を深めるには素晴らしいツールという印象です．未解決問題をAIに問うのは酷ですが，たとえばこの記事で考えている問題は，私が生まれたときに証明された問題で，その程度の数学問題にすぎず，誤りも少ないです．ネットで検索をすればきっとどこかに書いているでしょうが，圧倒的に答えにたどり着くのが早いという印象です（ただし，当たり前ですが問題に依存します）．

しかし，数理的な背景を理解していないと，そもそも質問が適切にできず，コードが正しいかどうかも判断できないでしょう．質問をするにしても最低限の数理的な理解が求められます．そもそも，どのようにAIに問えばよいのかという背景を言葉にする言葉が求められます．

ちなみに数学的な問題をAIに問うことや，バイオメカニクスの問題をAIにレビューすることは良いでしょうが，問う内容は誤らないでください．「なぜ短距離走者は腸腰筋が発達しているのでしょう？」などと問えば，レビューをしてくれるという意味では良いですが，バイオメカニクスでは理解が進んでいないことを示しています．

**「問い」を生成する思考回路の質が問われています．**

するとここで書いていく記事もそれなりにまだ役立つと考え，「きっと探せばどこかには書いているが，それでもバイオメカニクスなどで必要なトピック」を「どのようにAIに問えば良いのか？」「背後にどのような数理的問題があるのか」ということを意識しながら，可能ならコード付きで今後示していくことを続けていきます．

# はじめに
今回の話題に移ります．

計測データには必ず誤差が含まれます．たとえ変形しない剛体に貼付した２つの反射マーカを数フレームだけモーションキャプチャで計測し，その２点からベクトルを計算しとしても，それらが同じ方向を向き，同じ大きさとなる保証はありません．微小な変化が必ず発生します[^1]．

[^1]:モーションキャプチャの誤差を最も簡単に，かつ的確に精度を検証したければ，この「２つのマーカ間距離の変動」を調べるとよいでしょう．２点間距離は計測環境によって変化します．カメラと被写体が近ければ分解能が上がり変動は小さくなるでしょう．計測する場所を変えれば，それは変化しますし，たとえ同じ場所でも「２つのマーカの向きを変えるだけ」で精度が変わることもあります．複数のカメラで計測する以上，誤差の現れ方はとても複雑です．

同様にモーションキャプチャで剛体にマーカを貼り付けて，その剛体の姿勢を正確な**回転行列**で表現することを念頭におき，静止している剛体を計測し，**回転行列のキャリブレーション**を行う方法をここでは考えます．

# ベクトルの平均
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2567535/1f45b9f4-8f56-4a08-aa2b-4c50a56f2bc9.png" width= "600">

    図１：ベクトルの平均

計測対象が，たとえば２つのマーカの位置ベクトル（モーキャップで計測される座標）$\boldsymbol{x}_h, \boldsymbol{x}_0$から構成されるベクトルなら，通常行うことはベクトルの**平均化**です．全てのベクトルを加算し，計算に使用したベクトルの数で除すればよいわけです．もし，単位ベクトルを計算するなら，さらに，その平均化されたベクトルの大きさで除します．

たとえば，$\boldsymbol{k}$という同じ大きさと長さのはずのベクトルをモーションキャプチャで計測したとしましょう．そのベクトルの平均$\bar{\boldsymbol{k}}$は

$$
\bar{\boldsymbol{k}} = \frac{\sum_{i=1}^n \boldsymbol{k}_i}{n}
$$

と計算すればよいでしょう（図１）．ここで，$i$は時間と考えればよく，モーションキャプチャで$n$フレーム分計測を行ったとします．

これは一種の**キャリブレーション**です．

ちなみにマーカーベースの光学式モーションキャプチャは，バイオメカニクスでは比較的精度の高い計測機器ですが，万能ではありません．計測した空間は想像以上に歪んでいます．仮に100mの空間を多くのカメラで計測するとしましょう．その空間をより精度の高い計測機器でキャリブレーションし比較できたとすると，おそらくm単位で誤差が発生するでしょう．モーションキャプチャは計測空間の複雑な張り合わせです．しかし，速度や加速度などの差分の計算ではこの歪みはあまり問題になりません．力学計算に適した計測装置です．

# 回転行列の平均

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2567535/ace48d62-8c65-4cca-93f6-c8ccd8ac6257.png" width= "450">

    図２：回転行列の平均化？

では，静止した剛体に複数のマーカを貼付し，姿勢回転行列$\boldsymbol{R}_i$[^2]を複数フレーム分，計算した後に，そのキャリブレーションはどのように行えばよいでしょう？

[^2]:ChatGPTに「モーションキャプチャ用の３つの反射マーカを剛体に貼付し，その位置座標からその剛体の姿勢回転行列を計算する方法をPythonで教えて下さい．」と問うと，この脚注の上に示した参考コードを返してくれました．説明図を要求したところ正確でないので，筆者が作成しましました．

精度を気にしないなら，複数のデータではなくワンショットだけ計測するという手もありますが，精度を上げたいならやはり平均化する必要があるでしょう．通常ベクトルは２つのマーカの座標の引き算で計算できますが，剛体の姿勢は最低限３つのマーカが必要で，いくらか複雑な計算を行うことで姿勢回転行列を計算します．その複数の計算過程は，単にベクトルを計算する以上に誤差が発生するので，できるだけ精度を上げることが重要です[^3]．

[^3]:剛体に３つ，または４つなどの複数のマーカを貼付し，剛体の**姿勢回転行列**を定める方法はいずれ**Numerical Recipes in Biomechanics**という記事に書いていく予定です．それまでは過去に記述した[動かして学ぶバイオメカニクス#9　〜身体各部の座標系〜](https://note.com/ss_sports_lab/n/nf26b87359e9e)などを参照ください．

そこで，姿勢回転行列のキャリブレーションを

$$
\bar{\boldsymbol{R}} = \frac{\sum_{i=1}^n \boldsymbol{R}_i}{n}
$$

のように，単純に加算しその数で除すればよいでしょうか．答えはノーです．

ベクトルならその計算でよいのですが，回転行列は互いに直交する単位ベクトルを並べた**直交行列**という性質があります．平均化した回転行列が回転行列となるためには，この直交性を保つ必要があります．

そこで，「そもそも回転行列とは何か」，「行列における直交とな何か？」を簡単に述べます．

# そもそも回転行列とは？
バイオメカニクスにおける３次元空間の回転行列の導入は，**オイラー角**，すなわち３つの座標軸による回転

$$
\begin{bmatrix}
\cos\alpha & -\sin\alpha & 0 \\\\
\sin\alpha & \cos\alpha & 0 \\\\
0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
\cos\beta & 0 & \sin\beta \\\\
0 & 1 & 0 \\\\
-\sin\beta & 0 & \cos\beta
\end{bmatrix}
\begin{bmatrix}
1 & 0 & 0 \\\\
0 & \cos\gamma & -\sin\gamma \\\\
0 & \sin\gamma & \cos\gamma
\end{bmatrix}
$$
で学んだ，という方も多いのではないでしょうか？この導入方法は直感的なわかりやすさもありますが，誤差を発生しやすいオイラー角による導入は，個人的にはあまり推奨しません．あまり深入りしませんが，回転をパラメータで表すことは，バイオメカニクスでは忘れたほうが良いです．計算精度に違いをあまり感じなかったり，簡単さを優先し「計算できるなら，どちらでもよい」と考えがちですが，回転は，この後，説明するように直交する単位ベクトルを並べて表現する**直交行列**としての回転行列の導入のほうが，精度，数理的な理解という意味でメリットが多いでしょう．角度の不連続なども避けられます．ただし計算負荷は少しだけ高いかもしれません．まずはその直交性との関連について簡単にまとめます．

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2567535/dd1b9ce7-9ee9-4191-a0ca-1f36acb58146.png" width= "500">

    図３：正規直交基底

図３のようにバットに固定した座標系を考えます．これらの$xyz$座標軸に固定された単位ベクトルを$\boldsymbol{e}_i, \boldsymbol{e}_j, \boldsymbol{e}_z$を**正規直交基底**と呼びます．基底とは（３次元空間で）ベクトルを表現するために最低限必要な独立した要素のベクトルを意味します．正規は大きさ１，直交は各基底が直交していることを示しています．図３のようにバットに固定した座標系を考え，各$x$軸，$y$軸，$z$軸に，原点を始点とする単位ベクトル$\boldsymbol{e}_i, \boldsymbol{e}_j, \boldsymbol{e}_z$はりつけたものが正規直交基底です．この正規直交基底を並べた行列

$$
\boldsymbol{R} = \begin{bmatrix}
\vdots & \vdots & \vdots \\\\
\boldsymbol{e}_i & \boldsymbol{e}_j & \boldsymbol{e}_z \\\\
\vdots & \vdots & \vdots
\end{bmatrix}
$$

をこのバットの姿勢を表す姿勢回転行列として考えます．

この回転行列は

$$
\boldsymbol{R}^{-1} = \boldsymbol{R}^{T}
$$
のように転置行列が逆行列となる性質を持ち，これを**直交行列**と呼びます．ここで$\boldsymbol{R}^T$のように上付きで$T$の記号をつけた行列が**転置行列**です．

$$
\boldsymbol{A} = \begin{bmatrix}
a_{11} & a_{12} & a_{13} \\\\
a_{21} & a_{22} & a_{23} \\\\
a_{31} & a_{32} & a_{33}
\end{bmatrix}, \quad
\boldsymbol{A}^T = \begin{bmatrix}
a_{11} & a_{21} & a_{31} \\\\
a_{12} & a_{22} & a_{32} \\\\
a_{13} & a_{23} & a_{33}
\end{bmatrix}
$$
のように対角成分（$a_{ii}$）はそのままで，非対角成分（$a_{ij}, i\ne j$）を入れ替えた行列が対角行列です．

このことは，回転の逆変換を計算する際，簡単に転置するだけですみます．回転行列の逆行列を計算するために通常の逆行列の計算を行ってはいけないことを示しています．同じ結果を得られるという意味で誤りではありませんが，明らかに無駄な計算です．

このような性質を持つことがわかれば，次のことも容易にわかります．

$$
\begin{aligned}
\boldsymbol{R}^T \boldsymbol{R} &= \begin{bmatrix} \boldsymbol{e}_x^T\ \\\\ \boldsymbol{e}_y^T \\\\ \boldsymbol{e}_z^T \end{bmatrix}
\begin{bmatrix}\boldsymbol{e}_x & \boldsymbol{e}_y & \boldsymbol{e}_z \end{bmatrix}
\\\\
&=\begin{bmatrix}
\boldsymbol{e}_x^T \boldsymbol{e}_x & \boldsymbol{e}_x^T \boldsymbol{e}_y & \boldsymbol{e}_x^T \boldsymbol{e}_z \\\\
\boldsymbol{e}_y^T \boldsymbol{e}_x & \boldsymbol{e}_y^T \boldsymbol{e}_y & \boldsymbol{e}_y^T \boldsymbol{e}_z \\\\
\boldsymbol{e}_z^T \boldsymbol{e}_x & \boldsymbol{e}_z^T \boldsymbol{e}_y & \boldsymbol{e}_z^T \boldsymbol{e}_z
\end{bmatrix}
\\\\
&= \begin{bmatrix} 1 & 0 & 0 \\\\
0 & 1 & 0 \\\\
0 & 0 & 1
\end{bmatrix} = \boldsymbol{E}
\end{aligned}
$$

ここで$\boldsymbol{E}$は**単位行列**を表します．回転と逆回転を掛け算するともとに戻ることを示しています．

また，回転行列の転置と回転行列の積は，各基底ベクトル同士の内積（例えば，$\boldsymbol{e}_x^T \boldsymbol{e}_y$）で記述されていますが，直交する単位ベクトルの積は$0$ですから，非対角項が$0$となることを示しています．

つまり，図３の正規直交基底を順番に並べた回転行列$\boldsymbol{R}$は，直交行列（つまり$\boldsymbol{R}^T \boldsymbol{R} = \boldsymbol{E}$）の性質を満たします．

なお，行列$\boldsymbol{R}$が回転行列であるためには，さらに$|\boldsymbol{R}|>0$を満たす必要がありますが，これはまたどこかで述べることとしますが，**鏡映**（鏡で反転させる変換）を行う変換行列も直交行列となるためです．

先程も示しましたが，回転行列は

$$
\boldsymbol{R} = \begin{bmatrix}
\vdots & \vdots & \vdots \\\\
\boldsymbol{e}_i & \boldsymbol{e}_j & \boldsymbol{e}_k \\\\
\vdots & \vdots & \vdots
\end{bmatrix}
$$
のような構成（横に並べてもよい）になりますが，各基底ベクトルは

$$
\boldsymbol{e}_i^T \boldsymbol{e}_j = 0, i\ne j
$$
のように互いに直交し，

$$
|\boldsymbol{e}_i| = 1
$$

のように大きさが$1$となる性質を持っています．

過去の記事「[動かして学ぶバイオメカニクス#10　〜角速度ベクトル〜](https://note.com/ss_sports_lab/n/na80cd8198d22)」などにも記述していますのでご参照ください．


# 回転行列の平均化
複数の直交行列である回転行列を先に示した式のように平均化したときに，行列を構成するそれぞれの基底ベクトルの大きさが１で，互いに直交するという保証はありません．

そこで，平均化したあとに，平均化した行列を回転行列に修正する必要があります．

ここでは，**特異値分解**を用いた一般的な方法を述べます．この計算は最適化を行って導きます．証明は簡単ですが若干長いので，どのように計算を行うかだけを示します．

原理を知りたい方は，このあとに引用する文献や資料をご覧になってください．

### 特異値分解
**特異値分解**（**Singular Value Decomposition** ：**SVD**)は，数値計算ではいろいろなところで用いられます．主成分分析（PCA）と密接に関係し，シナジーの計算で使用されることもあります．行数と列数が同じ大きさの正方行列でも，そうでない場合も計算できます．

特異値分解を理解したい方は，まずは，下記の[特異値分解のWiki](https://ja.wikipedia.org/wiki/%E7%89%B9%E7%95%B0%E5%80%A4%E5%88%86%E8%A7%A3)の図の**剪断**（平行方向の歪み）の例をご覧になるとよいでしょう．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2567535/8d160479-bc44-4960-8240-20f649b6de71.png)Wikiより

行列の分解方法にはいくつもの方法がありますが，特異値分解は有用で最も強力に数値計算で使用される分解方法の一つで，行列$\boldsymbol{M}$を

$$
\boldsymbol{M} = \boldsymbol{U}\boldsymbol{\Sigma} \boldsymbol{V}^T
$$

のように特別な３つの行列$\boldsymbol{U}, \boldsymbol{\Sigma}, \boldsymbol{V}$に分解します．

このうち$\boldsymbol{U}, \boldsymbol{V}$は直交行列です．Wikiの図の$\boldsymbol{V}^{\*}$と$\boldsymbol{U}$の回転に相当します．Wikiの${}^*$はここでの転置記号${}^T$に相当します．これは，回転行列と想像すればわかりやすいでしょう．

行列$\boldsymbol{\Sigma}$は

$$
\boldsymbol{\Sigma} = 
\begin{bmatrix}
\sigma_1 & 0 & 0 & \cdots \\\\
0 & \sigma_1 & 0 & \cdots \\\\
0 & 0 & \sigma_3 & \cdots \\\\
\vdots & \vdots & \vdots & \ddots
\end{bmatrix}
$$
のような対角行列となります．$\sigma_i$は，**特異値**と呼び，各成分を伸縮する役割があります．固有値と少し似た性質を持ちます．

特異値は大きいほうから並べることが基本です．

主成分分析にたとえるなら，最も強く相関のあるほうから並べています．伸縮という意味では，Wikiの図の楕円の最も伸びている方向の成分です．少しだけ特異値の意味を補足します．

### 特異値の補足

<img width = "350" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2567535/9fd46635-a1cd-4c11-a703-be09adae0521.png">

    図４：データのばらつきと主軸

図４のように$x_1, x_2$の座標軸から構成される平面にデータ$\boldsymbol{x}_1, \boldsymbol{x}_2, \cdots, \boldsymbol{x}_n$のデータが分布しています．これを次の式のように並べたのが行列$\boldsymbol{M}$です．

<img width = "250" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2567535/9410c170-8766-4eba-bc21-d9dcf2fa12a7.png">

紫の楕円は，円をベクトル$\boldsymbol{w}_1$の方向に引き伸ばし，それに直交するベクトル$\boldsymbol{w}_2$の方向に縮めていることを示しています．統計やデータサイエンスの観点からは，$\boldsymbol{w}_1$の方向に相関があると考えればよく，**主成分分析**における**第１主成分**の軸方向を示しています．

対角成分に伸縮を意味する特異値$\sigma_i$を大きい順に並べた行列が$\boldsymbol{\Sigma}$です．

Wikiの図を思い出すと，特異値分解は行列を３つの，回転$\boldsymbol{U}$→伸縮$\boldsymbol{\Sigma}$→回転$\boldsymbol{V}$を行う変換に分解できることを示しています．

$\boldsymbol{U}$も$\boldsymbol{V}$も回転行列とイメージすれば，図３で回転行列とは直交する座標軸に単位ベクトルを貼り付けたものであることを述べました．そこで，イメージしやすい３次元で$\boldsymbol{M}$を考え，その特異値分解を構成する直交行列$\boldsymbol{U}$も$\boldsymbol{V}$も３次元なら

$$
\boldsymbol{U}=
\begin{bmatrix} \boldsymbol{u}_1&\boldsymbol{u}_2&\boldsymbol{u}_3
\end{bmatrix},~~
\boldsymbol{V}=
\begin{bmatrix} \boldsymbol{v}_1&\boldsymbol{v}_2&\boldsymbol{v}_3
\end{bmatrix}
$$

と書くことができ，$\boldsymbol{U}$を構成する，$\boldsymbol{u}_i$は互いに直交します．$\boldsymbol{V}$も同様です．

すると，3次の正方行列$\boldsymbol{M}$の特異値分解は
$$
\begin{bmatrix} \boldsymbol{u}_1&\boldsymbol{u}_2&\boldsymbol{u}_3
\end{bmatrix}
\begin{bmatrix} \sigma_1 & 0 & 0 \\\\
0 & \sigma_2 & 0 \\\\
0 & 0 & \sigma_3
\end{bmatrix}
\begin{bmatrix} \boldsymbol{v}_1^T \\\\\boldsymbol{v}_2^T \\\\
\boldsymbol{v}_3^T
\end{bmatrix} = \sigma_1 \boldsymbol{u}_1 \boldsymbol{v}_1^T + \sigma_2 \boldsymbol{u}_2 \boldsymbol{v}_2^T + \sigma_3 \boldsymbol{u}_3 \boldsymbol{v}_3^T
$$
と書くことができ，一般に
$$
\boldsymbol{M} = \boldsymbol{U} \boldsymbol{\Sigma} \boldsymbol{V}^T=\sum\sigma_i\boldsymbol{u}_i \boldsymbol{v}_i^T
$$
と記述できます．$\boldsymbol{u}_i \boldsymbol{v}_i^T$は行列ですが，「直線」的な関係（ランク１）があります．

固有値$\lambda_i$のように，特異値$\sigma_i$に対して，「特別な」伸び縮みするそれぞれ独立な（直交する）方向$\boldsymbol{u}_i \boldsymbol{v}_i^T$が存在するとイメージすると良いでしょう．

このような分解はとても便利です．固有値と特異値は伸縮という意味では似ていますが，同じ行列から固有値と特異値を計算しても，行列によっては，その値や，伸縮の方向や物理的意味は異なります．詳細はまたどこかで述べることとします．

### 特異値分解による補正
**Singular Spectrum Analysis** (SSA) というフィルタリングも，時系列を行列に埋め込んで特異値分解（SVD）を行う方法です．筋力のように非負（正）となる拘束がない場合の**シナジー解析**では，この特異値（伸縮）を活用します．

ここでは，平均化した行列を特異値分解し，直交行列である$\boldsymbol{U}, \boldsymbol{V}$を活用します．

つまり，平均化した回転行列$\bar{\boldsymbol{R}}$を

$$
\bar{\boldsymbol{R}} = \boldsymbol{U}\boldsymbol{\Sigma} \boldsymbol{V}^T
$$

のように特異値分解し，

$$
\hat{\boldsymbol{R}} = \boldsymbol{U} \boldsymbol{V}^T
$$

を計算すると，最適に回転行列として補正できます．特異値が全て1とすることで伸縮のない回転行列にすると考えます．結果，特異値$\boldsymbol{\Sigma}$を使用しないことになります．

そこで，ここで述べる最適化とは，$\bar{\boldsymbol{R}}$は回転（直交）行列ではないので，「$\bar{\boldsymbol{R}}$に最も近い回転行列を最小化」になります．つまり，

$$
J = || \bar{\boldsymbol{R}} - \boldsymbol{R} ||^2_F
$$
を最小化する問題に置き換えると，**プロクルステス問題**（Procrustes problem）として知られている問題になります．ここで$||~||_F$は**フロベニウスノルム**を意味します．フロベニウスノルムが行列の「大きさ」を表すノルムの一つで，

　　　　　　　　　　　　　　<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2567535/6d27e463-38f9-48eb-a898-1691b4847b31.png" width= "200">

のように計算します[^5]．

[^5]:行列の各成分の２乗和で，行列の全成分を一列に並べてベクトルとみなしたときのベクトルの長さの二乗（L2ノルム）と考えると良いでしょう．

$\boldsymbol{U}, \boldsymbol{V}$は直交行列で，異なる直交行列の積も直交行列なので，$\hat{\boldsymbol{R}}$も直交行列となります．回転行列と置き換えて考えれば，回転行列の積も回転行列になることから，ここで証明は行わないですが，少なくともこれが回転行列になることは直感的に理解できるかと思います．

証明は現・名古屋工業大学の玉木先生の[姿勢推定と回転行列](https://www.slideshare.net/slideshow/20090924/15277938)をご覧になるとよいでしょう．長いですが，基本的な線形代数で容易に理解できます．

また特異値分解については書籍「[線形代数の半歩先-データサイエンス・機械学習に挑む前の30話-](https://www.kspub.co.jp/book/detail/5386477.html)」にとてもわかりやすく書いてあります．

### Pythonコード
平均化した行列を回転行列に修正するコードを以下に示します．ChatGPTが書いてくれたコード（筆者が書いても，実質，同じコードになりますが，残念ながらあまり良いエンジニアではないので，ChatGPTのほうがきれいです）に少し手を加えたものです．

ちなみに「複数の回転行列データの平均を計算するPythonコードを教えて下さい」と[Curosr](https://www.cursor.com/ja)のエディタ内で，ChatGPTにコマンドKで質問をしただけです．

```python
import numpy as np
def average_rotation_matrices(matrices):
    """
    複数の3x3回転行列の平均をSVDを使って計算する。
    引数:
        matrices: shape (N, 3, 3) のnumpy配列。N個の回転行列。
    戻り値:
        平均回転行列 (3x3 numpy配列)
    """
    assert matrices.ndim == 3 and matrices.shape[1:] == (3, 3), "各回転行列は 3x3 の形で与えてください。"

    # 行列の平均をとる
    M = np.mean(matrices, axis=0)

    # SVDを実行
    U, _, Vt = np.linalg.svd(M)

    # 平均回転行列
    R_avg = U @ Vt

    # 行列式が負なら反射行列なので修正
    if np.linalg.det(R_avg) < 0:
        U[:, -1] *= -1
        R_avg = U @ Vt

    return R_avg
```

このコードの最後のif文は，念の為，鏡映となることを防ぐためのコードです．よぽど，データが暴れていない限りは，鏡映とはならないでしょう．そこまで暴れるようではキャリブレーションが成り立つ状況とは言えません．

この程度であればChatGPTは完璧です．恐るべし．

#### 参考コード
ChatGPTによる「モーションキャプチャ用の３つの反射マーカを剛体に貼付し，その位置座標からその剛体の姿勢回転行列を計算するPythonコード」
```python:
import numpy as np

def compute_rotation_from_three_markers(Q):
    """
    Q: shape (3, 3) → 各行がマーカーの世界座標（q1, q2, q3）
    戻り値: 3x3の回転行列 R
    """

    q1, q2, q3 = Q

    # x軸方向（q2 - q1）
    x_axis = q2 - q1
    x_axis /= np.linalg.norm(x_axis)

    # 仮想z軸（q2 - q1）と（q3 - q1）の外積（法線ベクトル）
    z_axis = np.cross(x_axis, q3 - q1)
    z_axis /= np.linalg.norm(z_axis)

    # y軸（z軸 × x軸）で右手系を作る
    y_axis = np.cross(z_axis, x_axis)
    y_axis /= np.linalg.norm(y_axis)

    # 回転行列を構築（列ベクトルとして）
    R = np.column_stack((x_axis, y_axis, z_axis))

    return R
```

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2567535/bdf67b6e-4aac-43b3-8bcc-c5a42bfeb2cb.png" width= "400">

    図５：マーカq1, q2, q3と座標軸の関係
    「q1 を原点」「q2-q1 をx軸」「q1,q2,q3が構成する平面の法線をz軸」
    　とするローカル座標系とみなした姿勢