はじめに
こんにちは！lainです。🌸☄️
本記事はカバー株式会社アドベントカレンダー2025の23日目の記事です。昨日の記事はLuncoさんによる「UnityのProfilerで場所を特定できないGCAllocの発生源を特定してみる」でした。
今回は赤外線センサを用いた一般的な光学式モーションキャプチャシステムがどのように物体の三次元位置を特定しているかを単純なモデルを用いて基礎部分を紹介していきます。
基本的な行列計算を理解できる方が対象となります。
ちなみに弊社のモーションキャプチャスタジオの様子はこちらの記事から垣間見ることができます。
光学式モーションキャプチャの概要
まずは光学式モーションキャプチャ（以下モーキャプ）がどのような原理で物体の位置を特定しているかをざっくり紹介します。
一般的な（パッシブ）光学式モーキャプは以下の流れで物体の三次元位置を特定します。
- カメラから光を出す
- 光が物体についているマーカーに当たり反射する
- 光がカメラに映り二次元の像が得られる
- 複数の二次元画像をもとに三次元位置を復元
光は赤外線を用いることが多く、マーカーには入射した光をまっすぐそのまま来た方向に返す再帰性反射材が使われます。（再帰性反射材は夜道で光るリストバンドなどにも使われています。）
反射してカメラに返ってきた光がカメラの撮像素子（イメージセンサ）で反応しそこから二次元の画像が得られます。
参考：モーションキャプチャーとは？
原理や事例、種類によるメリットデメリットをわかりやすく解説
これら複数の二次元画像から三次元位置を算出できればマーカーの位置がわかり、そこから物体の位置を知ることができます。
今回は複数の二次元画像から三次元位置を復元するアルゴリズムについて説明します。
前提
簡単のため二台のカメラからマーカーの三次元位置を算出することを考えます。更に誤差やレンズのゆがみ、オクルージョン（遮蔽）は考えないこととします。
物体から出た光が必ずレンズ中心を通るモデルをピンホールカメラモデルといいます。
これは画家たちが写実的な絵画を描くために用いたカメラ・オブスキュラというピンホールカメラモデルの原理を用いた装置。
三次元座標から撮像素子への二次元の射影を考える
いきなり二次元画像から三次元座標を求めに行く前に、わかりやすいように位置が既知の三次元物体を二次元とみなせる撮像素子に射影する過程を考えてみましょう。
まず、YZ平面上にモーキャプしたい物体とレンズと撮像素子がある状況を考えます。焦点距離をf（ピクセル換算）とすると、相似の関係と像が反転することを考慮すると撮像素子に映る物体の座標x,yが以下のようにあらわせます。X軸は参考画像を垂直に貫く方向とします。
\left\{
\begin{array}{ll}
x=-\frac{fX}{Z} & \\
y=-\frac{fY}{Z} &
\end{array}
\right.
ここで $\lambda=\frac{1}{Z}$とすると、
\left\{
\begin{array}{ll}
x=-\lambda fX & \\
y=-\lambda fY &
\end{array}
\right.
となりこれを行列表記すると、
\begin{pmatrix}
x \\
y \\
1
\end{pmatrix}
=\lambda
\begin{pmatrix}
-f & 0 & 0 \\
0 & -f & 0\\
0 & 0 & 1
\end{pmatrix}
\begin{pmatrix}
X \\
Y \\
Z
\end{pmatrix}
と変形できます。
この焦点距離を含む行列を内部パラメータ行列といいます。今回は扱いませんが、レンズのゆがみもこの行列に含めることができます。
また、実際には撮像面が傾いていたり、位置がずれていたりします。このずれはアフィン変換を用いて以下のように表せます。
アフィン変換については詳しくはこちらの記事を参照ください。
\begin{pmatrix}
R|T \\
\end{pmatrix}
=
\begin{pmatrix}
1 & 0 & 0& 0 \\
0 & \cos(\theta) & -\sin(\theta) & -dy\\
0 & \sin(\theta) & \cos(\theta) & -dz\\
0 & 0 & 0& 1
\end{pmatrix}
\begin{pmatrix}
X \\
Y \\
Z \\
1
\end{pmatrix}
カメラ自体の位置姿勢のずれを外部パラメータ行列と呼びます。
内部パラメータと外部パラメータの積を透視投影行列と呼びます。
透視投影行列を用いて、マーカの座標（$\bar{M}$）を二次元の座標($\boldsymbol{m}$)に変換する式は、
s\boldsymbol{m}=A(R|T)\bar{M}\tag{1}
と表せます。(s=1/$\lambda$とした）。
$\bar{M}$が三次元ベクトルとすると、$\boldsymbol{m}$と$\bar{M}$に関する式は2つしかないためカメラ一台では厳密には$\bar{M}$が求まりません。奥行が未知のままになっています。
次章では理想的なステレオカメラ二台を考え、本章を逆にたどっていくことで$\bar{M}$を求めていきます。
キャリブレーション
モーションキャプチャの解説動画などで棒（ワンド）を振り回す謎の儀式を見たことがあるかもしれません。
実はあの儀式によって透視投影行列が求まります。
ワンド自体の絶対座標は画像からは求まりませんが、ワンドの位置についているマーカーの位置は固定なため同時に写っている多くのカメラ画像の相対位置からワンドの絶対座標を推定しています。
参考
またキャリブレーションの最後にワンドを部屋の中心の床に置きますが、そこが座標系の原点となります。
RGBカメラを用いた画像ベースモーキャプではよくチェス盤の模様のボードを使ってキャリブレーションしますが、これもチェス盤を様々な角度からカメラに映すことでキャリブレーションをしています。
2台のカメラからマーカーの三次元座標を求める
理想的なステレオカメラ二台を考えます。ここでの理想とは、遮蔽が(オクルージョン)なくマーカーが必ず二台のカメラに映り、$\boldsymbol{m}$の位置の誤差がないこととします。
実際には現実のカメラでは必ず誤差が発生するため数多くのカメラをまんべんなく配置し多くのカメラにマーカーを映し、最小二乗法などを用いて統計的に誤差を最小化して推定していきます。
まず、透視投影行列を$P$とします。
$P$は3×4行列となるため、以下のようにおけます
P=\begin{pmatrix}
p_{11} & p_{12} & p_{13} & p_{14} \\
p_{21} & p_{22} & p_{23} & p_{24} \\
p_{31} & p_{32} & p_{33} & p_{34} \\
\end{pmatrix}
キャリブレーションが終わり、Pを既知としたうえでMを特定していきます。
二台のカメラのスケールファクタを$s_1,s_2$とします。
(1)より
s_{1}
\begin{pmatrix}
x_1 \\
y_1 \\
1
\end{pmatrix}
=
P_1
\begin{pmatrix}
X \\
Y \\
Z \\
1
\end{pmatrix}
\tag{2}
上記のように次元を1増やして最後の要素を1にした座標を同次座標といいます。
$s_2$も同様に定義したのち、$s_1,s_2$が未知数のため消去します。
(2)より
\begin{pmatrix}
s_1x_1 \\
s_1y_1 \\
s_1
\end{pmatrix}
=
\begin{pmatrix}
P_1^1M \\
P_1^2M \\
P_1^3M \\
\end{pmatrix}
\tag{2}
3行目より
$s_1=P_1^3M=(p_{3 1} p_{3 2} p_{3 3} p_{3 4})M$
1,2行目に代入して
\left\{
\begin{array}{ll}
P_1^3Mx_1=P_1^1 M & \\
P_1^3My_1=P_1^2 M &
\end{array}
\right.
これを変形して、
\left\{
\begin{array}{ll}
(P_1^3x_1-P_1^1)M= \boldsymbol{0} & \\
(P_1^3y_1-P_1^2)M =\boldsymbol{0}&
\end{array}
\right.
$s_2$も同様にして、
\left\{
\begin{array}{ll}
(P_2^3x_2-P_2^1)M= \boldsymbol{0} & \\
(P_2^3y_2-P_2^2)M =\boldsymbol{0}&
\end{array}
\right.
ここで、
\boldsymbol{B}=
\begin{pmatrix}
x_1P_1^3-P_1^1 \\
y_1P_1^3-P_1^2 \\
x_2P_2^3-P_2^1 \\
y_2P_2^3-P_2^2 \\
\end{pmatrix}
とすると、$\boldsymbol{BM}=\boldsymbol{0}$となればよいです。
ここで、x,yはカメラ画像中のマーカーの座標を表すので、未知数はM=(X,Y,Z)の3つであり、式は4つあるのでMを求めることができます。
しかし、Bが正則である場合$M=\boldsymbol{0}$となってしまい情報が失われてしまうため、Bは特異行列（正則でない）でなければいけません。
あとは行列計算のライブラリを用いてMを求めることができます。
今回のような理想的なモデルでは、直接Mを求めることができますが、実際の環境ではノイズや誤差によりBが正則になってしまうことがあります。
そのような問題は特異値分解などを用いて、Mが0に近づく条件を探ります。
実際には商用の多くの光学式モーキャプシステムは更に高度な補正を行っていますが、モーキャプの原理の古典的な基礎部分はこのようになっています。
終わりに
本記事では単純なモデルを用いてキャリブレーションからマーカーの三次元位置を特定するアルゴリズムまでを解説しました。
単純なモデルとはいえ、初歩的な行列計算のみで光学式モーキャプの原理が成り立っていたのは意外でした。光学式モーキャプのアルゴリズムの気持ちを知れたことで、筋肉痛になったり虚無になったりしがちなキャリブレーションがちょっとだけ面白く感じられる気がします。
明日の記事は@hikoalphaさんによる「【Unity】はじめてのRenderGraph」(https://qiita.com/hikoalpha/items/b502f457bdad8389212f)です！