【PNP】slam中的pnp和目标跟踪中的pnp

1. 什么是pnp

即利用已知三维结构与图像的对应关系求解相机与参考坐标系的相对关系（相机的外参）

2. pnp的应用

2.1. 求解相机到物体的变换矩阵(目标追踪)

2.1.1. 问题背景

1. 已知物体的大小，即知道物体的世界坐标系的坐标
   $$P_w=\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\end{array}\right]\text{\hspace{1em}(1)}$$

2. 已知三维特征点在像素坐标系的投影
   $$p=\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]\text{\hspace{1em}(2)}$$

3. 相机的内参矩阵
   $$K=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(3)}$$

2.1.2. 求解

利用世界坐标系到像素坐标系的转化公式获取待追踪物体与相机之间的旋转与平移关系
$$p=\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\frac{1}{z_c}\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}_c\\ y_c\\ z_c\end{array}\right)=\frac{1}{z_c}K{T}_{cw}{P}_w\text{\hspace{1em}(4)}$$
为了求解$T_{cw}$，至少需要3组特征点。

2.1.3. 至少需要的特征点数

2.1.3.1. 代数法

将(12)式展开
$$\begin{gathered} p=\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\frac{1}{z_c}\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}_c\\ y_c\\ z_c\end{array}\right)= \\ \frac{1}{z_c}\left(\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right)\left(\begin{array}{cccc}{R}_{11}& R_{12}& R_{13}& T_1\\ R_{21}& R_{22}& R_{23}& T_2\\ R_{31}& R_{32}& R_{33}& T_3\end{array}\right)\left(\begin{array}{c}{X}_w\\ Y_w\\ Z_w\end{array}\right)\text{\hspace{1em}(5)} \end{gathered}$$

对于R和T展开并且对矩阵相乘展开我们得到：
$$\begin{gathered} X_w\ast (f_x\ast R_{11}+c_x\ast R_{31}-x\ast R_{31})+Y_w\ast (f_x\ast R_{12}+c_x\ast R_{32}-u\ast R_{32})+Z_w\ast (f_x\ast R_{13}+c_x\ast R_{33}-u\ast R_{33}) \\ =T_3\ast u-f_x\ast T_1-c_x\ast T_3\text{\hspace{1em}(6)} \end{gathered}$$
$$\begin{gathered} X_w\ast (f_y\ast R_{21}+c_y\ast R_{31}-v\ast R_{31})+Y_w\ast (f_y\ast R_{22}+c_y\ast R_{32}-v\ast R_{32})+Z_w\ast (f_y\ast R_{23}+c_y\ast R_{33}-v\ast R_{33}) \\ =T_3\ast v-f_y\ast T_2-c_y\ast T_3\text{\hspace{1em}(7)} \end{gathered}$$

也就是说一组2D/3D点可以得到两个方程组

$f_x$ $f_y$$u_0$ $v_0$是相机内参，已知，$X_w$ $Y_w$ $x$ $y$是一组3D/2D点的坐标，所以未知数有$R_{11}$ $R_{12}$ $R_{13}$$R_{21}$ $R_{22}$ $R_{23}$ $R_{31}$ $R_{32}$ $R_{33}$ $T_1$ $T_2$ $T_3$一共12个，由于旋转矩阵是正交矩阵，每行每列都是单位向量且两两正交，所以$R$的自由度为3，秩也是3，比如知道$R_{11}$ $R_{12}$ $R_{13}$就能求出剩下的$R_{xx}$。加上平移向量的3个未知数，一共6个未知数，而每一组2D/3D点提供的$x$ $y$ $X_w$ $Y_w$ $Z_w$可以确立两个方程，所以3组2D/3D点的坐标能确立6个方程从而解出6个未知数。

$z_c$哪去了，$z_c$的意义是在相机坐标系下，特征点的Z轴坐标，所以这个$z_c$等于$t_3$

故PnP需要知道至少3组2D/3D点。

2.1.3.2. 几何法

• 当只有一个特征点时，相机在一个球面上任意一个位置观测特征点上的结果都是一致的，所以此时相机位姿的解有无数个(球面)
• 当只有两个特征点时，相机在两个球面的相交面上任意一个位置观测特征点上的结果都是一致的，所以此时相机位姿的解有无数个(平面)
• 当有三个特征点时，相机在三个球面的相交面上任意一个位置观测特征点上的结果都是一致的，所以此时相机位姿的解是有限的(交点)

2.1.4. solvepnp的使用

2.2. 求解两帧相机之间的变换矩阵矩阵

2.2.1. 问题背景

1. 已知第$k$帧的特征点的像素坐标以及特征点的深度信息$z_{c1}$
   $$p_1=\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)\text{\hspace{1em}(8)}$$
2. 已知第$k+1$帧的特征点的像素坐标
   $$p_2=\left(\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right)\text{\hspace{1em}(9)}$$
3. 相机的内参矩阵
   $$K=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(10)}$$

2.2.2. 求解

先将第$k$帧像素坐标系的特征点转成相机坐标系下$P_{c1}$

$$p_1=\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)=\frac{1}{z_{c1}}\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}_{c1}\\ y_{c1}\\ z_{c1}\end{array}\right)\text{\hspace{1em}(11)}$$

现在已知第$k$帧相机坐标系下特征点的三维坐标，现在我们当它是一个全新的世界坐标系下的坐标，用第$k+1$帧的像素坐标和这个全新的世界坐标计算出两帧相机之间的变换矩阵
$$\left(\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right)=\frac{1}{z_{c2}}\left(\begin{array}{cc}R& t\\ 0& 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}_{c1}\\ y_{c1}\\ z_{c1}\\ 1\end{array}\right)\text{\hspace{1em}(12)}$$
其中需要特征点的个数和上一种应用的推导一致。

3. 两种应用之间的区别

前者需要的是相邻帧间相机位姿的的变换矩阵,但是后者需要的是被跟踪物体和相机之间的变换矩阵,两者间有着共通之处,就是他们都是使用一样的坐标变换,只不过视觉里程计把前一帧看作世界坐标系罢了