# 3D Rehabilitation System
## 針孔成像模型
針孔相機模型如圖(一)所示，並且可以如圖(二)所示用由內在參數以及外在參數組成的數學式作表示。在三維空間中的座標點先經由外在參數轉換到以相機為中心的坐標系，並由內在參數將三維座標點投影到二維平面上，如圖(三）所示。內在參數以及外在參數除了是重建三維骨架的必備條件之外，校正的精準度也對重建三維骨架有很大影響。
圖(ㄧ) 針孔相機模型。

[comment]: <> (Latex Equation : $\begin{bmatrix} u\\ v\\ 1\\ \end{bmatrix} = \begin{bmatrix} f_s & s & x_0 \\ 0 & f_y & y_0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} r_{11} & r_{12} & r_{13} & t_1 \\ r_{21} & r_{22} & r_{23} & t_2 \\ r_{31} & r_{32} & r_{33} & t_13\\ \end{bmatrix} \begin{bmatrix} X\\ Y\\ Z\\ 1 \end{bmatrix}$)
圖(二) 針孔相機模型之數學式。

圖(三) 使用內在參數和外在參數之流程圖。

### 校正內在參數以及Distortion Coefficients
圖(三) 內在參數之數學式

內在參數將三維空間中的座標點投影到二維成像平面上。如圖(三)所示，相機內在參數可分為三個部分：`scaling`，`translation`,，以及`sheer`。 `scaling`用等比轉換將三維空間點投影到二維平面上；`translation`修正相機`focal point`沒有和`film origin`之間的差距；`Sheer`修正`Sheer effect`。

針孔相機模型假沒有假設鏡片，然而我們使用的攝影機有使用到鏡片。因此`Distortion Coefficient`修正光線通過鏡片因為折射而產生的影像扭曲，如圖(A)，以及鏡片和成像平面不平行導致的扭曲，如圖(B)。
圖(A) 鏡片折射導致影像扭曲。

圖(B) 鏡片和成像平面不平行。

## 校正外在參數
外在參數則是描述多相機系統之下，對每一台相機而言以該相機為中心的坐標系和多相機系統座標系之間的變換矩陣。經過外在參數轉換之後，三維點在多相機座標系中的座標可以轉換成在相機為中心的坐標系中的座標。外在參數如圖(四)所示由`Rotation`以及`Translation`組成，對應到轉換兩個座標系所需要的`Rotation Matrix`和`Translation Matrix`。

圖(四) 外在參數之數學式。

我們使用`solvePNP`對下列已知參數：
* 2D對應點
* 3D對應點
* 相機內在參數
* 相機`Distortion Coefficients`

解出`Rotation Matrix`以及`Translation Matrix`。

使用Aruco Marker所得到的外在參數是將以Aruco Marker為中心的坐標系轉換到以相機為中心的坐標系的矩陣。為了符合OpenPose設計方式，在實作上由使用者任意挑一台相機當作原點，其餘相機會求出和以使用者挑選的相機為中心的多相機座標之間的外在參數。

## 計算投影誤差以評估外在參數準確性
1. 用`Aruco Marker Detector`偵測`Aruco Marker`四個角在照片中的二維坐標。
2. 套用針孔成像模型的概念，將同一個角在所有相機中偵測到的(1)二維坐標，(2)校正得到的相機內在參數，(3)校正得到的相機內在參數，透過奇異值分解解出重建的三維坐標。
3. 再套用針孔成像模型，將重建的三維坐標透過每台相機的內在參數和外在參數重新投影到二維平面。
4. 透過計算偵測到之二維座標和重新投影之二維座標之間的誤差，將著個誤差當作投影誤差評估校正得到的外在參數的準確性。

## RanSAC
當3台相機重建出的3D點投影誤差過大時，我們用下列動作移除造成投影誤差過大的相機：
1. 我們從3台相機之中依序刪除每一台。
2. 用剩下兩台相機重建3D點。
3. 從所有用兩台相機重建出的3D點中選出投影誤差最小的點，用該點當作重建的3D座標。

## Kalman Filter
根據上一個state經過最佳化的結果和這一個state得到的觀察對這一個state的結果做最佳化。`Kalman filter`可以從包含雜訊的系統中估計穩定的observation。

**Overview**
**Predict**
Predict the state of k-1 timestep.
1. $\hat{x}_{k-1}, P_{k-1}$ `Optimized measurements of k-1 timestep`

    cv::KalmanFilter::statePost 
    cv::KalmanFilter::errorCovPost

2. $\hat{x}_{k}, P_{k}$ `Predicted states of k timestep`

    cv::KalmanFilter::predict() 

The prediction is based on the optimized measurements of k-1 timestep
3. $F_k$ `Transition Matrix`

    cv::KalmanFilter::transitionMatrix

`Transition matrix` models the transition model of the system.
> Example

> Transition Model
$p_k=p_{k-1}+\Delta tv_{k-1}$ → New Position = Old Position + Elapsed time * Old Velocity
$v_k=v_{k-1}$ → New Velocity = Old Velocity

> With Transition Matrix $F_k$ and state $\hat{x}_k$
$\hat{x}_k=F_k \hat{x}_{k-1,}$
$F_k=\begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix}$
$\hat{x}_k=\begin{bmatrix}p_k\\ v_k\end{bmatrix}$

4. $B_k, \vec{u}_k$
`Control Vector` and `Control Matrix`.`Control Vector` describes variables that are not related to the state, yet affect the system; `Control Matrix` describes how the `Control Vector` affects the system.

5. $Q_k$ `Noise Covariance Matrix`

    cv::KalmanFilter::processNoiseCov

`Noise Covariance Matrix` models the uncertaintly not associated to the world(the system is not tracking the uncertainty.)

**Update**
1. $\hat{x}^{'}_{k}, P^{'}_{k}$ : `Optimized measurement of k timestep`
2. $H_k$ `measurement matrix`

    cv::KalmanFilter::measurementMatrix

`measurement matrix` models the way to map from the state space to the observation space.
3. $\vec{z}_k$ `observed measurement`

    cv::KalmanFilter::correct(const Mat& measurement)

`observed measurement` is what we observed from sensors. For example: position and velocity from the GPS and the dashboard.
4. $R_k$ `measurement noise covariance matrix`

    cv::KalmanFilter::measurementNoiseCov

Measurement noise is drawn from zero mean Gaussian model with `measurement noise covariance matrix`.

How much can we trust the observed measuremnet?

## 骨架動作分析
我們將OpenPose骨架重建系統和骨架動作分析整合在一起，讓使用者能夠透過系統進行復健。我們計算出復健動作關節點之間的夾角和肢體的長度，並及時將夾角角度告訴使用者，讓使用者能夠知道自己動作的正確性。

我們取出相關的關節點，用關節點算出構成夾角的肢體之向量，並且算出兩個向量之間的夾角，如圖(十二)所示。

圖(十二) 取得向量之間的夾角。

我們同時使用向量算出該肢體的實際長度，如圖(十三)所示。
圖(十三) 三維坐標和關節點部位的對應關係。

#### 實驗誤差
受測者手臂長度：`0.23m/0.23m`
預測手臂長度 ：`0.33m/0.26m`