论文阅读：

• 风机叶片无人机自主巡检系统控制算法研究
• 风机叶片的无人机自主巡检系统

将$\alpha f$记为$f_x$，表示将$\beta f$记为$f_y$，有：

$\left\{\begin{array}{l}u=f_x \frac{X}{Z}+c_x \\ v=f_y \frac{Y}{Z}+c_y\end{array}\right.$

以上公式写成矩阵形式有：

$\left(\begin{array}{l}u \\ v \\ 1\end{array}\right)=\frac{1}{Z}\left(\begin{array}{ccc}f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1\end{array}\right)\left(\begin{array}{l}X \\ Y \\ Z\end{array}\right)=\frac{1}{Z} K P$

将Z消除归一化为

$\left(\begin{array}{l}u \\ v \\ 1\end{array}\right)=\left(\begin{array}{ccc}f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1\end{array}\right)\left(\begin{array}{l}X / Z \\ Y / Z \\ 1\end{array}\right)=K\left(\begin{array}{l}X_1^{\prime} \\ Y_1^{\prime} \\ 1\end{array}\right)=K P_1^{\prime}$

其中K为相机的内参矩阵，在相机出厂的时候就已经确定的固定参数，有些相机说明上会有K矩阵的详细参数，而有些则需要自己计算，计算的方法就涉及到相机的标定。

相机成像中坐标变换——

坐标系旋转

二维

三维坐标投影至X-O-Y平面，Z轴朝外，X-O-Y坐标轴绕Z轴逆时针旋转$\theta$角度得到X'-O'-Y'坐标系，P(X, Y, Z)变为$P^{\prime}\left(X^{\prime}, Y^{\prime}, Z^{\prime}\right)$

由几何关系： $\left\{\begin{array}{l}X=X^{\prime} \cdot \cos \theta-Y^{\prime} \cdot \sin \theta \\ Y=X^{\prime} \cdot \cos \theta+Y^{\prime} \cos \theta\end{array}\right.$

转换为矩阵：

$\begin{aligned} & {\left[\begin{array}{l}X^{\prime} \\ Y^{\prime}\end{array}\right]=\left[\begin{array}{cc}\cos \theta & \sin \theta \\ -\sin \theta & \cos \theta\end{array}\right]\left[\begin{array}{l}X \\ Y\end{array}\right]=r\left[\begin{array}{l}X \\ Y\end{array}\right]} \\ & {\left[\begin{array}{l}X \\ Y\end{array}\right]=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta\end{array}\right]\left[\begin{array}{l}X^{\prime} \\ Y^{\prime}\end{array}\right]=r^{\prime}\left[\begin{array}{l}X^{\prime} \\ Y^{\prime}\end{array}\right]}\end{aligned}$

三维

同理，根据二维坐标的旋转可以推导出三维坐标系轴旋转的旋转矩阵表达形式。三维坐标系的轴旋转可以先将一个坐标轴固定从而转化为二维坐标系旋转，再分别对X、Y、 Z轴依次操作可得： $\begin{aligned} & {\left[\begin{array}{l}X^{\prime} \\ Y^{\prime} \\ Z^{\prime}\end{array}\right]=\left[\begin{array}{ccc}1 & 0 & 0 \\ 0 & \cos \theta & \sin \theta \\ 0 & -\sin \theta & \cos \theta\end{array}\right]\left[\begin{array}{l}X \\ Y \\ Z\end{array}\right]=R_X\left[\begin{array}{l}X \\ Y \\ Z\end{array}\right]} \\ & {\left[\begin{array}{l}X^{\prime} \\ Y^{\prime} \\ Z^{\prime}\end{array}\right]=\left[\begin{array}{ccc}\cos \theta & 0 & -\sin \theta \\ 0 & 1 & 0 \\ \sin \theta & 0 & \cos \theta\end{array}\right]\left[\begin{array}{l}X \\ Y \\ Z\end{array}\right]=R_Y\left[\begin{array}{l}X \\ Y \\ Z\end{array}\right]} \\ & {\left[\begin{array}{l}X^{\prime} \\ Y^{\prime} \\ Z^{\prime}\end{array}\right]=\left[\begin{array}{ccc}\cos \theta & \sin \theta & 0 \\ -\sin \theta & \cos \theta & 0 \\ 0 & 0 & 1\end{array}\right]\left[\begin{array}{l}X \\ Y \\ Z\end{array}\right]=R_Z\left[\begin{array}{l}X \\ Y \\ Z\end{array}\right]}\end{aligned}$

其中$R_X 、 R_Y 、 R_Z$分别表示绕X、Y、Z轴旋转的三维旋转矩阵，若按照Z→Y→Z的顺序旋转，得到整体旋转矩阵$R=R_X R_Y R_Z$

相机坐标系和世界坐标系可以通过旋转矩阵$R_{c n}$（把“相机坐标中的向量”表达到“世界坐标”中的旋转）联系起来：

• 世界←相机：$P_n=R_{c n} P_c+T$
• 世界→相机：$P_c=R_{n c}\left(P_n-T\right)$

$P_n=\left[x_n, y_n, z_n\right]^T$表示世界坐标系下的坐标，$P_c=\left[x_c, y_c, z_c\right]^T$表示相机坐标系下的坐标，T表示平移

投影：相机坐标 → 归一化成像平面

小孔成像模型，焦距为 f

$x_d=f \frac{x_c}{z_c}, \quad y_d=f \frac{y_c}{z_c}$

用齐次形式：

$\left[\begin{array}{l}x_d \\ y_d \\ 1\end{array}\right]=\frac{1}{z_c} \cdot\left[\begin{array}{cccc}f & 0 & 0 & 0 \\ 0 & f & 0 & 0 \\ 0 & 0 & 1 & 0\end{array}\right]\left[\begin{array}{l}x_c \\ y_c \\ z_c \\ 1\end{array}\right]$

相机坐标$P_c=\left[x_c, y_c, z_c\right]^T$→归一化的成像平面坐标$P_d=\left[x_d, y_d, 1\right]^T$

成像平面坐标 → 像素坐标

• 用主点坐标 (u0,v0) 把原点从主点移动到像素系原点
• $\frac{x_d}{d x}$、$\frac{y_d}{d y}$表示像素增量

dx, dy：单个像素的物理尺寸（米/像素），其倒数把米换成像素：例如，

• 传感器像素间距：dx, dy（例如 4 μm/像素）
• 成像平面坐标 $x_d$，$y_d$ 的单位是米/毫米
• 像素数 = 物理长度 ÷ 像素间距，所以 x 像素增量是 x_d/dx，y 像素增量是 y_d/dy。

取$\alpha=\frac{1}{d x}, \quad \beta=\frac{1}{d y}$，写成齐次坐标的形式可得：

$\left[\begin{array}{l}u \\ v \\ 1\end{array}\right]=\left[\begin{array}{lll}\alpha & 0 & u_0 \\ 0 & \beta & v_0 \\ 0 & 0 & 1\end{array}\right]\left[\begin{array}{l}x_d \\ y_d \\ 1\end{array}\right]$

归一化的成像平面坐标$P_d=\left[x_d, y_d, 1\right]^T$→像素平面坐标$(u, v)$

$x_d=f \frac{x_c}{z_c}, \quad y_d=f \frac{y_c}{z_c}$

$\left[\begin{array}{l}u \\ v \\ 1\end{array}\right]=\left[\begin{array}{lll}\alpha f& 0 & u_0 \\ 0 & \beta f & v_0 \\ 0 & 0 & 1\end{array}\right]\left[\begin{array}{l}x_d \\ y_d \\ 1\end{array}\right]$

$f_x=a f, f_y=\beta f$

相机坐标系下的坐标→像素平面坐标：

$\left[\begin{array}{c}u \\ v \\ 1\end{array}\right]=\underbrace{\left[\begin{array}{ccc}f_x & 0 & u_0 \\ 0 & f_y & v_0 \\ 0 & 0 & 1\end{array}\right]}_K\left[\begin{array}{c}x_c / z_c \\ y_c / z_c \\ 1\end{array}\right]$

$\begin{aligned} & {\left[\begin{array}{l}u \\ v \\ 1\end{array}\right]=\frac{1}{z_c} K\left[\begin{array}{l}x_c \\ y_c \\ z_c\end{array}\right]} \\ & K=\left[\begin{array}{ccc}f_x & 0 & u_0 \\ 0 & f_y & v_0 \\ 0 & 0 & 1\end{array}\right]\end{aligned}$

结合

以上步骤结合起来，即：

$Z_C\left[\begin{array}{c}u \\ v \\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{d x} & 0 & u_0 \\ 0 & \frac{1}{d y} & v_0 \\ 0 & 0 & 1\end{array}\right]\left[\begin{array}{cccc}f & 0 & 0 & 0 \\ 0 & f & 0 & 0 \\ 0 & 0 & 1 & 0\end{array}\right]\left[\begin{array}{cc}R & t \\ 0_{1 \times 3} & 1\end{array}\right]\left[\begin{array}{c}X_W \\ Y_W \\ Z_W \\ 1\end{array}\right]$

$\left[\begin{array}{c}X_W \\ Y_W \\ Z_W \\ 1\end{array}\right]$为世界坐标系的点，u、v为像素坐标系下坐标

论文《无人机自主巡检系统/风机叶片无人机自主巡检系统控制算法研究》第三章思路

如何“正对”