已知畸变坐标，内参矩阵、旋转矩阵、平移向量、求世界坐标，公式怎么写？

假设畸变坐标为 $(u_d,v_d)$，内参矩阵为 $K$，旋转矩阵为 $R$，平移向量为 $t$，则世界坐标为 $(X,Y,Z)$。

首先，需要将畸变坐标转换为归一化相机平面坐标 $(x_c,y_c)$，公式为：

$$\begin{bmatrix}x_c \\ y_c \\ 1\end{bmatrix} = K^{-1} \begin{bmatrix}u_d \\ v_d \\ 1\end{bmatrix}$$

然后，将归一化相机平面坐标转换为相机坐标 $(X_c,Y_c,Z_c)$，公式为：

$$\begin{bmatrix}X_c \\ Y_c \\ Z_c\end{bmatrix} = \begin{bmatrix}x_c \\ y_c \\ 1\end{bmatrix} \cdot \frac{\|t\|}{\sqrt{X_c^2 + Y_c^2 + \|t\|^2}}$$

接着，将相机坐标转换为世界坐标 $(X,Y,Z)$，公式为：

$$\begin{bmatrix}X \\ Y \\ Z\end{bmatrix} = R^{-1} \cdot \begin{bmatrix}X_c \\ Y_c \\ Z_c\end{bmatrix} - R^{-1} \cdot t$$

综上所述，世界坐标的计算公式为：

$$\begin{bmatrix}X \\ Y \\ Z\end{bmatrix} = R^{-1} \cdot \left(\begin{bmatrix}x_c \\ y_c \\ 1\end{bmatrix} \cdot \frac{\|t\|}{\sqrt{X_c^2 + Y_c^2 + \|t\|^2}}\right) - R^{-1} \cdot t$$

其中 $K^{-1}$ 表示 $K$ 的逆矩阵，$R^{-1}$ 表示 $R$ 的逆矩阵。