python opencv2_python+opencv2相机位姿估计

最近在做基于图像的室内定位方面的研究，于是使用到了百度最新的室内数据库Image-based Localization (IBL) 。由于该数据库给出的数据是每幅图像和其对应相机的内外参数和光心投影方向，所以我需要先求出其6DOF预估姿态。再利用PoseNet网络对其实现基于图像的定位估计。好了，问题就很明确了：

(1)根据图像和激光雷达参数的3D点云实现2D-3D的匹配，找到每张图像上的至少四个特征点。即找到至少4个二维像素和3D点云点的对应点。

(2)根据这四组对应点和相机内外参数估计相机6DOF，即相机姿态。

今天先实现第二个问题。很幸运网上有这样几篇博客已经将相机位姿整个过程讲的比较清楚了http://www.cnblogs.com/singlex/p/pose_estimation_1.html。

但这篇文章是由c++写的，我在python上简单的对其进行了验证。

这是这张图给出的数据。

importcv2importnumpy as npimportmath

object_3d_points=np.array(([0, 0, 0],

[0,200, 0],

[150, 0, 0],

[150, 200, 0]), dtype=np.double)

object_2d_point= np.array(([2985, 1688],

[5081, 1690],

[2997, 2797],

[5544, 2757]), dtype=np.double)

camera_matrix= np.array(([6800.7, 0, 3065.8],

[0,6798.1, 16