做网站什么是三网合一,wordpress主题验证码,scratch少儿编程网站,建设企业银行官方网站PnP问题应用与一下场景#xff1a; 已知三维点和对应二维点以及相机相机内参数#xff0c;可以获取相机外参。 我们介绍其中的一种算法#xff1a;ePnP
算法流程
1、ePnP算法首先在世界坐标系内寻找4个控制点#xff0c;记作 C 1 w , C 2 w , C 3 w , C 4 w C_1^w,C_2^w,…PnP问题应用与一下场景 已知三维点和对应二维点以及相机相机内参数可以获取相机外参。 我们介绍其中的一种算法ePnP
算法流程
1、ePnP算法首先在世界坐标系内寻找4个控制点记作 C 1 w , C 2 w , C 3 w , C 4 w C_1^w,C_2^w,C_3^w,C_4^w C1w,C2w,C3w,C4w使得 对于世界坐标系内任意一点 P 1 w , P_1^w, P1w,存在对应的 α i [ α i 1 , α i 2 , α i 3 , α i 4 ] T \alpha_i[\alpha_{i1},\alpha_{i2},\alpha_{i3},\alpha_{i4}]^T αi[αi1,αi2,αi3,αi4]T满足 P i w ∑ j 1 4 α i j C j w , w i t h ∑ j 1 4 α i j 1 P_i^w \sum_{j1}^4\alpha_{ij}C_j^w, {\kern 20pt} with\sum_{j1}^4\alpha_{ij}1 Piwj1∑4αijCjw,withj1∑4αij1 世界坐标系上的 P w P^w Pw 经过 R , t R,t R,t 变换可以得到相机坐标系下的点 P c P^c Pc : P i c R P i w t R ( ∑ j 1 4 α i j C j w ) t P_i^c RP_i^wtR\left(\sum_{j1}^4\alpha_{ij}C_j^w\right) t PicRPiwtR(j1∑4αijCjw)t 由于 ∑ j 1 4 α i j 1 \sum_{j1}^4\alpha_{ij}1 ∑j14αij1,因此 t ∑ j 1 4 α i j t t\sum_{j1}^4\alpha_{ij}t t∑j14αijt 带入上式得 ( C j w 是世界坐标系下的控制点 C j c 是世界坐标系下的控制点 ) (C_j^w是世界坐标系下的控制点C_j^c是世界坐标系下的控制点) (Cjw是世界坐标系下的控制点Cjc是世界坐标系下的控制点): P i c ∑ j − 1 4 α i j ( R C j w t ) ∑ j − 1 4 α i j C j c , ( C j c R C j w t ) 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^wt \right)} Picj−1∑4αij(RCjwt)j−1∑4αijCjc,(CjcRCjwt)
通过内参矩阵建立相机坐标系下的点 P c P^c Pc 到像素坐标系下的点 p p p 的映射 p i s i [ u i v i 1 ] K P i c [ f u 0 u c 0 f v v c 0 0 1 ] ∑ j 1 4 α i j [ C x j c C y j c C z j c ] 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 \\ 001 \end{bmatrix}\sum_{j1}^4\alpha_{ij}\begin{bmatrix}C_{xj}^c \\C_{yj}^c \\ C_{zj}^c \end{bmatrix} pisi uivi1 KPic fu000fv0ucvc1 j1∑4αij CxjcCyjcCzjc 将公式展开可得 ∑ j 1 4 α i j f u C x j c α i j ( u c − u i ) C z j c 0 ∑ j 1 4 α i j f v C y j c α i j ( v c − v i ) C z j c 0 \sum_{j1}^4\alpha_{ij}f_uC^c_{xj} \alpha_{ij}(u_c-u_i)C_{zj}^c 0 \\ \sum_{j1}^4\alpha_{ij}f_vC^c_{yj} \alpha_{ij}(v_c-v_i)C_{zj}^c 0 j1∑4αijfuCxjcαij(uc−ui)Czjc0j1∑4αijfvCyjcαij(vc−vi)Czjc0 对于上述公式 只有相机坐标系下的四个控制点 C j c C_j^c Cjc 未知每个控制点有三个参数因此一共有12个未知数。每对点能建立两个方程组所以至少要6对点才能进行求解。 f u [ α i 1 α i 2 α i 3 α i 4 ] [ C x 1 c C x 2 c C x 3 c C x 4 c ] ( u c − u i ) [ α i 1 α i 2 α i 3 α i 4 ] [ C z 1 c C z 2 c C z 3 c C z 4 c ] 0 f v [ α i 1 α i 2 α i 3 α i 4 ] [ C y 1 c C y 2 c C y 3 c C y 4 c ] ( v c − v i ) [ α i 1 α i 2 α i 3 α i 4 ] [ C z 1 c C z 2 c C z 3 c C z 4 c ] 0 [ f u α i 1 f u α i 2 f u α i 3 f u α i 4 0 0 0 0 ( u c − u i ) α i 1 ( u c − u i ) α i 2 ( u c − u i ) α i 3 ( u c − u i ) α i 4 0 0 0 0 f v α i 1 f v α i 2 f v α i 3 f v α i 4 ( v c − v i ) α i 1 ( v c − v i ) α i 2 ( v c − v i ) α i 3 ( v c − v i ) α i 4 ] [ C x 1 c C x 2 c C x 3 c C x 4 c C y 1 c C y 2 c C y 3 c C y 4 c C z 1 c C z 2 c C z 3 c C z 4 c ] 0 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} 0000(u_c-u_i) \alpha_{i1} (u_c-u_i) \alpha_{i2} (u_c-u_i) \alpha_{i3} (u_c-u_i) \alpha_{i4} \\0000f_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 fu[αi1αi2αi3αi4] Cx1cCx2cCx3cCx4c (uc−ui)[αi1αi2αi3αi4] Cz1cCz2cCz3cCz4c 0fv[αi1αi2αi3αi4] Cy1cCy2cCy3cCy4c (vc−vi)[αi1αi2αi3αi4] Cz1cCz2cCz3cCz4c 0[fuαi10fuαi20fuαi30fuαi400fvαi10fvαi20fvαi30fvαi4(uc−ui)αi1(vc−vi)αi1(uc−ui)αi2(vc−vi)αi2(uc−ui)αi3(vc−vi)αi3(uc−ui)αi4(vc−vi)αi4] Cx1cCx2cCx3cCx4cCy1cCy2cCy3cCy4cCz1cCz2cCz3cCz4c 0 找到相机坐标系下的控制点后就能够求得相机外参: C j c R C j w t [ C x j c C y j c C z j c ] [ r 1 r 2 r 3 r 4 r 5 r 6 r 7 r 8 r 9 ] [ C x j w C y j w C z j w ] [ t 1 t 2 t 3 ] C x j c r 1 C x j w r 2 C y j w r 3 C z j w t 1 C y j c r 4 C x j w r 5 C y j w r 6 C z j w t 2 C z j c r 7 C x j w r 8 C y j w r 9 C z j w t 3 [ C x 1 c C y 1 c C z 1 c C x 2 c C y 2 c C z 2 c C x 3 c C y 3 c C z 3 c C x 4 c C y 4 c C z 4 c ] [ C x 1 w C y 1 w C z 1 w 0 0 0 0 0 0 1 0 0 0 0 0 C x 1 w C y 1 w C z 1 w 0 0 0 0 1 0 0 0 0 0 0 0 C x 1 w C y 1 w C z 1 w 0 0 1 C x 2 w C y 2 w C z 2 w 0 0 0 0 0 0 1 0 0 0 0 0 C x 2 w C y 2 w C z 2 w 0 0 0 0 1 0 0 0 0 0 0 0 C x 2 w C y 2 w C z 2 w 0 0 1 C x 3 w C y 3 w C z 3 w 0 0 0 0 0 0 1 0 0 0 0 0 C x 3 w C y 3 w C z 3 w 0 0 0 0 1 0 0 0 0 0 0 0 C x 3 w C y 3 w C z 3 w 0 0 1 C x 4 w C y 4 w C z 4 w 0 0 0 0 0 0 1 0 0 0 0 0 C x 4 w C y 4 w C z 4 w 0 0 0 0 1 0 0 0 0 0 0 0 C x 4 w C y 4 w C z 4 w 0 0 1 ] [ r 1 r 2 r 3 r 4 r 5 r 6 r 7 r 8 r 9 t 1 t 2 t 3 ] \begin{aligned} C_j^c RC_j^wt\\\\ \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}^w000000100 \\ 000C_{x1}^w C_{y1}^w C_{z1}^w 000010\\ 000000 C_{x1}^w C_{y1}^w C_{z1}^w 001\\ C_{x2}^w C_{y2}^w C_{z2}^w000000100 \\ 000C_{x2}^w C_{y2}^w C_{z2}^w 000010\\ 000000 C_{x2}^w C_{y2}^w C_{z2}^w 001 \\ C_{x3}^w C_{y3}^w C_{z3}^w000000100 \\ 000C_{x3}^w C_{y3}^w C_{z3}^w 000010\\ 000000 C_{x3}^w C_{y3}^w C_{z3}^w 001\\ C_{x4}^w C_{y4}^w C_{z4}^w000000100 \\ 000C_{x4}^w C_{y4}^w C_{z4}^w 000010\\ 000000 C_{x4}^w C_{y4}^w C_{z4}^w 001 \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} CjcRCjwt CxjcCyjcCzjc r1r4r7r2r5r8r3r6r9 CxjwCyjwCzjw t1t2t3 Cxjcr1Cxjwr2Cyjwr3Czjwt1Cyjcr4Cxjwr5Cyjwr6Czjwt2Czjcr7Cxjwr8Cyjwr9Czjwt3 Cx1cCy1cCz1cCx2cCy2cCz2cCx3cCy3cCz3cCx4cCy4cCz4c Cx1w00Cx2w00Cx3w00Cx4w00Cy1w00Cy2w00Cy3w00Cy4w00Cz1w00Cz2w00Cz3w00Cz4w000Cx1w00Cx2w00Cx3w00Cx4w00Cy1w00Cy2w00Cy3w00Cy4w00Cz1w00Cz2w00Cz3w00Cz4w000Cx1w00Cx2w00Cx3w00Cx4w00Cy1w00Cy2w00Cy3w00Cy4w00Cz1w00Cz2w00Cz3w00Cz4w100100100100010010010010001001001001 r1r2r3r4r5r6r7r8r9t1t2t3
控制点选取
原则上就是只要选择3个线性无关的点就可以表示任意一个三维点但由于方程组是4如何是3个控制点只能求得最小二乘解。论文中给出了具体的选择方法。 3D参考点集为 { P i w , i 1 , ⋯ , n } \left\{P^w_i,i1,\cdots,n \right \} {Piw,i1,⋯,n}, 选择3D点的中心为第一个控制点: C 1 w 1 n ∑ i 1 n P i w C^w_1\frac{1}{n}\sum_{i1}^nP^w_i C1wn1i1∑nPiw 进而得到矩阵: A [ ( P 1 w ) T − ( C 1 w ) T ⋯ ( P n w ) T − ( C n w ) T ] A\begin{bmatrix}{(P_{1}^{w})}^{T} - {(C_{1}^{w})}^{T} \\ \cdots \\ {(P_{n}^{w})}^{T} - {(C_{n}^{w})}^{T} \end{bmatrix} A (P1w)T−(C1w)T⋯(Pnw)T−(Cnw)T 记 A T A A^TA ATA的特征值为 λ i \lambda_i λi, 特征向量为 V i V_i Vi ,那么剩下的三个点为 C w C 1 w λ i 1 2 V i , i 1 , 2 , 3 C^w C_1^w \lambda^{\frac{1}{2}}_iV_i,\ i 1,2,3 CwC1wλi21Vi, i1,2,3
求解 α \alpha α
联立方程组: P i w α i 1 C 1 w α i 2 C 2 w α i 3 C 3 w α i 4 C 4 w [ x i y i z i ] α i 1 [ C x 1 w C y 1 w C z 1 w ] α i 2 [ C x 2 w C y 2 w C z 2 w ] α i 3 [ C x 3 w C y 3 w C z 3 w ] α i 4 [ C x 4 w C y 4 w C z 4 w ] \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} Piwαi1C1wαi2C2wαi3C3wαi4C4w xiyizi αi1 Cx1wCy1wCz1w αi2 Cx2wCy2wCz2w αi3 Cx3wCy3wCz3w αi4 Cx4wCy4wCz4w 每个点可以得到4个方程组: x i α i 1 C x 1 w α i 2 C x 2 w α i 3 C x 3 w α i 4 C x 4 w y i α i 1 C y 1 w α i 2 C y 2 w α i 3 C y 3 w α i 4 C y 4 w z i α i 1 C z 1 w α i 2 C z 2 w α i 3 C z 3 w α i 4 C z 4 w 1 α i 1 α i 2 α i 3 α i 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} xiαi1Cx1wαi2Cx2wαi3Cx3wαi4Cx4wyiαi1Cy1wαi2Cy2wαi3Cy3wαi4Cy4wziαi1Cz1wαi2Cz2wαi3Cz3wαi4Cz4w1αi1αi2αi3αi4 用矩阵的方式可表示为 [ x i y i z i 1 ] [ C x 1 w C x 2 w C x 3 w C x 4 w C y 1 w C y 2 w C y 3 w C y 4 w C z 1 w C z 2 w C z 3 w C z 4 w 1 1 1 1 ] [ α i 1 α i 2 α i 3 α i 4 ] ⟶ [ C x 1 w C x 2 w C x 3 w C x 4 w C y 1 w C y 2 w C y 3 w C y 4 w C z 1 w C z 2 w C z 3 w C z 4 w 1 1 1 1 ] − 1 [ x i y i z i 1 ] [ α i 1 α i 2 α i 3 α i 4 ] \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 \\ 1111 \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 \\ 1111 \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} xiyizi1 Cx1wCy1wCz1w1Cx2wCy2wCz2w1Cx3wCy3wCz3w1Cx4wCy4wCz4w1 αi1αi2αi3αi4 ⟶ Cx1wCy1wCz1w1Cx2wCy2wCz2w1Cx3wCy3wCz3w1Cx4wCy4wCz4w1 −1 xiyizi1 αi1αi2αi3αi4
void Rebuild::ePnP(const std::vectorEigen::Vector3d p3ds, const std::vectorEigen::Vector2d p2ds, Camera camera) {auto blog _blog;blog-write(ePnP:);/**寻找4个控制点**///求取重心Eigen::Vector3d center Eigen::Vector3d::Zero();std::vectorEigen::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::EigenSolverEigen::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::JacobiSVDEigen::MatrixXd svd(D, Eigen::ComputeFullV | Eigen::ComputeFullU);auto V svd.matrixV();std::vectorEigen::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);
}
