EUCM投影模型推導

• 世界座標系下的位姿
  $T_{cw} = \{ r_1, r_2, r_3, t_1, t_2, t_3 \}$

• 相機本體座標系下的三維點
  $P_c = \{ p_1, p_2, p_3 \}$

• 預定義：
  $\rho = \sqrt[]{\beta*(p_1^2+p_2^2)+p_3^2}$
  $\eta = (1 - \alpha)*p_3 + \alpha * \rho$

• EUCM投影過程
  $proj_0 = \frac {p_1}{(1-\alpha)*p_3+\alpha*\rho} = \frac{p_1}{\eta}$
  $proj_1 = \frac {p_2}{ (1-\alpha)*p_3+\alpha*\rho} = \frac {p_2}{\eta}$
  $res_0 = proj_0 * f_x + c_x$
  $res_1 = proj_1 * f_y + c_y$

• 三維點重投影殘差計算
  $err_0 = obs_u - res_0$
  $err_1 = obs_v - res_1$

令
$\mathbf{e} = \begin{bmatrix} err_1\\ err_2 \end{bmatrix}$

• 三維點重投影殘差對相機本體座標系下三維點的雅可比計算

$\frac{\partial err_0}{\partial p_1} = -\frac{\partial res_0}{\partial p_1} = -f_x * \frac{\partial proj_0}{\partial p_1}$

其中，
$\frac{\partial proj_0}{\partial p_1} = \frac{\partial (p_1*\eta^{-1})}{\partial p_1} = \eta^{-1}+[p_1*(-\eta^{-2})*\frac{\partial \eta}{\partial p_1}]$

其中，
$\frac{\partial \eta}{\partial p_1}=\alpha * \frac{\partial \rho}{\partial p_1}$

其中，
$\frac{\partial \rho}{\partial p_1} = \frac {\beta * p_1}{\rho}$

從而，
$\frac{\partial err_0}{\partial p_1} = -f_x * (\frac{1}{\eta}-\frac{p_1}{\eta^{2}}*\alpha*\frac {\beta*p_1}{\rho}) = f_x * (-\frac{1}{\eta}+\frac{\alpha*\beta*p_1^2}{\rho*\eta^{2}})$

---

$\frac{\partial err_0}{\partial p_2} = -\frac{\partial res_0}{\partial p_2} = -f_x * \frac{\partial proj_0}{\partial p_2}$

其中，
$\frac{\partial proj_0}{\partial p_2} = \frac{\partial (p_1*\eta^{-1})}{\partial p_2} = p_1*(-\eta^{-2})*\frac{\partial \eta}{\partial p_2}$

其中，
$\frac{\partial \eta}{\partial p_2}=\alpha * \frac{\partial \rho}{\partial p_2}$

其中，
$\frac{\partial \rho}{\partial p_2} = \frac {\beta * p_2}{\rho}$

從而，
$\frac{\partial err_0}{\partial p_2} = -f_x * (-\frac{p_1}{\eta^{2}}*\alpha*\frac {\beta*p_2}{\rho}) = f_x * (\frac{\alpha*\beta*p_1*p_2}{\rho*\eta^{2}})$

---

$\frac{\partial err_0}{\partial p_3} = -f_x * \frac{\partial proj_0}{\partial p_3}$

其中，
$\frac{\partial proj_0}{\partial p_3} = \frac{\partial (p_1*\eta^{-1})}{\partial p_3} = p_1*(-\eta^{-2})*\frac{\partial \eta}{\partial p_3}$

其中，
$\frac{\partial \eta}{\partial p_3} = (1-\alpha) + \alpha*\frac{\partial \rho}{\partial p_3}$

其中，
$\frac{\partial \rho}{\partial p_3} = \frac {p_3}{\rho}$

從而，

$\frac{\partial err_0}{\partial p_3} = f_x * p_1*[(1-\alpha)+\frac{\alpha*p_3}{\rho}]*\frac{1}{\eta^2}$

---

根據對偶性，
$\frac{\partial err_1}{\partial p_1} = f_y * (\frac{\alpha*\beta*p_1*p_2}{\rho*\eta^{2}})$

---

$\frac{\partial err_1}{\partial p_2} = f_y * (-\frac{1}{\eta}+\frac{\alpha*\beta*p_2^2}{\rho*\eta^{2}})$

---

$\frac{\partial err_1}{\partial p_3} = f_y * p_2*[(1-\alpha)+\frac{\alpha*p_3}{\rho}]*\frac{1}{\eta^2}$

綜上，三維點重投影殘差對相機本體座標系下的三維點的雅可比矩陣爲：

$\frac{\partial \mathbf{e}}{\partial P_c} = \begin{bmatrix} f_x * (-\frac{1}{\eta}+\frac{\alpha*\beta*p_1^2}{\rho*\eta^{2}}) & f_x * (\frac{\alpha*\beta*p_1*p_2}{\rho*\eta^{2}}) & f_x * p_1*[(1-\alpha)+\frac{\alpha*p_3}{\rho}]*\frac{1}{\eta^2}\\ f_y * (\frac{\alpha*\beta*p_1*p_2}{\rho*\eta^{2}}) & f_y * (-\frac{1}{\eta}+\frac{\alpha*\beta*p_2^2}{\rho*\eta^{2}}) & f_y * p_2*[(1-\alpha)+\frac{\alpha*p_3}{\rho}]*\frac{1}{\eta^2} \end{bmatrix}$

從而，三維點重投影殘差對世界座標系下的三維點的雅可比矩陣爲：

$\frac{\partial \mathbf{e}}{\partial P_w} = \frac{\partial \mathbf{e}}{\partial P_c} \frac{\partial P_c}{\partial P_w} = \frac{\partial \mathbf{e}}{\partial P_c}*R_{cw}$

其中，$R_{cw}$是相機位姿$T_{cw}$的旋轉分量。

• 三維點重投影殘差對位姿的雅可比計算
  根據鏈式法則，

$\frac{\partial \mathbf{e}}{\partial T_{cw}} = \frac{\partial \mathbf{e}}{\partial P_c}*\frac{\partial P_c}{\partial T_{cw}}$

其中，
$\frac{\partial P_c}{\partial T_{cw}} =\begin{bmatrix} -\hat{P_c} & I_3 \end{bmatrix}$

其中，
$\hat{P_c} = \begin{bmatrix} 0 & -p_3 & p_2 \\ p_3 & 0 & -p_1 \\ -p_2 & p_1 & 0 \end{bmatrix}$