【三维重建】ePnP

PnP问题应用与一下场景：
已知三维点和对应二维点以及相机相机内参数，可以获取相机外参。
我们介绍其中的一种算法：ePnP

算法流程

1、ePnP算法首先在世界坐标系内寻找4个控制点，记作$C_1^w,C_2^w,C_3^w,C_4^w$，使得：
对于世界坐标系内任意一点$P_1^w,$存在对应的${\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$$
世界坐标系上的 $P^w$ 经过 $R,t$ 变换可以得到相机坐标系下的点 $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是世界坐标系下的控制点，C_j^c是世界坐标系下的控制点)$:
$$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)$$

通过内参矩阵，建立相机坐标系下的点 $P^c$ 到像素坐标系下的点 $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}$$
对于上述公式 只有相机坐标系下的四个控制点 $C_j^c$​ 未知，每个控制点有三个参数，因此一共有12个未知数。每对点能建立两个方程组，所以，至少要6对点才能进行求解。
$$\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}$$
找到相机坐标系下的控制点后，就能够求得相机外参:
$$\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}$$

控制点选取

原则上，就是只要选择3个线性无关的点，就可以表示任意一个三维点，但由于方程组是4，如何是3个控制点只能求得最小二乘解。论文中给出了具体的选择方法。 3D参考点集为 { P i w , i = 1 , ⋯ , n } \left\{P^w_i,i=1,\cdots,n \right \} {Piw​,i=1,⋯,n}, 选择3D点的中心为第一个控制点:
$$C_1^w=\frac{1}{n}\sum _{i=1}^n{P}_i^w$$
进而得到矩阵:
$$A=\left[\begin{array}{c}{(P_1^w)}^T-{(C_1^w)}^T\\ \cdots \\ {(P_n^w)}^T-{(C_n^w)}^T\end{array}\right]$$
记$A^{T}A$的特征值为 ${\lambda }_i$, 特征向量为 $V_i$ ,那么剩下的三个点为：
$$C^w=C_1^w+{\lambda }_i^{\frac{1}{2}}{V}_i,i=1,2,3$$

求解$\alpha$

联立方程组:
$$\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}$$
每个点可以得到4个方程组:
$$\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个控制点**///求取重心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("");//构建矩阵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);
}