世界坐标系到相机坐标系 python实现 世界坐标和相机坐标

坐标系变换

1. 几个坐标系：

• 世界坐标系(world coordinate system)：
  用户定义的三维世界的坐标系，为了描述目标物在真实世界里的位置而被引入。单位为m。
• 相机坐标系(camera coordinate system)：在相机上建立的坐标系，为了从相机的角度描述物体位置而定义，作为沟通世界坐标系和图像/像素坐标系的中间一环。单位为m。
• 图像坐标系(image coordinate system)：为了描述成像过程中物体从相机坐标系到图像坐标系的投影透射关系而引入，方便进一步得到像素坐标系下的坐标。 单位为m。
• 像素坐标系(pixel coordinate system)：为了描述物体成像后的像点在数字图像上（相片）的坐标而引入，是我们真正从相机内读取到的信息所在的坐标系。单位为个（像素数目）。

2.相机参数

16个单目相机的参数：

• 4个内部参数（只与相机有关）：
  $fx,fy,u0,v0,(1/dx、1/dy、\gamma 、u0、v0、f )$
  实际其$fx = F*S_x$，其中的$F$就是焦距(上面的$f$).Sx是像素/每毫米(即上面的$1/dx$)，其是最后面图里的后两个矩阵进行先相乘，得出的，则把它看成整体，就相当于4个内参。把 $\gamma$
• 5个畸变参数D：
  $k1,k2,k3$ 径向畸变系数;
  $p1,p2$ 是切向畸变系数;
  径向畸变：产生原因是光线在远离透镜中心的地方比靠近中心的地方更加弯曲,径向畸变主要包含桶形畸变和枕形畸变两种。
  切向畸变：产生的原因透镜不完全平行于图像平面，这种现象发生于成像仪被粘贴在摄像机的时候。
• 6个外部参数(取决于相机在世界中的位置):
  3个旋转参数$R$;
  3个平移参数$T$

3. 坐标系之间的转换

3.1 整体公式

$Z_c\left[\begin{array}{c}{u } \\ {v} \\ {1}\end{array}\right] =\left[\begin{array}{ccc}{\frac{1}{dx}} & {\gamma} & {c_x} \\ {0} & {\frac{1}{dy}} & {c_y} \\ {0} & {0} & {1}\end{array}\right] \left[\begin{array}{ccc}{f} & {0} & {0} \\ {0} & {f} & {0} \\ {0} & {0} & {1}\end{array}\right] \left[\begin{array}{cc}{\boldsymbol{R}} & {\boldsymbol{t}} \\ {\boldsymbol{0}^{T}} & {1}\end{array}\right] \left[\begin{array}{c}{X_w } \\ {Y_w} \\ {Z_w} \\{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}{cc}{\boldsymbol{R}} & {\boldsymbol{t}} \\ {\boldsymbol{0}^{T}} & {1}\end{array}\right] \left[\begin{array}{c}{X_w } \\ {Y_w} \\ {Z_w} \\{1}\end{array}\right]$

3.2 世界坐标系->相机坐标系：

刚体变换(regidbody motion):
三维空间中，当物体不发生形变时，对一个几何物体作旋转 $R$， 平移 $T$ 的运动，称之为刚体变换。世界坐标系到相机坐标系的变化就是刚体变换，又因为$R$和$T$与相机无关，所有又称其为相机外参。
首先考虑旋转。我们设某个单位正交基 $(e_1; e_2; e_3)$经过一次旋转，变成了 $(e_1′ ; e_2′ ; e_3′ )$。那么，对于同一个向量 a（注意该向量并没有随着坐标系的旋转而发生运动），它在两个坐标系下的坐标为 $[a_1; a_2; a_3]^T$ 和 $[a_1′ ; a_2′ ; a_3′ ]^T$。

根据坐标的定义，有

$\left[e_{1}, e_{2}, e_{3}\right]\left[\begin{array}{l}{a_{1}} \\ {a_{2}} \\ {a_{3}}\end{array}\right]=\left[e_{1}^{\prime}, e_{2}^{\prime}, e_{3}^{\prime}\right]\left[\begin{array}{c}{a_{1}^{\prime}} \\ {a_{2}^{\prime}} \\ {a_{3}^{\prime}}\end{array}\right]$
左右同事乘

$\left[\begin{array}{c}{e_{1}^{T}} \\ {e_{2}^{T}} \\ {e_{3}^{T}}\end{array}\right]$
，得到

$\left[\begin{array}{l}{a_{1}} \\ {a_{2}} \\ {a_{3}}\end{array}\right]=\left[\begin{array}{lll}{e_{1}^{T} e_{1}^{\prime}} & {e_{1}^{T} e_{2}^{\prime}} & {e_{1}^{T} e_{3}^{\prime}} \\ {e_{2}^{T} e_{1}^{\prime}} & {e_{2}^{T} e_{2}^{\prime}} & {e_{2}^{T} e_{3}^{\prime}} \\ {e_{3}^{T} e_{1}^{\prime}} & {e_{3}^{T} e_{2}^{\prime}} & {e_{3}^{T} e_{3}^{\prime}}\end{array}\right]\left[\begin{array}{c}{a_{1}^{\prime}} \\ {a_{2}^{\prime}} \\ {a_{3}^{\prime}}\end{array}\right] \triangleq R a^{\prime}$
中间的矩阵 $R$ 由两组基之间的内积组成，刻画了旋转前后同一个向量的坐标变换关系。只要旋转是一样的，那么这个矩阵也是一样的。可以说，矩阵 $R$ 描述了旋转本身。因此它又称为旋转矩阵。
旋转矩阵有一些特别的性质：是一个行列式为 1 的正交矩阵。
可以把旋转矩阵的集合定义如下：

$SO(n)=\left\{\boldsymbol{R} \in \mathbb{R}^{n \times n} | \boldsymbol{R} \boldsymbol{R}^{T}=\boldsymbol{I}, \operatorname{det}(\boldsymbol{R})=1\right\}$
$n$ 代表坐标轴个数，比如 $SO(3)$ 就代表包含 $x，y，z$ 三个坐标轴的坐标系。
在欧氏变换中，除了旋转之外还有一个平移。考虑世界坐标系中的向量 $ a $，经过一次
旋转 $R$和一次平移 $t$ 后，得到了$a′$，那么把旋转和平移合到一起，有：

$\boldsymbol{a}^{\prime}=\boldsymbol{R} \boldsymbol{a}+\boldsymbol{t}$
其实表示为坐标就是

$\left[\begin{array}{c}{X_{\mathrm{c}}} \\ {Y_{\mathrm{c}}} \\ {Z_{\mathrm{c}}}\end{array}\right]=\left[\begin{array}{ccc}{r_{00}} & {r_{01}} & {r_{02}} \\ {r_{10}} & {r_{11}} & {r_{12}} \\ {r_{20}} & {r_{21}} & {r_{22}}\end{array}\right]\left[\begin{array}{c}{X} \\ {Y} \\ {Z}\end{array}\right]+\left[\begin{array}{c}{T_{x}} \\ {T_{y}} \\ {T_{z}}\end{array}\right]$
上面变换关系不是一个线性关系。假设我们进行了两次变换：$R1; t1$ 和 $R2; t2$，满足：
$\boldsymbol{b}=\boldsymbol{R}_{1} \boldsymbol{a}+\boldsymbol{t}_{1}, \quad \boldsymbol{c}=\boldsymbol{R}_{2} \boldsymbol{b}+\boldsymbol{t}_{2}$ 合并为 $c=R_{2}\left(R_{1} a+t_{1}\right)+t_{2}$，这样当变化次数特别多的时候很复杂。
所以一般在三维向量的末尾添加一个1，表示为齐次坐标的形式

$\left[\begin{array}{c}{a^{\prime}} \\ {1}\end{array}\right]=\left[\begin{array}{cc}{\boldsymbol{R}} & {\boldsymbol{t}} \\ {\boldsymbol{0}^{T}} & {1}\end{array}\right]\left[\begin{array}{l}{\boldsymbol{a}} \\ {1}\end{array}\right] \triangleq \boldsymbol{T}\left[\begin{array}{l}{\boldsymbol{a}} \\ {1}\end{array}\right]$
对于这个四维向量，我们可以把旋转和平移写在一个矩阵里面，使得整个关系变成了线性关系。该式中，矩阵 $T$ 称为变换矩阵（Transform Matrix） 。
相机坐标系和世界坐标系的坐标原点之间的距离，受 $x,y,z$ 三个方向上的分量共同控制，所以具有三个自由度。$R$ 其实是分别绕 $X, Y, Z$三轴旋转的效果之和.

绕z轴旋转如右图

$绕z旋转矩阵： \left[\begin{array}{l}{X_{c}} \\ {Y_{c}} \\ {Z_{c}}\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]$
$绕x旋转矩阵： \left[\begin{array}{c}{X_{c}} \\ {Y_{c}} \\ {Z_{c}}\end{array}\right]=\left[\begin{array}{ccc}{\cos \theta} & {0} & {-\sin \theta} \\ {0_{1}+1} & {1} & {0} \\ {\sin \theta} & {0} & {\cos \theta}\end{array}\right]\left[\begin{array}{c}{X} \\ {Y} \\ {Z}\end{array}\right]$
$绕y旋转矩阵： \left[\begin{array}{l}{X_{\mathrm{c}}} \\ {Y_{\mathrm{c}}} \\ {Z_{\mathrm{c}}}\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}{c}{X} \\ {Y} \\ {Z}\end{array}\right]$
以上分别为绕 $x, y, z$轴旋转的矩阵，所以旋转矩阵为 $R=R_x*R_y * R_z$

设 $O-x-y-z$为相机坐标系， $z$ 轴指向相机前方，$O$为摄像机的光心，也是针孔模型中的针孔。

3.3 相机坐标系->图像坐标系：

现实世界的空间点$P$，经过小孔 $O$ 投影之后，落在物理成像平面 $O′-x′-y′$ 上，成像点为 $P′$。设 $P$的坐标为 $[X; Y; Z]^T$， $P′$ 为 $[X′; Y ′; Z′]^T$，并且设物理成像平面到小孔的距离为 $f$（焦距）。

我们平时处理一般会用归一化成像平面，把成像位置转换到与它对称的红色位置，这样比较容易描述各个坐标系之间关系。

根据三角形相似关系，有 $\frac{Z}{f}=- \frac{X}{X$为了方便，一般去掉负号。
整理后：

$X′ = \frac{fX}{Z}$
$Y′ = \frac{fY}{Z}$

3.4 图像坐标系->像素坐标系：

因为相机最后得到的是像素，所以在成像平面上对像进行采样和量化，把图像坐标系的点转化到一个像素平面 $o-u-v$。像素坐标系与图像坐标系处于同一平面，像素坐标系原点为左上角，图像坐标系原点为中心。

像素坐标系‹通常的定义方式是：原点 $o′$ 位于图像的左上角， $u$轴向右与 $x$轴平行，$v$
轴向下与$y$轴平行。像素坐标系与成像平面之间，相差了一个缩放和一个原点的平移。
设像素坐标在$u$轴上缩放了$α$倍，在 v$上缩放了$β$ 倍。同时，原点平移了$[cx;cy]^T$。那么， $P′$的坐标与像素坐标$[u;v]^T$的关系为：
$\begin{cases} u = α X$

$\frac{1}{dx}$, $\frac{1}{dy}$分别为在$x$, $y$方向上每个像素的物理尺寸,把 $αf$ 合并为 $f_x$，$βf$ 合并为 $f_y$ 得到：
$\begin{cases} u = f_x\frac{X}{Z}+ c_x \\ v = f_y\frac{Y}{Z} + c_y \end{cases}$
$f $ 的单位为米， $α; β$ 的单位为像素每米，所以 $f_x; f_y$的单位为像 素。

把该式写成矩阵形式，会更加简洁，不过左侧需要用到齐次坐标, $\gamma$ 为$x, y$不垂直时的扭曲系数，一般为0：

$\left[\begin{array}{c}{u } \\ {v} \\ {1}\end{array}\right] = \frac{1}{Z} \left[\begin{array}{ccc}{f_x} & {\gamma} & {c_x} \\ {0} & {f_y} & {c_y} \\ {0} & {0} & {1}\end{array}\right] \left[\begin{array}{c}{X } \\ {Y} \\ {Z}\end{array}\right] = \frac{1}{Z} KP$
习惯把Z移动到左面：

$Z\left[\begin{array}{c}{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}{c}{X } \\ {Y} \\ {Z}\end{array}\right] =KP$

把中间的量组成的矩阵称为相机的内参数矩阵（Camera Intrinsics） $K$。

除了内参之外，自然还有相对的外参。考虑到上式中，我们使用的是 $P$在相机坐标系下的坐标。由于相机在运动，所以 $P $的相机坐标应该是它的世界坐标（记为 $P_w$），根据相机的当前位姿，变换到相机坐标系下的结果。相机的位姿由它的旋转矩阵 $R$ 和平移向量 $t$来描述。那么有：
$ZP_{uv} = Z \left[\begin{array}{c} u \\v \\1\end{array}\right] = K (RP_W +t) = KTP_w$
相机的位姿 $R; t$又称为相机的外参数（Camera Extrinsics） 。相比于不变的内参，外参会随着相机运动发生改变，同时也是 SLAM中待估计的目标，代表着机器人的轨迹.

3.5 畸变：

• 畸变形成的原因：
  为了获得好的成像效果，相机的前方一般都加了透镜。对成像时光线的传播会产生新的影响: 一是透镜自身的形状对光线传播的影响，二是在机械组装过程中，透镜和成像平面不可能完全平行，光线穿过透镜投影到成像面时的位置发生变化。

由透镜形状引起的畸变称之为径向畸变。在针孔模型中，一条直线投影到像素平面上还是一条直线。可是，在实际拍摄的照片中，摄像机的透镜往往使得真实环境中的一条直线在图片中变成了曲线‹。越靠近图像的边缘，这种现象越明显。由于实际加工制作的透镜往往是中心对称的，这使得不规则的畸变通常径向对称。它们主要分为两大类， 桶形畸变和枕形畸变。

桶形畸变是由于图像放大率随着离光轴的距离增加而减小，而枕形畸变却恰好相反。
在这两种畸变中，穿过图像中心和光轴有交点的直线还能保持形状不变。

除了透镜的形状会引入径向畸变外，在相机的组装过程中由于不能使得透镜和成像面严格平行也会引入切向畸变。

平面上的任意一点 $p$可以用笛卡尔坐标表示为 $[x, y]^{T}$ , 也可以把它写成极坐标的形式
$[r; θ]^T$，其中 $r$ 表示点 $p$ 离坐标系原点的距离， $θ$ 表示和水平轴的夹角。径向畸变可看成坐标点沿着长度方向发生了变化 $δr$, 也就是其距离原点的长度发生了变化。切向畸变可以
看成坐标点沿着切线方向发生了变化，也就是水平夹角发生了变化 $δθ$。

对于径向畸变，无论是桶形畸变还是枕形畸变，由于它们都是随着离中心的距离增加
而增加。我们可以用一个多项式函数来描述畸变前后的坐标变化：这类畸变可以用和距中
心距离有关的二次及高次多项式函数进行纠正：

$\begin{array}{l}{x_{\text {corrected}}=x\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right)} \\ {y_{\text {corrected}}=y\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right)}\end{array}$

其中 $[x, y]^{T}$ 是未纠正的点的坐标，$[xcorrected; ycorrected]^T$ 是纠正后的点的坐标，注意它们都是归一化平面上的点，而不是像素平面上的点。在上式中，对于畸变较小的图像中心区域，畸变纠正主要是 $k1$ 起作用。而对于畸变较大的边缘区域主要是 $k2$ 起作用。普通摄像头用这两个系数就能很好的纠正径向畸变。对畸变很大的摄像头，比如鱼眼镜头，可以加入 $k3$

另一方面，对于切向畸变，可以使用另外的两个参数 $p1, p2$来进行纠正:

$\begin{array}{l}{x_{\text {corrected}}=x+2 p_{1} x y+p_{2}\left(r^{2}+2 x^{2}\right)} \\ {y_{\text {corrected}}=y+p_{1}\left(r^{2}+2 y^{2}\right)+2 p_{2} x y}\end{array}$

对于相机坐标系中的一点 $P(X; Y; Z)$，我们能够通过五个畸变系数找到这个点在像素平面上的正确位置:

1. 将三维空间点投影到归一化图像平面。设它的归一化坐标为 [x; y]T。
2. 对归一化平面上的点进行径向畸变和切向畸变纠正。
3. 将纠正后的点通过内参数矩阵投影到像素平面，得到该点在图像上的正确位置。
1. 首先，世界坐标系下有一个固定的点 P_w；
2. 由于相机在运动，它的运动由 R; t 或变换矩阵描述。 P 的相机坐标为：
P_c= R * P_w + t。
3. 这时的 P_c 仍有 X; Y; Z 三个量，把它们投影到归一化平面 Z = 1 上，得到 P 的归
一化相机坐标： Pc = [X/Z; Y /Z; 1]T 。
4. 最后, P 的归一化坐标经过内参后，对应到它的像素坐标： P_uv = K * P_c。

双目坐标

1.理想情况的双目相机模型

$\frac{z-f}{z}=\frac{b-u_l+u_r}{b}$, 因为$d=u_l-u_r$,
所以最后$z=\frac{f*b}{d}$

2. 真实的双目相机模型

真实场景的双目摄像机其实不是严格平行向前。所以需要双目标定得到两个相机之间的平移和旋转矩阵。
参考：《视觉slam十四讲》