5. 把演示程序使用的EPnP 改成其他PnP 方法，并研究它们的工作原理。

下面的代码是关于EPNP算法的，具体的原理请参考原文。

代码链接：https://github.com/jessecw/EPnP_Eigen

在README.md的文件里有具体的执行方法，不过需要注意的是执行程序实在bin文件夹下的

 文件具体分布如下所示：

 执行结果：

./test_epnp 

eigen_vector:
1 0 0 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0 0 0
0 0 1 0 0 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0 0 0 0
0 0 0 0 1 0 0 0 0 0 0 0
0 0 0 0 0 1 0 0 0 0 0 0
0 0 0 0 0 0 1 0 0 0 0 0
0 0 0 0 0 0 0 1 0 0 0 0
0 0 0 0 0 0 0 0 1 0 0 0
0 0 0 0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 0 0 0 0 1 0
0 0 0 0 0 0 0 0 0 0 0 1
R
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
R_estimated 
1 0 0
0 1 0
0 0 1
t_cw
1
0
0
t_estimated
-nan
-nan
-nan

./test_dlt 

R
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
R_estimated 
 3.39519e-313  6.90396e-310  6.90396e-310
-3.18328e+162  6.90396e-310  6.90396e-310
 6.90396e-310  6.90396e-310  6.90396e-310
R_derter:0
K
100   0 160
  0 100 120
  0   0   1
K_estimated
6.90396e-310 6.95278e-310 6.95278e-310
6.90396e-310 4.65766e-310 6.90396e-310
6.90396e-310 6.95278e-310 6.90396e-310
t_w
1
0
0
t_estimated
0
0

转载于：

EPnP算法 - 知乎https://zhuanlan.zhihu.com/p/76361026