PNP问题学习

PNP（Perspective）问题：

求解3D到2D点对的运动问题。
求解方法：

1. P3P
2. EPnP(Efficient PnP)直接线性变换
3. UPnP
4. 光束平差法(Bundle Adjustment,BA)

直接线性变换

直接线性变换用于求解相机内参
齐次做标
归一化平面坐标

1. 直接线性变化
   （1）：假设空间上某个点的齐次做标为$P=(X,Y,Z,1{)}^T$,其投影在图像$I_1$上的点为：$x_1=(u_1,v_1,1{)}^T$
   （2）：相机的位姿时未知的，定义增广矩阵为$[R|t{]}_{3\times 4}$包含了旋转与平移信息。展开为：
   $$s\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=\left[\begin{array}{cccc}{t}_1& t_2& t_3& t_4\\ t_5& t_6& t_7& t_8\\ t_9& t_{10}& t_{11}& t_{12}\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]$$
   （3）:得到$u_1,v_1$为：
   $$u_1=\frac{t_{1}X+t_{2}Y+t_{3}Z+t_4}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}},v_1=\frac{t_{5}X+t_{6}Y+t_{7}Z+t_8}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}}$$
   （4）：令$T_1=(t_1,t_2,t_3,t_4{)}^T,T_2=(t_5,t_6,t_7,t_8{)}^T,T_3=(t_9,t_{10},t_{11},t_{12}{)}^T$ 得到：
   $$\begin{gathered} T_1^{T}P-T_3^{T}P{u}_1=0 \\ {T}_2^{T}P-T_3^{T}P{v}_1=0 \end{gathered}$$
   （5）：假设有N个特征点，则可以列出下列方程,求解即可：
   $P_n=(X_n,Y_n,Z_n,1{)}^T,n\in (1,2,..,n)$
   $$\left[\begin{array}{ccc}{P}_1^T& 0& -u_1{P}_1^T\\ 0& P_1^T& -v_1{P}_1^T\\ ...& ...& ...\\ P_N^T& 0& -u_N{P}_N^T\\ 0& P_N^T& -v_N{P}_N^T\end{array}\right]\left[\begin{array}{c}{T}_1\\ T_2\\ T_3\end{array}\right]=0$$

2. 直接线性变换用于求解相机内参
   （1）：像素坐标系与三维空间坐标具有如下关系
   $$\begin{gathered} f:焦距; \\ {u}_0,v_0:相机中心点的偏差; \\ 1/dx,1/dy:每个像素对应的实际距离 \end{gathered}$$
   $$Z_C\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{dx}& 0& u_0\\ 0& \frac{1}{dy}& 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^T& 1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]=\left[\begin{array}{cccc}{I}_1& I_2& I_3& I_4\\ I_5& I_6& I_7& I_8\\ I_9& I_{10}& I_{11}& I_{12}\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]$$
   （2）：使用DLT求解：
   $$\{\begin{array}{l}u=(I_{1}X+I_{2}Y+I_{3}Z+I_4)/Z_C\\ v=(I_{5}X+I_{6}Y+I_{7}Z+I_8)/Z_C\\ Z_C=I_{9}X+I_{10}Y+I_{11}Z+I_{12}\end{array}=\{\begin{array}{l}u=\frac{(I_{1}X+I_{2}Y+I_{3}Z+I_4)/Z_C}{I_{9}X+I_{10}Y+I_{11}Z+I_{12}}\\ v=\frac{(I_{5}X+I_{6}Y+I_{7}Z+I_8)}{I_{9}X+I_{10}Y+I_{11}Z+I_{12}}\end{array}$$
   （3）：得到：
   $$\{\begin{array}{l}{I}_1=\frac{f{r}_{11}}{dx}+u_0{r}_{31}\\ I_2=\frac{f{r}_{12}}{dx}+u_0{r}_{32}\\ I_3=\frac{f{r}_{13}}{dx}+u_0{r}_{33}\\ I_4=\frac{f{t}_1}{dx}+u_0{t}_3\\ I_5=\frac{f{r}_{21}}{dx}+v_0{r}_{31}\\ I_6=\frac{f{r}_{22}}{dx}+v_0{r}_{32}\\ I_7=\frac{f{r}_{23}}{dx}+v_0{r}_{33}\\ I_8=\frac{f{t}_2}{dx}+v_0{t}_3\\ I_9=r_{31}\\ I_{10}=r_{31}\\ I_{11}=r_{33}\\ I_{12}=t_3\end{array}$$
   （4）：进行变换的到下列方程主：
   $$\left[\begin{array}{ccccccccccc}X& Y& Z& 1& 0& 0& 0& 0& -Xu& -Yu& -Zu\\ 0& 0& 0& 0& X& Y& Z& 1& -X_v& -Yv& -Zv\\ ...& & & & & & & & & & \end{array}\right]\left[\begin{array}{c}{I}_1\\ I_2\\ I_3\\ I_4\\ I_5\\ I_6\\ I_7\\ I_8\\ I_9\\ I_{10}\\ I_{11}\\ I_{12}\end{array}\right]=\left[\begin{array}{c}u\\ v\end{array}\right]⟺BL=C$$
   矩阵解为:$L=(B^{T}B{)}^{-1}(B^{T}C)$
   （4）：内参数计算
   $$\{\begin{array}{l}{u}_0=I_1{I}_9+I_2{I}_{10}+I_3{I}_{11}\\ v_0=I_5{I}_9+I_6{I}_{10}+I_7{I}_{11}\\ f_u=\sqrt{I_2^2+I_2^2+I_3^2-u_0^2}\\ f_v=\sqrt{I_5^2+I_6^2+I_7^2-v_0^2}\end{array}$$

P3P

3个匹配点即可求解

1. 如上图式存在如下关系：
   $$\angle aOb=\angle AOB;\angle aOc=\angle AOC;\angle cOb=\angle COB$$

2. 利用余弦定理有：
   $$\begin{gathered} O{A}^2+O{B}^2-2OA\ast OB\ast cos<a,b>=A{B}^2 \\ O{B}^2+O{C}^2-2OB\ast OC\ast cos<b,c>=B{C}^2 \\ O{A}^2+O{C}^2-2OA\ast OC\ast cos<a,c>=A{C}^2 \end{gathered}$$

3. 对2中所有式子除以$O{C}^2$,并记$x=OA/OC,y=OB/OC$,得：
   $$\begin{gathered} x^2+y^2-2xycos<a,b>=A{B}^2/O{C}^2 \\ {y}^2+1-2ycos<b,c>=B{C}^2/O{C}^2 \\ {x}^2+1-2xcos<a,c>=A{C}^2/O{C}^2 \end{gathered}$$

4. 再令$v=A{B}^2/O{C}^2,uv=B{C}^2/O{C}^2,wv=A{C}^2/O{C}^2$
   $$\begin{gathered} x^2+y^2-2xycos<a,b>-v=0 \\ {y}^2+1-2ycos<b,c>-uv=0 \\ {x}^2+1-2xcos<a,c>-wv=0 \end{gathered}$$

5. 得到方程如下：
   $$\begin{gathered} (1-u)y^2-u{x}^2-cos<b,c>y+2uxycos<a,b>+1=0 \\ (1-w)x^2-w{y}^2-cos<a,c>x+2wxycos<a,b>+1=0 \end{gathered}$$
   由于2D点的图像位置已知，可以得到3个余弦值$cos<a,b>,cos<b,c>,cos<a,c>,$
   已知世界坐标系的3个坐标值可以得到$u=\frac{B{C}^2}{A{B}^2},w=\frac{A{C}^2}{A{B}^2}$
   可以得到一个2元2次方程，需要使用吴消元法。

6. 求解步骤：
   问题：输入的是上一帧中的3D点，和这些3D点在当前帧中的投影得到的2D点，求得的是当前帧相对于上一帧的位姿变换
   转载
   （1）：已知：像素坐标系下的参数如下$a(a_u,a_v),b(b_u,b_v),c(c_u,c_v)$,实际坐标$A(A_x,A_y,A_z),B(B_x,B_y,B_z),C(C_x,C_y,C_z)$
   （2）：计算3个余弦值$cos<a,b>,cos<b,c>,cos<a,c>,$:
   首先将像素坐标转换到相机坐标系即(以点a为例)：
   $$a_x=\frac{a_u-cx}{f_x},a_y=\frac{a_v-c_y}{f_y},a_z=1$$
   单位化：$N_a=\sqrt{a_x^2+a_y^2+a_z^2}$
   $$a_x^{\prime }=\frac{a_x}{N_a},a_y^{\prime }=\frac{a_y}{N_a},a_z^{\prime }=\frac{a_z}{N_a}$$
   得到余弦值：
   $$\begin{gathered} cos<a^{\prime },b^{\prime }>=a_x^{\prime }{b}_x^{\prime }+a_y^{\prime }{b}_y^{\prime }+c_x^{\prime }{c}_y^{\prime } \\ cos<a^{\prime },c^{\prime }>=a_x^{\prime }{c}_x^{\prime }+a_y^{\prime }{c}_y^{\prime }+b_x^{\prime }{b}_y^{\prime } \\ cos<b^{\prime },c^{\prime }>=b_x^{\prime }{c}_x^{\prime }+b_y^{\prime }{c}_y^{\prime }+a_x^{\prime }{a}_y^{\prime } \end{gathered}$$

   （3）：计算AB,BC,BC的值(AB为例)：
   $$AB=\sqrt{(A_x-B_x{)}^2+(A_y-B_y{)}^2+(A_z-B_z{)}^2}$$
   （4）：求解等效方程：
   $$\{\begin{array}{l}{a}_4{x}^4+a_3{x}^3+a_2{x}^2+a_{1}x+a_0=0\\ b_{1}y-b_0=0\end{array}$$
   $p=2cos<a,c>;q=2cos<b,c>;r=2cos<a,b>$(5):得到x,y，$|AB|$的值求解,得到$PC,PB,PA$
   $$x^2+y^2-2xycos<a,b>=\frac{A{B}^2}{P{C}^2},y=\frac{PB}{PC},x=\frac{PA}{PC}$$
   转化为向量:
   $a:单位向量;||PA||:模值;A:在相机坐标系下坐标$
   $$A=\overrightarrow{a}||PA||$$

（6）直接求变换前后的旋转矩阵
7. 缺点：
（1）：对3对点要求严格，如果有点受到噪声影响，或存在误匹配，算法失效
（2）：当对应点多余三个，则不能获得更多有效信息。

最小化重投影法求解PNP

把相机和三维点放在一起进行最小化（Bundle Adjustment）

1. 像素与空间位置点对应关系如下：
   $P:空间点$
   $u_i:像素坐标$
   $K:相机内参$
   $T:相机旋转平移的李群表示$
   $$s_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=KT\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\\ 1\end{array}\right]⟺s_i{u}_u=KT{P}_i$$

2. 误差项求和构建最小二乘问题
   $$T\ast =argmi{n}_T\frac{1}{2}\sum _{i=1}^n||u_i-\frac{1}{s_i}KT{P}_i|{|}^2（||重投影误差||）$$
   误差有2维，使用齐次做标时，误差为3维，最后一维一直为1，该维度误差一直为零，所以常使用非齐次做标。

3. 求解一阶导数
   使用高斯牛顿法或者列文伯格-马夸尔特方法需要用到一阶导数
   $$P^{\prime }=\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\end{array}\right]=T{P}_{[1:3]}$$

4. 得到：
   $$\begin{gathered} su=K{P}^{\prime }⟺\left[\begin{array}{c}su\\ sv\\ s\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}^{\prime }\\ Y^{\prime }\\ Z^{\prime }\end{array}\right] \\ u=f_x\frac{X^{\prime }}{Z^{\prime }}+c_x,v=f_y\frac{Y^{\prime }}{Z^{\prime }}+c_y \end{gathered}$$

5. 扰动求导（像素关于相机位姿的导数）：
   $\oplus :李代数上的左乘扰动$
   $$\frac{\partial e}{\partial \delta \xi }=li{m}_{\delta \xi ->0}\frac{e(\delta \xi \oplus \xi )-e(\xi )}{\delta \xi }=\frac{\partial e}{\partial P^{\prime }}\frac{\partial P^{\prime }}{\partial \delta \xi }(像素关于相机位姿的导数)$$
   (1):求解$\frac{\partial e}{\partial P^{\prime }}$:
   $$\frac{\partial e}{\partial P^{\prime }}=-\left[\begin{array}{ccc}\frac{\partial u}{\partial X^{\prime }}& \frac{\partial u}{\partial Y^{\prime }}& \frac{\partial u}{\partial Z^{\prime }}\\ \frac{\partial v}{\partial X^{\prime }}& \frac{\partial v}{\partial Y^{\prime }}& \frac{\partial v}{\partial Z^{\prime }}\end{array}\right]=-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]$$
   (2):求解$\frac{\partial P^{\prime }}{\partial \delta \xi }$:
   $$\begin{gathered} \frac{\partial P}{\partial \delta \xi }=\underset{\delta \xi ->0}{\lim }\frac{e^{\delta \xi \times }{e}^{\xi \times }p-e^{\xi \times }p}{\delta \xi } \\ =\underset{\delta \xi ->0}{\lim }\frac{(I+\delta \xi \times )e^{\xi \times }p-e^{\xi \times }p}{\delta \xi } \\ =\underset{\delta \xi ->0}{\lim }\frac{\delta \xi \times e^{\xi \times }p}{\delta \xi } \\ =\underset{\delta \xi ->0}{\lim }\frac{\left[\begin{array}{cc}\delta \varphi \times & \delta p\\ 0^T& 0\end{array}\right]\left[\begin{array}{c}Rp+t\\ 1\end{array}\right]}{\delta \xi } \\ =\underset{\delta \xi ->0}{\lim }\frac{\left[\begin{array}{c}\delta \varphi \times (Rp+t)+\delta p\\ 0^T\end{array}\right]}{{\left[\begin{array}{cc}\delta p& \delta \varphi \end{array}\right]}^T} \\ =\left[\begin{array}{cc}I& -(Rp+t)\times \\ 0^T& 0^T\end{array}\right] \\ =(TP{)}^{\odot } \end{gathered}$$
   $$\frac{\partial P}{\partial \delta \xi }=\left[\begin{array}{cc}I& -P^{\prime }\times \end{array}\right]$$
   (3):得到$\frac{\partial e}{\partial \delta \xi }$
   $$\frac{\partial e}{\partial \delta \xi }=-\left[\begin{array}{cccccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}& -\frac{f_x{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& f_x+\frac{f_x{X}^{\prime 2}}{Z^{\prime 2}}& -\frac{f_x{Y}^{\prime }}{Z^{\prime }}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}& -f_y-\frac{f_y{Y}^{\prime 2}}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }}{Z^{\prime }}\end{array}\right]$$

6. 像素关于相机位置的导数
   $$\begin{gathered} \frac{\partial e}{\partial P}=\frac{\partial e}{\partial P^{\prime }}\frac{\partial P^{\prime }}{\partial P} \\ =-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]R \end{gathered}$$

EPNP

参考1
参考2
质心坐标求解
已知：
（1）:n个世界坐标系中的3D参考点坐标
（2）:n个3D点对应的、投影在图像上的2D参考点。
（3）:摄像机内参K
问题：求解PNP。
解决：EPnP算法将世界坐标系中的3D坐标表示为一组虚拟的控制点的加权和，需要4个不共面的控制点，四个控制点在摄像头参考坐标系下的坐标是未知的，需要先计算4个控制点在相机坐标系下的坐标，再计算相机内参。

1. 已知：
   $右上角标：w:表示世界坐标系的坐标,c：相机坐标系下的坐标$
   $P_i^w,(i=1,2,...,n):3D参考点在世界坐标系中的非齐次坐标$
   $P_i^c,(i=1,2,...,n):摄像头参考坐标系中的非齐次坐标$
   $c_j^w,(j=1,2,3,4):4个控制点在世界坐标系中的非齐次坐标,4个点不共面$
   $c_j^c,(j=1,2,3,4):4个控制点在摄像头参考坐标系中的非齐次坐标$
   ${\alpha }_{ij}:齐次质心坐标,唯一确定$
   在世界坐标系下存在如下关系：
   $$P_i^w=\sum _{j=1}^4{\alpha }_{ij}{c}_j^w,(\sum _{j=1}^4{\alpha }_{ij}=1)$$
   在相机坐标系下存在如下关系：
   $$P_i^c=\sum _{j=1}^4{\alpha }_{ij}{c}_j^c$$
   $P_i^c={\sum }_{j=1}^4{\alpha }_{ij}{c}_j^c$证明：
   $\left[\begin{array}{cc}R& t\end{array}\right]:相机外参$
   $$\begin{gathered} c_j^c=\left[\begin{array}{cc}R& t\end{array}\right]\left[\begin{array}{c}{c}_j^c\\ 1\end{array}\right] \\ {P}_i^c=\left[\begin{array}{cc}R& t\end{array}\right]\left[\begin{array}{c}{P}_i^w\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\end{array}\right]\left[\begin{array}{c}{\sum }_{j=1}^4{\alpha }_{ij}{c}_j^w\\ 1\end{array}\right] \end{gathered}$$
   得到：
   $$P_i^c=\left[\begin{array}{cc}R& t\end{array}\right]\left[\begin{array}{c}{\sum }_{j=1}^4{\alpha }_{ij}{c}_j^w\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\end{array}\right]\left[\begin{array}{c}{\sum }_{j=1}^4{\alpha }_{ij}{c}_j^w\\ {\sum }_{j=1}^4{\alpha }_{ij}\end{array}\right]=\sum _{j=1}^4{\alpha }_{ij}\left[\begin{array}{cc}R& t\end{array}\right]\left[\begin{array}{c}{c}_j^w\\ 1\end{array}\right]=\sum _{j=1}^4{\alpha }_{ij}{c}_j^c$$

2. 如何选取控制点：
   （1）：第一个控制点：所有3D点的质心位置
   $$c_1^w=\frac{1}{n}\sum _{i=1}^n{P}_i^w$$
   （2）：其余控制点按如下方法计算矩阵：
   $$A=\left[\begin{array}{c}(p_1^w{)}^T-(c_1^w{)}^T\\ ...\\ (p_n^w{)}^T-(c_1^w{)}^T\end{array}\right]$$
   计算$A^{T}A的3个特征值：{\lambda }_1,{\lambda }_2,{\lambda }_3$,对应的特征向量为$v_1,v_2,v_3$那么剩余3个控制点为：$$\{\begin{array}{l}{c}_2^w=c_1^w+\sqrt{\frac{{\lambda }_1}{n}{v}_1}\\ c_3^w=c_1^w+\sqrt{\frac{{\lambda }_2}{n}{v}_2}\\ c_4^w=c_1^w+\sqrt{\frac{{\lambda }_3}{n}{v}_3}\end{array}$$

3. 求控制点在相机坐标系下的坐标
   (1) :已知相机投影模型为：
   $$\begin{gathered} w_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=K{P}_i^c=K\sum _{j=1}^4{\alpha }_{ij}{c}_j^c \\ =\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\sum _{j=1}^4{\alpha }_{ij}\left[\begin{array}{c}{x}_j^c\\ y_j^c\\ z_j^c\end{array}\right] \end{gathered}$$
   (2):化简得到：
   $f_x,f_y,c_x,c_y:相机内参已知$
   $u_i,v_i:2D点坐标已知$
   $$\{\begin{array}{l}{\sum }_{j=1}^4({\alpha }_{ij}{f}_x{x}_j^c+{\alpha }_{ij}(c_x-u_i)z_j^c)=0\\ {\sum }_{j=1}^4({\alpha }_{ij}{f}_y{y}_j^c+{\alpha }_{ij}(c_y-v_i)z_j^c)=0\end{array}$$
   4个控制变量下的坐标$x_j^c、y_j^c、z_i^c$未知，共12个位置参数
   （3）：4个控制变量下的坐标$x_j^c、y_j^c、z_i^c$未知，共12个位置参数，一个点确定2个方程，共n个点建一个线性方程如下：
   $M_{2n\times 12}$
   $x:12未知参数$
   $$Mx=0$$
   （4）：解得X
   $$x=\sum _{i=1}^N{\beta }_i{v}_i$$
   $v_i$为M的右奇异向量，即$M^{T}M的特征值为0对应的特征向量$
   （5）:已经解得了$v_i$，需要求解${\beta }_i$,${\beta }_i$与N(特征向量为0的解得个数)相关，N分别取1、2、3、4的情况下考虑。
   N=1:
   $x=\beta v$,此时控制点在世界坐标系下和相机坐标系下的距离是相等的：
   $$||c_i^c-c_j^c|{|}^2=||c_i^w-c_j^w|{|}^2$$
   引入一个约束：
   $v^{[i]}:$第i个控制点所占据的3个元素组成的向量。
   $$||\beta v^{[i]}-\beta v^{[j]}|{|}^2=||c_i^w-c_j^w|{|}^2$$
   得到$\beta$的一个封闭解：
   β = ∑ { i , j } ∈ [ 1 : 4 ] ∣ ∣ v [ i ] − v [ j ] ∣ ∣ ∗ ∣ ∣ c i w − v j w ∣ ∣ ∑ { i , j } ∈ [ 1 : 4 ] ∣ ∣ v [ i ] − v [ j ] ∣ ∣ 2 \beta=\frac{\sum_{\{i,j\}\in[1:4]}||v^{[i]}-v^{[j]}||*||c^w_i-v^w_j||}{\sum_{\{i,j\}\in[1:4]}||v^{[i]}-v^{[j]}||^2} β=∑{i,j}∈[1:4]​∣∣v[i]−v[j]∣∣2∑{i,j}∈[1:4]​∣∣v[i]−v[j]∣∣∗∣∣ciw​−vjw​∣∣​
   N=2:
   $x={\beta }_1{v}_1+{\beta }_2{v}_2$
   引入约束：
   $$\begin{gathered} ||({\beta }_1{v}_1^{[i]}+{\beta }_2{v}_2^{[i]})-({\beta }_1{v}_1^{[j]}+{\beta }_2{v}_2^{[j]})||=||c_i^w-c_j^w|{|}^2 \\ ⟺||{\beta }_1(v_1^{[i]}-v_1^{[j]})+{\beta }_2(v_2^{[i]}-v_2^{[j]})|{|}^2=||c_i^w-c_j^w|{|}^2 \end{gathered}$$
   令：$S_1=v_1^{[i]}-v_1^{[j]};S_2=v_2^{[i]}-v_2^{[j]};c=||c_i^w-c_j^w|{|}^2$
   得到：${\beta }_1^2{S}_1^T{S}_1+2{\beta }_1{\beta }_2{S}_1^T{S}_2+{\beta }_2^2{S}_2^T{S}_2=c$
   再令：$\beta ={\left[\begin{array}{ccc}{\beta }_{11}& {\beta }_{12}& {\beta }_{22}\end{array}\right]}^T={\left[\begin{array}{ccc}{\beta }_1^2& {\beta }_1{\beta }_2& {\beta }_2^2\end{array}\right]}^T$
   4个控制点可以构造出6个线性方程，组成:
   $p:由参数c构成$
   $$L_{6\times 3}\beta =p_{6\times 1}$$
   求解方程有两个解，再加上一个条件，控制点在摄像机的前端，即$c_j^c$的z分量要大于0，从而${\beta }_1,{\beta }_2$唯一确定。
   N=3:
   与N=2时的解法相同，这里的$\beta ={\left[\begin{array}{c}{\beta }_{11},{\beta }_{12},{\beta }_{13},{\beta }_{22},{\beta }_{23},{\beta }_{33}\end{array}\right]}^T$
   N=4:
   未知数的个数多于方程的个数了,
   $\beta ={\left[\begin{array}{c}{\beta }_{11},{\beta }_{12},{\beta }_{13},{\beta }_{14},{\beta }_{22},{\beta }_{23},{\beta }_{24},{\beta }_{33},{\beta }_{34},{\beta }_{44}\end{array}\right]}^T$
   加入新的优化，减少未知量：
   ${\beta }_{ab}={\beta }_a{\beta }_b,\beta bc=\frac{{\beta }_{ab}{\beta }_{ac}}{{\beta }_{aa}}$

4. 使用高斯牛顿法求解最优化问题。
   （1）：优化目标：
   $$Erro{r}_{i,j}(\beta )=\sum _{(i,j)s.t.i<j}||c_i^c-c_j^c|{|}^2-||c_i^w-c_j^w|{|}^2$$
   （2）：令$cc=||c_i^c-c_j^c|{|}^2,cw=||c_i^w-c_j^w|{|}^2$
   $$\begin{gathered} cc=||({\beta }_1{v}_1^{[i]}+{\beta }_2{v}_2^{[i]}+{\beta }_3{v}_3^{[i]}+{\beta }_4{v}_4^{[i]})-({\beta }_1{v}_1^{[j]}+{\beta }_2{v}_2^{[j]}+{\beta }_3{v}_3^{[j]}+{\beta }_4{v}_4^{[j]})|{|}^2 \\ =||{\beta }_1(v_1^{[i]}-v_1^{[j]})+{\beta }_2(v_2^{[i]}-v_2^{[j]})+{\beta }_3(v_3^{[i]}-v_3^{[j]})+{\beta }_4(v_4^{[i]}-v_4^{[j]})|| \end{gathered}$$
   （3）：再令$S_1=v_1^{[i]}-v_1^{[j]};S_2=v_2^{[i]}-v_2^{[j]};S_3=v_3^{[i]}-v_3^{[j]};S_4=v_4^{[i]}-v_1^{[j]};$:
   $$\begin{gathered} cc={\beta }_1^2{S}_1^T{S}_1+2{\beta }_1{\beta }_2{S}_1^T{S}_2+2{\beta }_1{\beta }_3{S}_1^T{S}_3+2{\beta }_1{\beta }_4{S}_1^T{S}_4 \\ +{\beta }_2^2{S}_2^T{S}_2+2{\beta }_2{\beta }_2{S}_2^T{S}_3+2{\beta }_2{\beta }_4{S}_2^T{S}_4 \\ +{\beta }_3^2{S}_3^T{S}_3+22{\beta }_3{\beta }_4{S}_3^T{S}_4 \\ +{\beta }_4^2{S}_4^T{S}_4 \end{gathered}$$
   （4）：求解$J_{ij}$
   $$\begin{gathered} J_{ij}=\frac{\partial [Erro{r}_{i,j}(\beta )]}{\partial \beta } \\ =\left[\begin{array}{c}2{\beta }_1{S}_1^T{S}_1+2{\beta }_2{S}_1^T{S}_2+2{\beta }_3{S}_1^T{S}_3+2{\beta }_1{S}_1^T{S}_4\\ 2{\beta }_2{S}_2^T{S}_1+2{\beta }_2{S}_2^T{S}_2+2{\beta }_3{S}_2^T{S}_3+2{\beta }_1{S}_2^T{S}_4\\ 2{\beta }_3{S}_3^T{S}_1+2{\beta }_2{S}_3^T{S}_2+2{\beta }_3{S}_3^T{S}_3+2{\beta }_1{S}_3^T{S}_4\\ 2{\beta }_4{S}_4^T{S}_1+2{\beta }_2{S}_4^T{S}_2+2{\beta }_3{S}_4^T{S}_3+2{\beta }_1{S}_4^T{S}_4\end{array}\right] \end{gathered}$$
   （5）得到雅可比矩阵：
   $$J=\left[\begin{array}{c}{J}_{1,2}\\ J_{1,3}\\ J_{1,4}\\ J_{2,3}\\ J_{2,4}\\ J_{3,4}\end{array}\right]$$
   （6）残差$r$为:
   $$r=\left[\begin{array}{c}Erro{r}_{1,2}(\beta )\\ Erro{r}_{1,3}(\beta )\\ Erro{r}_{1,4}(\beta )\\ Erro{r}_{2,3}(\beta )\\ Erro{r}_{2,4}(\beta )\\ Erro{r}_{3,4}(\beta )\end{array}\right]$$
   （7）增量方程为：
   $$J^{T}J\delta \beta =-J^{T}r$$
   （8）更新方程为：
   $$\beta =\beta +\delta \beta$$

5. 求解·若[R|t]

• ICP后边文章