当前位置：首页 > news >正文

【三维重建】ePnP

news 2026/2/10 1:57:12

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, {\kern 20pt} with\sum_{j=1}^4\alpha_{ij}=1$
世界坐标系上的 $P^w$ 经过 $R, t$ 变换可以得到相机坐标系下的点 $P^c$ :
$P_i^c = RP_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}(RC_j^w + t)= \sum_{j-1}^4\alpha_{ij}C_j^c,{\color{red} \left(C_j^c = RC_j^w+t \right)}$

通过内参矩阵，建立相机坐标系下的点 $P^c$ 到像素坐标系下的点 $p$ 的映射：
$p_i = s_i\begin{bmatrix}u_i \\ v_i \\ 1 \end{bmatrix} = KP^c_i=\begin{bmatrix}f_u &0 & u_c \\0& f_v & v_c \\ 0&0&1 \end{bmatrix}\sum_{j=1}^4\alpha_{ij}\begin{bmatrix}C_{xj}^c \\C_{yj}^c \\ C_{zj}^c \end{bmatrix}$
将公式展开，可得：
$\sum_{j=1}^4\alpha_{ij}f_uC^c_{xj} + \alpha_{ij}(u_c-u_i)C_{zj}^c = 0 \\ \sum_{j=1}^4\alpha_{ij}f_vC^c_{yj} + \alpha_{ij}(v_c-v_i)C_{zj}^c = 0$
对于上述公式只有相机坐标系下的四个控制点 $C_j^c$ 未知，每个控制点有三个参数，因此一共有12个未知数。每对点能建立两个方程组，所以，至少要6对点才能进行求解。
$f_u \begin{bmatrix} \alpha_{i1} & \alpha_{i2} & \alpha_{i3} & \alpha_{i4} \end{bmatrix}\begin{bmatrix} C_{x1}^c \\ C_{x2}^c \\ C_{x3}^c \\ C_{x4}^c\end{bmatrix} + (u_c-u_i) \begin{bmatrix} \alpha_{i1} & \alpha_{i2} & \alpha_{i3} & \alpha_{i4} \end{bmatrix} \begin{bmatrix} C_{z1}^c \\ C_{z2}^c \\ C_{z3}^c \\ C_{z4}^c\end{bmatrix} =0 \\ f_v \begin{bmatrix} \alpha_{i1} & \alpha_{i2} & \alpha_{i3} & \alpha_{i4} \end{bmatrix}\begin{bmatrix} C_{y1}^c \\ C_{y2}^c \\ C_{y3}^c \\ C_{y4}^c\end{bmatrix} + (v_c-v_i) \begin{bmatrix} \alpha_{i1} & \alpha_{i2} & \alpha_{i3} & \alpha_{i4} \end{bmatrix} \begin{bmatrix} C_{z1}^c \\ C_{z2}^c \\ C_{z3}^c \\ C_{z4}^c\end{bmatrix} =0 \\\\\\ \begin{bmatrix} 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{bmatrix}\begin{bmatrix}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{bmatrix} =0$
找到相机坐标系下的控制点后，就能够求得相机外参:
$\begin{aligned} &C_j^c = RC_j^w+t\\\\ &\begin{bmatrix}C_{xj}^c \\ C_{yj}^c \\ C_{zj}^c \end{bmatrix} = \begin{bmatrix} r_1 & r_2 & r_3 \\ r_4 & r_5 & r_6 \\r_7 & r_8 & r_9 \end{bmatrix}\begin{bmatrix}C_{xj}^w \\ C_{yj}^w \\ C_{zj}^w \end{bmatrix} + \begin{bmatrix} t_1 \\ t_2 \\ t_3\end{bmatrix} \\\\ &C_{xj}^c = r_1C_{xj}^w + r_2C_{yj}^w + r_3C_{zj}^w + t_1\\ &C_{yj}^c = r_4C_{xj}^w + r_5C_{yj}^w + r_6C_{zj}^w + t_2\\ &C_{zj}^c = r_7C_{xj}^w + r_8C_{yj}^w + r_9C_{zj}^w + t_3\\ \\\\ &\begin{bmatrix}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{bmatrix} = \begin{bmatrix} 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{bmatrix} \begin{bmatrix}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{bmatrix} \end{aligned}$

控制点选取

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

求解 $\alpha$

联立方程组:
$\begin{aligned} &P_i^w = \alpha_{i1}C_1^w+\alpha_{i2}C_2^w+\alpha_{i3}C_3^w+\alpha_{i4}C_4^w \\\\ &\begin{bmatrix} x_i \\y_i\\z_i\end{bmatrix} = \alpha_{i1}\begin{bmatrix} C_{x1}^w \\C_{y1}^w\\C_{z1}^w\end{bmatrix}+\alpha_{i2}\begin{bmatrix} C_{x2}^w \\C_{y2}^w\\C_{z2}^w\end{bmatrix}+\alpha_{i3}\begin{bmatrix} C_{x3}^w \\C_{y3}^w\\C_{z3}^w\end{bmatrix}+\alpha_{i4}\begin{bmatrix} C_{x4}^w \\C_{y4}^w\\C_{z4}^w\end{bmatrix} \end{aligned}$
每个点可以得到4个方程组:
$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}$
用矩阵的方式可表示为：
$\begin{bmatrix} x_i \\ y_i \\ z_i \\ 1 \end{bmatrix} = \begin{bmatrix} 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{bmatrix} \begin{bmatrix} \alpha_{i1} \\ \alpha_{i2} \\ \alpha_{i3} \\ \alpha_{i4}\end{bmatrix} \longrightarrow \begin{bmatrix} 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{bmatrix} ^{-1}\begin{bmatrix} x_i \\ y_i \\ z_i \\ 1 \end{bmatrix} = \begin{bmatrix} \alpha_{i1} \\ \alpha_{i2} \\ \alpha_{i3} \\ \alpha_{i4}\end{bmatrix}$

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);
}

【三维重建】ePnP

算法流程

控制点选取

求解 $\alpha$

相关文章：