PnP問題應(yīng)用與一下場景：
已知三維點(diǎn)和對(duì)應(yīng)二維點(diǎn)以及相機(jī)相機(jī)內(nèi)參數(shù)，可以獲取相機(jī)外參。
我們介紹其中的一種算法：ePnP

算法流程

1、ePnP算法首先在世界坐標(biāo)系內(nèi)尋找4個(gè)控制點(diǎn)，記作$C_1^w,C_2^w,C_3^w,C_4^w$，使得：
對(duì)于世界坐標(biāo)系內(nèi)任意一點(diǎn)$P_1^w,$存在對(duì)應(yīng)的${\alpha }_i=[{\alpha }_{i1},{\alpha }_{i2},{\alpha }_{i3},{\alpha }_{i4}{]}^T$，滿足：
$$P_i^w=\sum _{j=1}^4{\alpha }_{ij}{C}_j^w,with\sum _{j=1}^4{\alpha }_{ij}=1$$
世界坐標(biāo)系上的 $P^w$ 經(jīng)過 $R,t$ 變換可以得到相機(jī)坐標(biāo)系下的點(diǎn) $P^c$ :
$$P_i^c=R{P}_i^w+t=R\left(\sum _{j=1}^4{\alpha }_{ij}{C}_j^w\right)+t$$
由于${\sum }_{j=1}^4{\alpha }_{ij}=1$,因此$t={\sum }_{j=1}^4{\alpha }_{ij}t$ 帶入上式得 $(C_j^w是世界坐標(biāo)系下的控制點(diǎn)，C_j^c是世界坐標(biāo)系下的控制點(diǎn))$:
$$P_i^c=\sum _{j?1}^4{\alpha }_{ij}(R{C}_j^w+t)=\sum _{j?1}^4{\alpha }_{ij}{C}_j^c,\left(C_j^c=R{C}_j^w+t\right)$$

通過內(nèi)參矩陣，建立相機(jī)坐標(biāo)系下的點(diǎn) $P^c$ 到像素坐標(biāo)系下的點(diǎn) $p$ 的映射：
$$p_i=s_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=K{P}_i^c=\left[\begin{array}{ccc}{f}_u& 0& u_c\\ 0& f_v& v_c\\ 0& 0& 1\end{array}\right]\sum _{j=1}^4{\alpha }_{ij}\left[\begin{array}{c}{C}_{xj}^c\\ C_{yj}^c\\ C_{zj}^c\end{array}\right]$$
將公式展開，可得：
$$\begin{gathered} \sum _{j=1}^4{\alpha }_{ij}{f}_u{C}_{xj}^c+{\alpha }_{ij}(u_c?u_i)C_{zj}^c=0 \\ \sum _{j=1}^4{\alpha }_{ij}{f}_v{C}_{yj}^c+{\alpha }_{ij}(v_c?v_i)C_{zj}^c=0 \end{gathered}$$
對(duì)于上述公式 只有相機(jī)坐標(biāo)系下的四個(gè)控制點(diǎn) $C_j^c$? 未知，每個(gè)控制點(diǎn)有三個(gè)參數(shù)，因此一共有12個(gè)未知數(shù)。每對(duì)點(diǎn)能建立兩個(gè)方程組，所以，至少要6對(duì)點(diǎn)才能進(jìn)行求解。
$$\begin{gathered} f_u\left[\begin{array}{cccc}{\alpha }_{i1}& {\alpha }_{i2}& {\alpha }_{i3}& {\alpha }_{i4}\end{array}\right]\left[\begin{array}{c}{C}_{x1}^c\\ C_{x2}^c\\ C_{x3}^c\\ C_{x4}^c\end{array}\right]+(u_c?u_i)\left[\begin{array}{cccc}{\alpha }_{i1}& {\alpha }_{i2}& {\alpha }_{i3}& {\alpha }_{i4}\end{array}\right]\left[\begin{array}{c}{C}_{z1}^c\\ C_{z2}^c\\ C_{z3}^c\\ C_{z4}^c\end{array}\right]=0 \\ {f}_v\left[\begin{array}{cccc}{\alpha }_{i1}& {\alpha }_{i2}& {\alpha }_{i3}& {\alpha }_{i4}\end{array}\right]\left[\begin{array}{c}{C}_{y1}^c\\ C_{y2}^c\\ C_{y3}^c\\ C_{y4}^c\end{array}\right]+(v_c?v_i)\left[\begin{array}{cccc}{\alpha }_{i1}& {\alpha }_{i2}& {\alpha }_{i3}& {\alpha }_{i4}\end{array}\right]\left[\begin{array}{c}{C}_{z1}^c\\ C_{z2}^c\\ C_{z3}^c\\ C_{z4}^c\end{array}\right]=0 \\ \left[\begin{array}{cccccccccccc}{f}_u{\alpha }_{i1}& f_u{\alpha }_{i2}& f_u{\alpha }_{i3}& f_u{\alpha }_{i4}& 0& 0& 0& 0& (u_c?u_i){\alpha }_{i1}& (u_c?u_i){\alpha }_{i2}& (u_c?u_i){\alpha }_{i3}& (u_c?u_i){\alpha }_{i4}\\ 0& 0& 0& 0& f_v{\alpha }_{i1}& f_v{\alpha }_{i2}& f_v{\alpha }_{i3}& f_v{\alpha }_{i4}& (v_c?v_i){\alpha }_{i1}& (v_c?v_i){\alpha }_{i2}& (v_c?v_i){\alpha }_{i3}& (v_c?v_i){\alpha }_{i4}\end{array}\right]\left[\begin{array}{c}{C}_{x1}^c\\ C_{x2}^c\\ C_{x3}^c\\ C_{x4}^c\\ C_{y1}^c\\ C_{y2}^c\\ C_{y3}^c\\ C_{y4}^c\\ C_{z1}^c\\ C_{z2}^c\\ C_{z3}^c\\ C_{z4}^c\end{array}\right]=0 \end{gathered}$$
找到相機(jī)坐標(biāo)系下的控制點(diǎn)后，就能夠求得相機(jī)外參:
$$\begin{array}{rl}& C_j^c=R{C}_j^w+t\\ & \\ & \left[\begin{array}{c}{C}_{xj}^c\\ C_{yj}^c\\ C_{zj}^c\end{array}\right]=\left[\begin{array}{ccc}{r}_1& r_2& r_3\\ r_4& r_5& r_6\\ r_7& r_8& r_9\end{array}\right]\left[\begin{array}{c}{C}_{xj}^w\\ C_{yj}^w\\ C_{zj}^w\end{array}\right]+\left[\begin{array}{c}{t}_1\\ t_2\\ t_3\end{array}\right]\\ & \\ & C_{xj}^c=r_1{C}_{xj}^w+r_2{C}_{yj}^w+r_3{C}_{zj}^w+t_1\\ & C_{yj}^c=r_4{C}_{xj}^w+r_5{C}_{yj}^w+r_6{C}_{zj}^w+t_2\\ & C_{zj}^c=r_7{C}_{xj}^w+r_8{C}_{yj}^w+r_9{C}_{zj}^w+t_3\\ & \\ & \\ & \left[\begin{array}{c}{C}_{x1}^c\\ C_{y1}^c\\ C_{z1}^c\\ C_{x2}^c\\ C_{y2}^c\\ C_{z2}^c\\ C_{x3}^c\\ C_{y3}^c\\ C_{z3}^c\\ C_{x4}^c\\ C_{y4}^c\\ C_{z4}^c\end{array}\right]=\left[\begin{array}{cccccccccccc}{C}_{x1}^w& C_{y1}^w& C_{z1}^w& 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& C_{x1}^w& C_{y1}^w& C_{z1}^w& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& C_{x1}^w& C_{y1}^w& C_{z1}^w& 0& 0& 1\\ C_{x2}^w& C_{y2}^w& C_{z2}^w& 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& C_{x2}^w& C_{y2}^w& C_{z2}^w& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& C_{x2}^w& C_{y2}^w& C_{z2}^w& 0& 0& 1\\ C_{x3}^w& C_{y3}^w& C_{z3}^w& 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& C_{x3}^w& C_{y3}^w& C_{z3}^w& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& C_{x3}^w& C_{y3}^w& C_{z3}^w& 0& 0& 1\\ C_{x4}^w& C_{y4}^w& C_{z4}^w& 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& C_{x4}^w& C_{y4}^w& C_{z4}^w& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& C_{x4}^w& C_{y4}^w& C_{z4}^w& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{r}_1\\ r_2\\ r_3\\ r_4\\ r_5\\ r_6\\ r_7\\ r_8\\ r_9\\ t_1\\ t_2\\ t_3\end{array}\right]\end{array}$$

控制點(diǎn)選取

原則上，就是只要選擇3個(gè)線性無關(guān)的點(diǎn)，就可以表示任意一個(gè)三維點(diǎn)，但由于方程組是4，如何是3個(gè)控制點(diǎn)只能求得最小二乘解。論文中給出了具體的選擇方法。 3D參考點(diǎn)集為 { P i w , i = 1 , ? , n } \left\{P^w_i,i=1,\cdots,n \right \} {Piw?,i=1,?,n}, 選擇3D點(diǎn)的中心為第一個(gè)控制點(diǎn):
$$C_1^w=\frac{1}{n}\sum _{i=1}^n{P}_i^w$$
進(jìn)而得到矩陣:
$$A=\left[\begin{array}{c}{(P_1^w)}^T?{(C_1^w)}^T\\ ?\\ {(P_n^w)}^T?{(C_n^w)}^T\end{array}\right]$$
記$A^{T}A$的特征值為 ${\lambda }_i$, 特征向量為 $V_i$ ,那么剩下的三個(gè)點(diǎn)為：
$$C^w=C_1^w+{\lambda }_i^{\frac{1}{2}}{V}_i,i=1,2,3$$

求解$\alpha$

聯(lián)立方程組:
$$\begin{array}{rl}& P_i^w={\alpha }_{i1}{C}_1^w+{\alpha }_{i2}{C}_2^w+{\alpha }_{i3}{C}_3^w+{\alpha }_{i4}{C}_4^w\\ & \\ & \left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]={\alpha }_{i1}\left[\begin{array}{c}{C}_{x1}^w\\ C_{y1}^w\\ C_{z1}^w\end{array}\right]+{\alpha }_{i2}\left[\begin{array}{c}{C}_{x2}^w\\ C_{y2}^w\\ C_{z2}^w\end{array}\right]+{\alpha }_{i3}\left[\begin{array}{c}{C}_{x3}^w\\ C_{y3}^w\\ C_{z3}^w\end{array}\right]+{\alpha }_{i4}\left[\begin{array}{c}{C}_{x4}^w\\ C_{y4}^w\\ C_{z4}^w\end{array}\right]\end{array}$$
每個(gè)點(diǎn)可以得到4個(gè)方程組:
$$\begin{gathered} x_i={\alpha }_{i1}{C}_{x1}^w+{\alpha }_{i2}{C}_{x2}^w+{\alpha }_{i3}{C}_{x3}^w+{\alpha }_{i4}{C}_{x4}^w \\ {y}_i={\alpha }_{i1}{C}_{y1}^w+{\alpha }_{i2}{C}_{y2}^w+{\alpha }_{i3}{C}_{y3}^w+{\alpha }_{i4}{C}_{y4}^w \\ {z}_i={\alpha }_{i1}{C}_{z1}^w+{\alpha }_{i2}{C}_{z2}^w+{\alpha }_{i3}{C}_{z3}^w+{\alpha }_{i4}{C}_{z4}^w \\ 1={\alpha }_{i1}+{\alpha }_{i2}+{\alpha }_{i3}+{\alpha }_{i4} \end{gathered}$$
用矩陣的方式可表示為：
$$\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\\ 1\end{array}\right]=\left[\begin{array}{cccc}{C}_{x1}^w& C_{x2}^w& C_{x3}^w& C_{x4}^w\\ C_{y1}^w& C_{y2}^w& C_{y3}^w& C_{y4}^w\\ C_{z1}^w& C_{z2}^w& C_{z3}^w& C_{z4}^w\\ 1& 1& 1& 1\end{array}\right]\left[\begin{array}{c}{\alpha }_{i1}\\ {\alpha }_{i2}\\ {\alpha }_{i3}\\ {\alpha }_{i4}\end{array}\right]?{\left[\begin{array}{cccc}{C}_{x1}^w& C_{x2}^w& C_{x3}^w& C_{x4}^w\\ C_{y1}^w& C_{y2}^w& C_{y3}^w& C_{y4}^w\\ C_{z1}^w& C_{z2}^w& C_{z3}^w& C_{z4}^w\\ 1& 1& 1& 1\end{array}\right]}^{?1}\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\\ 1\end{array}\right]=\left[\begin{array}{c}{\alpha }_{i1}\\ {\alpha }_{i2}\\ {\alpha }_{i3}\\ {\alpha }_{i4}\end{array}\right]$$

void Rebuild::ePnP(const std::vector<Eigen::Vector3d> &p3ds, const std::vector<Eigen::Vector2d> &p2ds, Camera &camera) {auto blog = _blog;blog->write("ePnP:");/**尋找4個(gè)控制點(diǎn)**///求取重心Eigen::Vector3d center = Eigen::Vector3d::Zero();std::vector<Eigen::Vector3d> control_points_w(4);for (int i = 0; i < p3ds.size(); i++) {center(0) += p3ds[i](0);center(1) += p3ds[i](1);center(2) += p3ds[i](2);}center(0) /= p3ds.size();center(1) /= p3ds.size();center(2) /= p3ds.size();control_points_w[0] = center;blog->write("center:");blog->write(center);blog->write("");//構(gòu)建矩陣Eigen::MatrixXd A = Eigen::MatrixXd::Zero(p3ds.size(), 3);for (int i = 0; i < p3ds.size(); i++) {A(i, 0) = p3ds[i](0) - center(0);A(i, 1) = p3ds[i](1) - center(1);A(i, 2) = p3ds[i](2) - center(2);}Eigen::MatrixXd M = A.transpose() * A;Eigen::EigenSolver<Eigen::MatrixXd> solver(M);Eigen::VectorXd eigenValues = solver.eigenvalues().real();Eigen::MatrixXd eigenVectors = solver.eigenvectors().real();blog->write("eigenValues:");blog->write(eigenValues);blog->write("");blog->write("eigenVectors:");blog->write(eigenVectors);blog->write("");for (int i = 1; i < 4; i++) {control_points_w[i] = control_points_w[0] + sqrt(eigenValues(i - 1)) * eigenVectors.col(i - 1);}blog->write("control_points_w:");for (int i = 0; i < 4; i++) {blog->write(control_points_w[i]);}blog->write("");/**求解alpha**/Eigen::MatrixXd C_w = Eigen::MatrixXd::Zero(4, 4);for (int i = 0; i < 4; i++) {double x = control_points_w[i](0);double y = control_points_w[i](1);double z = control_points_w[i](2);C_w(0, i) = x;C_w(1, i) = y;C_w(2, i) = z;C_w(3, i) = 1;}Eigen::MatrixXd C_w_inv = C_w.inverse();blog->write("C_w:");blog->write(C_w);blog->write("");blog->write("C_w_inv:");blog->write(C_w_inv);blog->write("");double fu = camera._K(0, 0);double fv = camera._K(1, 1);double uc = camera._K(0, 2);double vc = camera._K(1, 2);Eigen::MatrixXd D =Eigen::MatrixXd::Zero(int(2 * p3ds.size()), 12);for (int i = 0; i < p3ds.size(); i++) {Eigen::Vector3d p3d = p3ds[i];Eigen::Vector2d p2d = p2ds[i];Eigen::Vector4d b;b << p3d(0), p3d(1), p3d(2), 1;Eigen::Vector4d alpha = C_w_inv * b;D(i * 2, 0) = fu * alpha(0);D(i * 2, 1) = fu * alpha(1);D(i * 2, 2) = fu * alpha(2);D(i * 2, 3) = fu * alpha(3);D(i * 2, 8) = (uc - p2d(0)) * alpha(0);D(i * 2, 9) = (uc - p2d(0)) * alpha(1);D(i * 2, 10) = (uc - p2d(0)) * alpha(2);D(i * 2, 11) = (uc - p2d(0)) * alpha(3);D(i * 2 + 1, 4) = fv * alpha(0);D(i * 2 + 1, 5) = fv * alpha(1);D(i * 2 + 1, 6) = fv * alpha(2);D(i * 2 + 1, 7) = fv * alpha(3);D(i * 2 + 1, 8) = (vc - p2d(1)) * alpha(0);D(i * 2 + 1, 9) = (vc - p2d(1)) * alpha(1);D(i * 2 + 1, 10) = (vc - p2d(1)) * alpha(2);D(i * 2 + 1, 11) = (vc - p2d(1)) * alpha(3);if(i == 0){blog->write("alpha:");blog->write(alpha);blog->write("");}}blog->write("D:");blog->write(D);blog->write("");Eigen::JacobiSVD<Eigen::MatrixXd> svd(D, Eigen::ComputeFullV | Eigen::ComputeFullU);auto V = svd.matrixV();std::vector<Eigen::Vector3d> control_points_c(4);blog->write("control_points_c:");for (int i = 0; i < 4; i++) {double x = V(i);double y = V(i + 4);double z = V(i + 8);Eigen::Vector3d p3d(x, y, z);control_points_c[i] = p3d;blog->write(p3d);}blog->write("");Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(12, 12);Eigen::VectorXd C_c = Eigen::VectorXd::Zero(12);for (int i = 0; i < 4; i++) {C_c(i * 3) = control_points_c[i](0);C_c(i * 3 + 1) = control_points_c[i](1);C_c(i * 3 + 2) = control_points_c[i](2);Q(i * 3, 0) = control_points_w[i](0);Q(i * 3, 1) = control_points_w[i](1);Q(i * 3, 2) = control_points_w[i](2);Q(i * 3, 9) = 1;Q(i * 3 + 1, 3) = control_points_w[i](0);Q(i * 3 + 1, 4) = control_points_w[i](1);Q(i * 3 + 1, 5) = control_points_w[i](2);Q(i * 3 + 1, 10) = 1;Q(i * 3 + 2, 6) = control_points_w[i](0);Q(i * 3 + 2, 7) = control_points_w[i](1);Q(i * 3 + 2, 8) = control_points_w[i](2);Q(i * 3 + 2, 11) = 1;}blog->write("Q:");blog->write(Q);blog->write("");blog->write("C_c:");blog->write(C_c);blog->write("");Eigen::MatrixXd Q_inv = Q.inverse();blog->write("Q_inv:");blog->write(Q_inv);blog->write("");Eigen::VectorXd ans = Eigen::VectorXd::Zero(12);ans = Q_inv * C_c;blog->write("ans:");blog->write(ans);blog->write("");camera._R(0, 0) = ans(0);camera._R(0, 1) = ans(1);camera._R(0, 2) = ans(2);camera._R(1, 0) = ans(3);camera._R(1, 1) = ans(4);camera._R(1, 2) = ans(5);camera._R(2, 0) = ans(6);camera._R(2, 1) = ans(7);camera._R(2, 2) = ans(8);camera._t(0) = ans(9);camera._t(1) = ans(10);camera._t(2) = ans(11);
}