MediaPipeを使って3Dアバターを動かす

　MediaPipe ではカメラ画像などの入力画像から人物やオブジェクトなどを認識できるライブラリです。
　[前回の記事では MediaPipe を Windows 環境にインストールした](https://qiita.com/Entis/items/e470ce54e70244aaf4a9)ので、MediaPipe のトラッキング情報を、既にある「3Dアバターをリアルタイムにトラッキング情報に合わせて動かす自作のプログラム」へ利用できるようプログラムを組んでいきます。
　この記事ではやった作業を思い出しながら要点を記していきたいと思います。
![アバター動作画面スクリーンショット](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/285025/2ea12d0f-2504-30ca-8789-a782d9dab7a2.jpeg "一般のカメラで動作するアバタープログラム")


# MediaPipeプロジェクト

　自作のソフトに MediaPipe を利用するにあたり、ビルド環境が根本的に異なることなどから、MediaPipe のトラッキング情報をソケット通信で送り、MediaPipe を利用したプログラム側は独立した簡単なものにすることにしました。
　まず、MediaPipe を展開したディレクトリに、以下のようにディレクトリを作成し BUILD ファイルを設置し、その他プログラムソースなども作成しました。

```
+ mediapipe
  + projects
     + CaptureServer
        - BUILD
        - run_graph_main.cc
        - CaptureServerCalculator.cpp
        - CaptureServerCalculator.h
        - CaptureServerHost.cpp
        - CaptureServerHost.h
        - CaptureServer.pbtxt
```
　BUILD ファイルはサンプルを参考にし、以下のようにしました。
（今回使用していない依存ファイルも含んでいますが、そのうち使うかもしれないので（面倒なので）そのままにしてあります）

```BUILD.
package(default_visibility = ["//mediapipe/examples:__subpackages__"])

cc_binary(
    name = "CaptureServer",
    srcs = [
    	"run_graph_main.cc",
    	"CaptureServerCalculator.cpp",
    	"CaptureServerHost.cpp",
    ],
    deps = [
        "//mediapipe/graphs/hand_tracking:desktop_tflite_calculators",

        "//mediapipe/graphs/pose_tracking:pose_tracking_cpu_deps",
        "//mediapipe/graphs/pose_tracking:upper_body_pose_tracking_cpu_deps",

        "//mediapipe/graphs/face_detection:desktop_live_calculators",
        "//mediapipe/graphs/face_mesh:desktop_live_calculators",

        "//mediapipe/framework:calculator_framework",
        "//mediapipe/framework/formats:image_frame",
        "//mediapipe/framework/formats:image_frame_opencv",
        "//mediapipe/framework/port:commandlineflags",
        "//mediapipe/framework/port:file_helpers",
        "//mediapipe/framework/port:opencv_highgui",
        "//mediapipe/framework/port:opencv_imgproc",
        "//mediapipe/framework/port:opencv_video",
        "//mediapipe/framework/port:parse_text_proto",
        "//mediapipe/framework/port:status",

        "//mediapipe/graphs/iris_tracking:iris_depth_cpu_deps",
        "//mediapipe/graphs/iris_tracking:iris_tracking_cpu_deps",
    ],
)
```

　また、プロジェクトのビルド用バッチファイルを以下のように作成しておきました。

```build.bat
@echo off
%~d0
cd %~p0
bazel build -c opt --define MEDIAPIPE_DISABLE_GPU=1 mediapipe/projects/CaptureServer:CaptureServer
pause
```

　ついでに実行用のバッチファイルは以下のようにしました。

```run.bat
@echo off
%~d0
cd %~p0
set GLOG_logtostderr=1
"bazel-bin/mediapipe/projects/CaptureServer/CaptureServer.exe" --calculator_graph_config_file=mediapipe/projects/CaptureServer/CaptureServer.pbtxt
pause
```

　これらのバッチファイルは MediaPipe を展開したルート（WORKSPACE ファイルがあるディレクトリ）に置いています。別の場所に置きたい場合には cd で MediaPipe のルートに移動するようにするとよいでしょう。

※[前回の記事](https://qiita.com/Entis/items/e470ce54e70244aaf4a9)で、Windows でジャンクション越しのパスが通らない、と書きましたが、それは誤りで自動的に生成される "bazel-bin" という名前が問題なのか、exe ファイルのパスをダブルクォーテーションで囲めば大丈夫です。


## MediaPipeのフレームワーク

　MediaPipe は DirectShow のフィルターノードのように、ノードの入力に別のノードの出力を接続していくことで処理を実現するフレームワークのようです。
　画像認識してランドマークを出力するノードが用意されているので、そういうのを繋げていくと欲しい出力を生成できます。
　ノードの処理は Calculator で供給するようなので、カメラ画像を処理して得られた人物のランドマークを受け取ってそのまま出力する Calculator を自作し、そのノードからデータを送信することにしました。

　まず処理フレームワークとして run_graph_main.cc にサンプルコードの demo_run_graph_main.cc をコピーして持ってきます。
　ここに

```run_graph_main.cc
#include "CaptureServerHost.h"
```

を追加し、main 関数の冒頭に

```run_graph_main.cc
int main(int argc, char** argv)
{
	Socket::Initialize() ;
	CaptureServerHost	host ;
	if ( !host.BeginServer() )
	{
		LOG(ERROR) << "failed to begin server." ;
	}
```

を挿入し、終了時前に

```run_graph_main.cc
	host.EndServer() ;
```

を呼び出すようにします。

　ここで、CaptureServerHost クラスは何の変哲もない、認識した人物のランドマーク情報を送信するだけのサーバークラスです。
　また、Socket クラスも Socket をラップしただけのクラスで、Socket::Initialize 関数ではソケットライブラリの初期化を行います（Windows の場合、WSAStartup をソケットを利用する前に呼び出す必要があります）。
　これらの処理は CaptureServerHost.h 及び CaptureServerHost.cpp に記述しました。
　取り立てて特筆するほどの内容でもないので、ここでは詳細は省略します。


## ノードの Calculator 定義

　人物のランドマークを受け取る Calculator を CaptureServerCalculator.h 及び CaptureServerCalculator.cpp に記述します。

```CaptureServerCalculator.h
#include <cmath>

#include "mediapipe/framework/calculator_framework.h"
#include "mediapipe/framework/calculator_options.pb.h"
#include "mediapipe/framework/formats/landmark.pb.h"
#include "mediapipe/framework/formats/rect.pb.h"
#include "mediapipe/framework/port/ret_check.h"
#include "mediapipe/framework/port/status.h"

namespace mediapipe {

class CaptureServerCalculator : public CalculatorBase
{
public:
	static mediapipe::Status GetContract( CalculatorContract* cc ) ;
	mediapipe::Status Open( CalculatorContext* cc ) override ;
	mediapipe::Status Process( CalculatorContext* cc ) override ;
};
REGISTER_CALCULATOR(CaptureServerCalculator);

}
```

　CaptureServerCalculator.h はこれだけです。
　ここで「CaptureServerCalculator」という独自の Calculator を定義しました。この名前でノードグラフ（MediaPipe の場合、何と言うのが正式名称なんでしょう？）を構築する際に、ノードの Calculator として使用できます。


### GetContract関数

　この CaptureServerCalculator は、顔と手のランドマークと画像を入力し、画像を出力する Calculator です。
　入力や出力の情報は GetContract メソッドに以下のように記述します。

```CaptureServerCalculator.cpp
static constexpr char szLandmarksTag[] = "LANDMARKS" ;
static constexpr char szHandednessTag[] = "HANDEDNESS" ;
static constexpr char szPalmDetectionsTag[] = "PALM_DETECTIONS" ;
static constexpr char szFaceLandmarksTag[] = "FACE_LANDMARKS" ;
static constexpr char szPoseLandmarksTag[] = "POSE_LANDMARKS" ;
static constexpr char szInputImageTag[] = "IMAGE" ;
static constexpr char szOutputImageTag[] = "IMAGE" ;

mediapipe::Status
	 CaptureServerCalculator::GetContract( CalculatorContract* cc )
{
	cc->Inputs().Tag(szLandmarksTag).
			Set< std::vector<mediapipe::NormalizedLandmarkList> >() ;
	cc->Inputs().Tag(szHandednessTag).
			Set< std::vector<mediapipe::ClassificationList> >() ;
	cc->Inputs().Tag(szPalmDetectionsTag).
			Set< std::vector<mediapipe::Detection> >() ;
	cc->Inputs().Tag(szFaceLandmarksTag).
			Set< std::vector<mediapipe::NormalizedLandmarkList> >() ;
//	cc->Inputs().Tag(szPoseLandmarksTag).Set<mediapipe::NormalizedLandmarkList>() ;
	cc->Inputs().Tag(szInputImageTag).Set<mediapipe::ImageFrame>() ;
	cc->Outputs().Tag(szOutputImageTag).Set<mediapipe::ImageFrame>() ;
	return	mediapipe::OkStatus() ;
}
```

　コメントアウトはポーズデータの入力です。今は外してあります。
　データの型はランタイムで判定され、間違えたノードの繋ぎ方をしたり、GetContract で設定した型が間違えていると、実行時にエラーが出て終了すると思います。

### Openメソッド

　Open 関数では何もしていません。

```CaptureServerCalculator.cpp
mediapipe::Status
	CaptureServerCalculator::Open( CalculatorContext* cc )
{
	cc->SetOffset( TimestampDiff(0) ) ;
	return	mediapipe::OkStatus() ;
}
```

### Process メソッド

　実際のデータの受け取りと処理は Process メソッドで行います。
　例えば szLandmarksTag に対応する入力を取得するコードは以下のようになります。
（「NormalizedLandmarkList の配列」になっているのは、複数の人物を識別できるためだと思います）

```CaptureServerCalculator.cpp
mediapipe::Status
	CaptureServerCalculator::Process( CalculatorContext* cc )
{
	if ( !cc->Inputs().Tag(szLandmarksTag).IsEmpty() )
	{
		const std::vector<NormalizedLandmarkList>& landmarks =
		    cc->Inputs().Tag(szLandmarksTag).
				Get< std::vector<NormalizedLandmarkList> >() ;
```

　ランドマークは、ざっくり言うと3次元座標の配列が渡されます。mediapipe::NormalizedLandmarkList 型に該当します。
　配列サイズは landmark_size メソッドで取得し、要素は mediapipe::NormalizedLandmark 型です。
　座標の各要素は x, y, z メソッドで取得できます。
　ですので、取得するコードは以下のような感じになります。

```
// const mediapipe::NormalizedLandmarkList& list
const int	size = list.landmark_size() ;
for ( int i = 0; i < size; i ++ )
{
	const mediapipe::NormalizedLandmark&	landmark = list.landmark( i ) ;
	landmark.x() ;  // x 座標
	landmark.y() ;  // y 座標
	landmark.z() ;  // z 座標
}
```

　NormalizedLandmark の x と y 座標は入力画像の幅と高さに対応して 0～1 に正規化されています。z は x のスケールでの奥行き（プラスが奥）ですが、入力データによっては学習が十分ではなく使えないようです（ポーズデータなど）。

　この CaptureServerCalculator ではランドマークの座標データなどをそのまま送信するだけですので、処理は左右の手の判定などを行う程度で以下のようにそのままデータを受け渡しています。

```CaptureServerCalculator.cpp
const ImageFrame&	image = cc->Inputs().Tag(szInputImageTag).Get<ImageFrame>() ;
CaptureServerHost *	pServer = CaptureServerHost::GetInstance() ;
if ( pServer != nullptr )
{
	pServer->PostData
		( image.Width(), image.Height(), pnllFace, pnllLHand, pnllRHand ) ;
}
```

　CaptureServerHost::GetInstance 関数は、main 関数内の host インスタンスを取得しています。static メンバにインスタンスのポインタを保持していて、それを返しているだけの簡単なものです。

　因みに画像データは受け取ったデータをそのまま出力しているので、以下のように書いて関数を終了しています。

```CaptureServerCalculator.cpp
cc->Outputs()
    .Tag(szOutputImageTag).AddPacket
		( cc->Inputs().Tag(szInputImageTag)
						.Value().At(cc->InputTimestamp()) ) ;
return mediapipe::OkStatus() ;
```


##ノードグラフの記述

　ノードの繋ぎ方は CaptureServer.pbtxt に記述しました。
　サンプルを参考に画像から顔と手を探せるようにノードを構築します。
　CaptureServerCalculator ノードは以下のように記述しました。

```CaptureServer.pbtxt
input_stream: "input_video"
output_stream: "output_video"

#（中略）

node {
  calculator: "CaptureServerCalculator"
  input_stream: "IMAGE:output_video3"
  input_stream: "LANDMARKS:landmarks"
  input_stream: "HANDEDNESS:handedness"
  input_stream: "PALM_DETECTIONS:multi_palm_detections"
  input_stream: "FACE_LANDMARKS:multi_face_landmarks"
  output_stream: "IMAGE:output_video"
}
```


# 3Dアバターを動かす

　既存のアバターを動かすプログラムに、MediapPipe を利用したトラッキング機能を追加します。
　因みに、既存のプログラムは HMD とコントローラーのトラッキングを利用したものと、Intel RealSence SDK を利用したものを実装していましたが、ここに新たにトラッキング方法を追加することになります。


## 顔

　顔の様々な情報を取り出すために、顔メッシュの頂点の内、抽出する頂点指標を定義しておきます。

```
enum	FaceLandmarkIndex
{
	faceLandmarkOvalTop		= 10,
	faceLandmarkOvalBottom	= 152,
	faceLandmarkOvalLeft	= 234,
	faceLandmarkOvalRight	= 454,
	faceLandmarkLEyeLeft	= 33,
	faceLandmarkLEyeRight	= 133,
	faceLandmarkLEyeTop		= 159,
	faceLandmarkLEyeBottom	= 145,
	faceLandmarkREyeLeft	= 382,
	faceLandmarkREyeRight	= 263,
	faceLandmarkREyeTop		= 386,
	faceLandmarkREyeBottom	= 374,
	faceLandmarkLipLeft		= 78,
	faceLandmarkLipRight	= 308,
	faceLandmarkLipTop		= 13,
	faceLandmarkLipBottom	= 14,
	faceLandmarkMaxIndex	= 454,
} ;
```

　顔メッシュの頂点の順序については、公式なドキュメントのどこを見ればいいのか分からなかったので、FaceLandmarksToRenderDataCalculator のソースコード face_landmarks_to_render_data_calculator.cc を見て、ある程度あたりを付けてから、実際に描画して使えそうな頂点を探しました。
![face_landmark_index.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/285025/0b752f9d-c092-b415-a63d-331580b2f284.png)


### 顔の回転行列

　顔の回転行列は以下のようなコードで計算しました。

```
	S3DMatrix	matF( landmarks.m_sizeFrame.w,
						landmarks.m_sizeFrame.h,
						landmarks.m_sizeFrame.w ) ;
	const S3DVector *	pvLandmark = landmarks.m_pFace->GetConstArray() ;
	S3DVector	vOvalLR = matF * pvLandmark[faceLandmarkOvalRight]
							- matF * pvLandmark[faceLandmarkOvalLeft] ;
	S3DVector	vOvalBT = matF * pvLandmark[faceLandmarkOvalBottom]
							- matF * pvLandmark[faceLandmarkOvalTop] ;
	S3DVector	vOvalZ = vOvalLR * vOvalBT ;

	S3DMatrix	matHeadZ( 1, 1, 1 ) ;
	matHeadZ.RevolveByAngleOn( vOvalZ ) ;

	S3DMatrix	matHeadRZ( 1, 1, 1 ) ;
	matHeadRZ.VectorRotationOf( matHeadZ * vOvalLR, S3DVector( 1, 0, 0 ) ) ;

	matHead = (matHeadRZ * matHeadZ).Inverse() ;
```
　
　自前の 3D ライブラリを利用していることと、アバターモデルはｚ方向に向かって対面で作ってあること（一般的な 3D ゲーム等ではｚに順方向）、アバターのトラッキングを左右逆（鏡映し）にしているので、応用する場合には注意が必要です。

　S3DMatrix は 3x3 行列で、3つの引数で構築する場合には対角行列で初期化しています。
　自前の 3D ライブラリの座標系は、ｘ右、ｙ下、ｚ奥方向、という世にも珍しい座標系を採用しています（一般的なライブラリや3DCGソフトに一致する座標系はない………と思っていましたが、Vulkan と同じ？らしい？？？）。そして、MediaPipe の NormalizedLandmark の座標系とも一致していますが、異なる場合には matF の初期化パラメータの符号を反転してください。
　landmarks.m_sizeFrame.w は画像幅、landmarks.m_sizeFrame.h は画像高で、matF はアスペクト比を含め 3D 処理をする空間へ変換する行列になっています。

　また、S3DVector 同士の積はベクトルの外積を計算し、vOvalZ は顔の後頭部（z+）方向を向いています（座標系に注意）。
　そして RevolveByAngleOn は任意ベクトル（この場合 vOvalZ）を (0,0,z) へ回転する行列を計算します。
　VectorRotationOf では matHeadZ * vOvalLR を (1,0,0) へ回転する行列を計算します（vOvalZ 周りのジンバル回転）。
　最終的に matHeadRZ * matHeadZ の逆行列を顔の回転行列として得ます。


### 顔の座標とキャリブレーション

　顔の3次元的な座標を計算する上で z 座標は MediaPipe のランドマーク座標から得ることはできません。
　カメラの画角が分かっていれば原理上は絶対的な z 座標を計算できますが、プログラムはカメラの画角情報を持っていません（MediaPipe には写真ファイルに含まれるメタ情報から画角を取得し絶対的なｚ座標を計算するフィルタも含まれていますが、一般的なビデオカメラでは出来ません）。
　そこで本プログラムでは、カメラまでの距離と両目間の距離をあらかじめキャリブレーション情報として入力しておき、キャリブレーションを実行すると、その時点の座標（及び顔の向きを正面として）を基準に3Dアバターへ反映させる座標を修正しています。

　ｚ座標の計算を行うには、キャリブレーション時にまず以下のようなコードで zScale を求めます。

```
	S3DVector	vEyesLR = matF * ((pvLandmark[faceLandmarkREyeLeft]
								+ pvLandmark[faceLandmarkREyeRight]
								- pvLandmark[faceLandmarkLEyeLeft]
								- pvLandmark[faceLandmarkLEyeRight]) * 0.5f) ;

	const float32_t	CameraZ = <キャリブレーション時のカメラからの距離> ;
	const float32_t	EyeAbsLen = <両目の間隔> ; 
	zScale = vEyesLR.Absolute() / EyeAbsLen * CameraZ ;
```

　vEyesLR はランドマークから計算した両目間の距離（ランドマーク座標空間）です。
　CameraZ と EyeAbsLen の単位はアバターモデルの座標空間の単位に合わせます（モデルがメートル単位ならメートル）。

　そして計算済み（キャリブレーション済み）の zScale を用いて目までの距離を以下のように計算できます。

```
	float32_t	zEye = EyeAbsLen * zScale / vEyesLR.Absolute() ;
```

　この zEye と、3次元座標を求めたい顔のランドマーク座標 vFacePos から以下のように vHeadPos を計算しています。

```
	S3DVector	vFacePos = (matF * pvLandmark[faceLandmarkOvalRight]
							+ matF * pvLandmark[faceLandmarkOvalLeft]) * 0.5f ;
	S3DVector	vEyePos = (matF * pvLandmark[faceLandmarkLEyeLeft]
							+ matF * pvLandmark[faceLandmarkREyeRight]) * 0.5f ;
	vHeadPos.x = vFacePos.x - (float32_t) landmarks.m_sizeFrame.w * 0.5f ;
	vHeadPos.y = vFacePos.y - (float32_t) landmarks.m_sizeFrame.h * 0.5f ;
	vHeadPos.z = zEye + (vFacePos.z - vEyePos.z) / zScale * CameraZ　;
	vHeadPos.x *= vHeadPos.z / zScale * CameraZ　;
	vHeadPos.y *= vHeadPos.z / zScale * CameraZ　;
```

　ここで求めた vHeadPos は設置しているカメラの視線方向を z+ とする空間の座標です。
　これをキャリブレーション時の顔の正面方向を z- とする空間へ変換するには、キャリブレーション時の顔の回転行列と座標を使って以下のように matWorld, vHeadWorld を計算しておきます。

```
	S3DMatrix	matInitHead = <キャリブレーション時の顔の回転行列> ;
	S3DVector	vInitHeadPos = <キャリブレーション時の顔の座標> ;
	S3DMatrix	matWorld = matInitHead.Inverse() ;
	S3DVector	vHeadWorld = matWorld * -vInitHeadPos ;
```

　そしてキャリブレーションされた空間の顔の回転行列と座標を以下のように計算します。

```
	S3DMatrix	matNormalHeadAngle = matWorld * matHead ;
	S3DVector	vNormalHeadPos = matWorld * vHeadPos + vHeadWorld ;
```


### 目と口

　目閉じ度合いと、口開け具合は、以下のように顔の輪郭の上下の（3次元的な）長さに対する比を計算し、この値から 0～1 の値に正規化した値を最終的に求めています。

```
	S3DVector	vOvalBT = matF * pvLandmark[faceLandmarkOvalBottom]
							- matF * pvLandmark[faceLandmarkOvalTop] ;
	S3DVector	vEyeL = matF * pvLandmark[faceLandmarkLEyeBottom]
							- matF * pvLandmark[faceLandmarkLEyeTop] ;
	S3DVector	vEyeR = matF * pvLandmark[faceLandmarkREyeBottom]
							- matF * pvLandmark[faceLandmarkREyeTop] ;
	S3DVector	vMouth = matF * pvLandmark[faceLandmarkLipBottom]
							- matF * pvLandmark[faceLandmarkLipTop] ;
	double	fpOvalBT = vOvalBT.Absolute() ;
	fpEyeL = vEyeL.Absolute() / fpOvalBT ;
	fpEyeR = vEyeR.Absolute() / fpOvalBT ;
	fpMouth = vMouth.Absolute() / fpOvalBT ;
```

　しかし（特に目は）綺麗な値は取れません（目を閉じてもランドマークの点はくっつくほど接近しません）。
　そこで直近フレームの値を使った平均、標準偏差、最小値を求め、以下のようにそれらの値を使って正規化した値を生成しています。
　毎フレーム値を更新するために

```math
\mu = \frac{1}{n} \sum_{i=1}^{n} x_i \\
\sigma^2 = \frac{1}{n} \sum_{i=1}^{n} x_i^2 - \mu^2
```

の公式を利用します。

```
	fpCurEye = <ランドマークから求めた目の垂直幅の、顔輪郭高に対する比率> ;

	nEyeSampleCount ++ ;						// サンプル数
	fpSumEye += fpCurEye ;						// 合計
	fpSig2Eye += fpCurEye * fpCurEye ;			// 二乗の合計
	fpAvgEye = fpSumEye / nEyeSampleCount ;		// 平均
	fpSigmaEye =								// 標準偏差
		sqrt( fpSig2Eye / nEyeSampleCount
					- fpAvgEye * fpAvgEye ) ;
	if ( fpCurEye < fpMinEye )
	{
		fpMinEye = fpCurEye ;					// 最小値
	}
	if ( nEyeSampleCount >= 600 )				// 300フレーム毎に半分の重み
	{
		fpSumEye *= 0.5 ;
		fpSig2Eye *= 0.5 ;
		nEyeSampleCount /= 2 ;
	}

	double	fpEyeHeight = fpAvgEye - fpSigmaEye - fpMinEye ;
	fpNormalEye = 1.0 ;
	if ( fpEyeHeight > 0.0 )
	{
		fpNormalEye = (fpCurEye - fpMinEye) / fpEyeHeight ;  // 正規化された値
	}
```


## 手

　手のランドマークの頂点指標を定義しておきます。

```
enum	HandLandmarkIndex
{
	hadLandmarkWrist,
	hadLandmarkThumbCMC,
	hadLandmarkThumbMCP,
	hadLandmarkThumbIP,
	hadLandmarkThumbTIP,
	fingerLandmarkIndexMCP,
	fingerLandmarkIndexPIP,
	fingerLandmarkIndexDIP,
	fingerLandmarkIndexTIP,
	fingerLandmarkMiddleMCP,
	fingerLandmarkMiddlePIP,
	fingerLandmarkMiddleDIP,
	fingerLandmarkMiddleTIP,
	fingerLandmarkRingMCP,
	fingerLandmarkRingPIP,
	fingerLandmarkRingDIP,
	fingerLandmarkRingTIP,
	fingerLandmarkPinkyMCP,
	fingerLandmarkPinkyPIP,
	fingerLandmarkPinkyDIP,
	fingerLandmarkPinkyTIP,
	handLandmarkCount,
} ;
```

　この指標はドキュメントに分かりやすく載っているので、顔メッシュと違って分かりやすいです。
![hand_landmark_index.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/285025/7e038de2-440a-46d4-ffc2-a8900add1cc4.png)



### 手の座標

　手の座標は、顔の座標と同じ要領で計算できます。
　顔のキャリブレーションで計算した zScale と matWorld, vHeadWorld、及び、手のひら（手首から指の根本まで）の長さを用いて以下のように座標を計算しています。

```
	// カメラからの絶対ｚ座標計算 => zHand
	S3DVector	vIndexDir = matF * pvLandmark[fingerLandmarkIndexMCP]
							- matF * pvLandmark[hadLandmarkWrist] ;
	S3DVector	vPinkyDir = matF * pvLandmark[fingerLandmarkPinkyMCP]
							- matF * pvLandmark[hadLandmarkWrist] ;

	const float32_t	fpPalmLength =
					(float32_t) (vIndexDir.Absolute()
								+ vPinkyDir.Absolute()) * 0.5f ;
	const float32_t	CameraZ = <キャリブレーション時のカメラからの距離> ;
	const float32_t	PalmAbsLen = <手首から指の根本までの長さ> ;
	const float32_t	zScale = <キャリブレーション時に得たスケール> ;
	const float32_t	zHand = PalmAbsLen * zScale / fpPalmLength ;

	// 座標空間変換
	S3DVector	vLandmark[handLandmarkCount] ;
	for ( int i = 0; i < handLandmarkCount; i ++ )
	{
		vLandmark[i] = matF * pvLandmark[i] ;
		vLandmark[i].x -= (float32_t) sizeSrcImage.w * 0.5f ;
		vLandmark[i].y -= (float32_t) sizeSrcImage.h * 0.5f ;
	}

	// 逆ワールド変換 => vNormalHandPos
	S3DMatrix	matWorld = <キャリブレーションで求めたワールド行列> ;
	S3DVector	vHeadWorld = <キャリブレーションで求めたワールド座標> ;
	S3DVector	vHandPos = vLandmark[hadLandmarkWrist] ;
	vHandPos.z = zHand + (vHandPos.z - pvLandmark[hadLandmarkWrist].z) / zScale * CameraZ ;
	vHandPos.x *= vHandPos.z / zScale * CameraZ ;
	vHandPos.y *= vHandPos.z / zScale * CameraZ ;
	S3DVector	vNormalHandPos = matWorld * vHandPos + vHeadWorld ;
```

　PalmAbsLen は顔の時ど同様、アバターモデルのスケールの座標空間の単位に合わせます。
　sizeSrcImage.w, sizeSrcImage.h はカメラ画像の幅と高さです。
　座標空間を変換して vLandmark 配列に格納していますが、この座標は後でアバターの手のボーンを計算する際に使います。
　この座標は本来なら matWorld で回転すべきかと思いますが、コードがまずかったのか、画像認識のz精度の問題か、少し歪んだように見えたので matWorld で回転していません。

　この処理を左右両手分行います。

### 手首の回転

　手の回転行列は以下のように計算しています。

```
	S3DVector	vPIDir = matF * pvLandmark[fingerLandmarkPinkyMCP]
							- matF * pvLandmark[fingerLandmarkIndexMCP] ;
	S3DVector	vHandTipDir = vIndexDir + vPIDir * 0.5f ;
	vHandTipDir.Normalize() ;

	S3DMatrix	matHandZ( 1, 1, 1 ) ;
	matHandZ.RevolveByAngleOn( vHandTipDir ) ;

	S3DVector	vThumbDir = -vPIDir ;
	vThumbDir -= vHandTipDir * vHandTipDir.InnerProduct( vThumbDir ) ;

	S3DMatrix	matHandRZ( 1, 1, 1 ) ;
	matHandRZ.VectorRotationOf
		( matHandZ * vThumbDir, S3DVector( fpThumbSign, 0, 0 ) ) ;

	S3DMatrix	matHand = (matHandRZ * matHandZ).Inverse() ;
```

　基本的に顔の回転行列の求め方と同じです。
　fpThumbSign は右手と左手の違いです（親指の方向を 1 又は -1 で指定）。
　アバターに鏡映しで反映させているので、右手の回転をアバターモデルの左手に、左手の回転をアバターモデルの右手へ反映させています。


## ボーンへの反映

　表示する 3D アバターモデルは、FBX や glTF などから独自の形式に変換したモデルを使用しています。
　その際、ボーンは正規化され、ニュートラル状態のボーンの回転行列は単位行列になるように変換されています。子ボーンは親ボーンからの相対座標を持っていますが、正規化されているのでニュートラル状態では絶対座標空間の差分と一致しています。


### 顔の位置と向き

　顔の位置は腰のボーン回転で、顔の向きは顔のボーン（顔の付け根ボーン）の回転へ反映させます。

　座標をボーンの回転に反映させる場合、私の自前ライブラリではボーンハンドルと呼んでいるものを使います。
　ボーンハンドルとは、そのボーンのローカル空間での位置ベクトルで、任意の位置ベクトルを与えるとボーンハンドルがそのベクトルの向きへ回転するという抽象物です。

　具体的には、ボーンの操作前の（絶対）回転行列を M、ボーンの（絶対）座標を b、ボーンハンドル（ローカルベクトル）を h、操作（絶対）座標を p とすると、新しいボーンの（絶対）回転行列は

```math
R( M \cdot h, (p - b) ) \cdot M
```

で得られます。
　ここで、R(a,b) はベクトル a を回転してベクトル b と平行にする

```math
b = \frac{ \begin{vmatrix} b \end{vmatrix} }{ \begin{vmatrix} a \end{vmatrix} } R(a,b) \cdot a
```

となるような回転行列です。
（具体的には、[以前書いた記事の「ベクトル・行列（自分用メモ）」](https://qiita.com/Entis/items/71b66a9ef92cc960488b)に相当するものが自前ライブラリで実装されています）

　尚、顔の回転は、ニュートラル状態で正面（対面）を向いているので、matNormalHeadAngle をそのまま設定しています。
※実際には、顔の親ボーンの絶対回転行列を P とした時、P.Inverse() * matNormalHeadAngle を顔ボーンのローカルな回転行列として設定しています。


### 手の位置

　手の位置は、肩のボーンと肘のボーンを回転させる IK 処理を実施しています。
　これも自前ライブラリの機能を利用して実装しましたが、詳細はここでは省略します。

　なお、MediaPipe ではポーズフィルタもあるため、本来は肩の位置や肘の位置も（さらには膝や脚の位置も）推定することができます。しかし、ポーズフィルタは（MediaPipe の同梱されているサンプルも）動かなかったため今回は採用していません。
　使えなくてもバストアップの表示であれば IK 処理で大体なんとかなるので、原因について、Windows 環境では動かないのか、あるいは私の環境固有の問題により動かないのか、深く調べていないので良く分かりません。


### 手首の回転

　手首の回転は顔に比べると少し複雑です。
　なぜならアバターモデルのニュートラル状態の手がどのような向きになっているかは決まっていないためです。
　ですので、まずモデルを読み込んだら（ニュートラル状態の）ボーン座標と、肘の IK 用のベクトルを用いて以下のように手の逆行列を求めておきます。

```
	S3DDVector	vHandDir = <手の指先方向ベクトル> ;
	vHandDir.Normalize() ;

	S3DDMatrix	matHand( 1, 1, 1 ) ;
	matHand.VectorRotationOf( S3DDVector( 0, 0, 1 ), vHandDir ) ;

	S3DDVector	vThumb = <親指方向ベクトル> ;
	S3DDVector	vElbowFoldDir = <肘曲げ方向ベクトル> ; // =(0,0,-1)
	S3DDMatrix	matGimbal( 1, 1, 1 ) ;
	matGimbal.VectorRotationOf( vThumb, vElbowFoldDir ) ;

	S3DMatrix	matIHand = (matGimbal * matHand).Inverse() ;
```

　vHandDir と vThumb はボーンの位置から計算できますが、vElbowFoldDir はボーンの位置からは計算できません。親指が前に向いた状態であれば (0,0,-1) を使用します（対面向きのモデルの場合）。

　手のボーン（手首ボーン）の（絶対）回転行列は以下のようにします。

```
	S3DDMatrix	matHandBone = matHand * matIHand ;
```

※ここでの matHand は手のランドマークから計算した手首の回転行列


### 指の回転

　まずアバターモデルの各指ボーンに対応するランドマーク指標の配列を

```
	static const HandLandmarkIndex	s_iFingerRefBone[fingerBoneCount][2] =
	{
		{ hadLandmarkWrist, hadLandmarkThumbCMC },
		{ hadLandmarkThumbIP, hadLandmarkThumbTIP },
		{ fingerLandmarkIndexMCP, fingerLandmarkIndexPIP },
		{ fingerLandmarkIndexPIP, fingerLandmarkIndexDIP },
		{ fingerLandmarkIndexDIP, fingerLandmarkIndexTIP },
		{ fingerLandmarkMiddleMCP, fingerLandmarkMiddlePIP },
		{ fingerLandmarkMiddlePIP, fingerLandmarkMiddleDIP },
		{ fingerLandmarkMiddleDIP, fingerLandmarkMiddleTIP },
		{ fingerLandmarkRingMCP, fingerLandmarkRingPIP },
		{ fingerLandmarkRingPIP, fingerLandmarkRingDIP },
		{ fingerLandmarkRingDIP, fingerLandmarkRingTIP },
		{ fingerLandmarkPinkyMCP, fingerLandmarkPinkyPIP },
		{ fingerLandmarkPinkyPIP, fingerLandmarkPinkyDIP },
		{ fingerLandmarkPinkyDIP, fingerLandmarkPinkyTIP },
	} ;
```

と定義します。これはボーンの根本→指先の順のランドマークの指標です。

　そして以下のような手順でボーンの回転行列を設定していきます。

```
	for ( int i = 0; i < fingerBoneCount; i ++ )
	{
		S3DModelBoneSpace *	pBone = ik.pFinger[i] ;
		S3DModelBoneSpace *	pParent = pBone->GetParentBone() ;
		pParent->CalcGlobalTransformation( matParentBone, vParentBone ) ;

		S3DDVector	vBoneHandle = matParentBone * pBone->GetBoneHandle() ;
		S3DDVector	vCaptured = (hpa.vLandmark[s_iFingerRefBone[i][1]]
									- hpa.vLandmark[s_iFingerRefBone[i][0]]) ;

		S3DDMatrix	matRot( 1, 1, 1 ) ;
		matRot.VectorRotationOf( vBoneHandle, vCaptured ) ;

		pBone->SetLocalTransformation( matParentBone.Inverse() * matRot * matParentBone ) ;
	}
```


# 最後に

　以上で MediaPipe のトラッキング情報から3Dアバターをリアルタイムで動かすことが出来ると思います。
　要点のみに絞って書いたつもりですが、それでも実際にプログラムを組んで動作確認をするのにかかったのより、この記事を書くのにかかった時間の方が長くなってしまったような気がします。きっと気のせいでしょう。