相机姿态估计（Pose Estimation）

（未完待续.....）

根据针孔相机模型，相机成像平面一点的像素坐标p和该点在世界坐标系下的3D坐标P有$p=KP$的关系，如果用齐次坐标表示则有：

$$dp=KP$$

其中d是空间点深度（为了将p的齐次项变为1），K是相机内参数矩阵，p和P都是齐次坐标。

于是如果以第一个相机的坐标系为参照，对于两个相机则有：$$d_0p_0=KP，d_1p_1=K(RP+t)$$

其中R为旋转矩阵（Rotation），t为平移向量（Translation）。令$x = K^{-1}p$，去掉内参K归一化成：

$$d_0x_0=P， d_1x_1=RP+t$$

由这两式得：$$d_1x_1 = R(d_0x_0)+t$$