EUCM投影模型推导

• 世界坐标系下的位姿
  T c w = { r 1 , r 2 , r 3 , t 1 , t 2 , t 3 } T_{cw} = \{ r_1, r_2, r_3, t_1, t_2, t_3 \} Tcw​={r1​,r2​,r3​,t1​,t2​,t3​}

• 相机本体坐标系下的三维点
  P c = { p 1 , p 2 , p 3 } P_c = \{ p_1, p_2, p_3 \} Pc​={p1​,p2​,p3​}

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

• EUCM投影过程
  $$pro{j}_0=\frac{p_1}{(1-\alpha )\ast p_3+\alpha \ast \rho }=\frac{p_1}{\eta }$$
  $$pro{j}_1=\frac{p_2}{(1-\alpha )\ast p_3+\alpha \ast \rho }=\frac{p_2}{\eta }$$
  $$re{s}_0=pro{j}_0\ast f_x+c_x$$
  $$re{s}_1=pro{j}_1\ast f_y+c_y$$

• 三维点重投影残差计算
  $$er{r}_0=ob{s}_u-re{s}_0$$
  $$er{r}_1=ob{s}_v-re{s}_1$$

令
$$\mathbf{e}=\left[\begin{array}{c}er{r}_1\\ er{r}_2\end{array}\right]$$

• 三维点重投影残差对相机本体坐标系下三维点的雅可比计算

$$\frac{\partial er{r}_0}{\partial p_1}=-\frac{\partial re{s}_0}{\partial p_1}=-f_x\ast \frac{\partial pro{j}_0}{\partial p_1}$$

其中，
$$\frac{\partial pro{j}_0}{\partial p_1}=\frac{\partial (p_1\ast {\eta }^{-1})}{\partial p_1}={\eta }^{-1}+[p_1\ast (-{\eta }^{-2})\ast \frac{\partial \eta }{\partial p_1}]$$

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

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

从而，
$$\frac{\partial er{r}_0}{\partial p_1}=-f_x\ast (\frac{1}{\eta }-\frac{p_1}{{\eta }^2}\ast \alpha \ast \frac{\beta \ast p_1}{\rho })=f_x\ast (-\frac{1}{\eta }+\frac{\alpha \ast \beta \ast p_1^2}{\rho \ast {\eta }^2})$$

---

$$\frac{\partial er{r}_0}{\partial p_2}=-\frac{\partial re{s}_0}{\partial p_2}=-f_x\ast \frac{\partial pro{j}_0}{\partial p_2}$$

其中，
$$\frac{\partial pro{j}_0}{\partial p_2}=\frac{\partial (p_1\ast {\eta }^{-1})}{\partial p_2}=p_1\ast (-{\eta }^{-2})\ast \frac{\partial \eta }{\partial p_2}$$

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

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

从而，
$$\frac{\partial er{r}_0}{\partial p_2}=-f_x\ast (-\frac{p_1}{{\eta }^2}\ast \alpha \ast \frac{\beta \ast p_2}{\rho })=f_x\ast (\frac{\alpha \ast \beta \ast p_1\ast p_2}{\rho \ast {\eta }^2})$$

---

$$\frac{\partial er{r}_0}{\partial p_3}=-f_x\ast \frac{\partial pro{j}_0}{\partial p_3}$$

其中，
$$\frac{\partial pro{j}_0}{\partial p_3}=\frac{\partial (p_1\ast {\eta }^{-1})}{\partial p_3}=p_1\ast (-{\eta }^{-2})\ast \frac{\partial \eta }{\partial p_3}$$

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

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

从而，

$$\frac{\partial er{r}_0}{\partial p_3}=f_x\ast p_1\ast [(1-\alpha )+\frac{\alpha \ast p_3}{\rho }]\ast \frac{1}{{\eta }^2}$$

---

根据对偶性，
$$\frac{\partial er{r}_1}{\partial p_1}=f_y\ast (\frac{\alpha \ast \beta \ast p_1\ast p_2}{\rho \ast {\eta }^2})$$

---

$$\frac{\partial er{r}_1}{\partial p_2}=f_y\ast (-\frac{1}{\eta }+\frac{\alpha \ast \beta \ast p_2^2}{\rho \ast {\eta }^2})$$

---

$$\frac{\partial er{r}_1}{\partial p_3}=f_y\ast p_2\ast [(1-\alpha )+\frac{\alpha \ast p_3}{\rho }]\ast \frac{1}{{\eta }^2}$$

综上，三维点重投影残差对相机本体坐标系下的三维点的雅可比矩阵为：

$$\frac{\partial \mathbf{e}}{\partial P_c}=\left[\begin{array}{ccc}{f}_x\ast (-\frac{1}{\eta }+\frac{\alpha \ast \beta \ast p_1^2}{\rho \ast {\eta }^2})& f_x\ast (\frac{\alpha \ast \beta \ast p_1\ast p_2}{\rho \ast {\eta }^2})& f_x\ast p_1\ast [(1-\alpha )+\frac{\alpha \ast p_3}{\rho }]\ast \frac{1}{{\eta }^2}\\ f_y\ast (\frac{\alpha \ast \beta \ast p_1\ast p_2}{\rho \ast {\eta }^2})& f_y\ast (-\frac{1}{\eta }+\frac{\alpha \ast \beta \ast p_2^2}{\rho \ast {\eta }^2})& f_y\ast p_2\ast [(1-\alpha )+\frac{\alpha \ast p_3}{\rho }]\ast \frac{1}{{\eta }^2}\end{array}\right]$$

从而，三维点重投影残差对世界坐标系下的三维点的雅可比矩阵为：

$$\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}\ast R_{cw}$$

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

• 三维点重投影残差对位姿的雅可比计算
  根据链式法则，

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

其中，
$$\frac{\partial P_c}{\partial T_{cw}}=\left[\begin{array}{cc}-\widehat{P_c}& I_3\end{array}\right]$$

其中，
$$\widehat{P_c}=\left[\begin{array}{ccc}0& -p_3& p_2\\ p_3& 0& -p_1\\ -p_2& p_1& 0\end{array}\right]$$