solvepnp

solvepnp测距，提前push世界坐标，根据图像坐标求解pnp，使用ITERATIVE方法，（P3P结果存在较大误差），结果重投影回图像坐标系，判断误差，测试误差在0.5像素内。