opencv: solvePnP和matlab: estimateWorldCameraPose 比较

这两种函数都可以求解pnp问题，得到的结果可以相互转换。

import cv2
from scipy.spatial.transform import Rotation as R

ret, rvecs, tvecs = cv2.solvePnP(corners_3d, corners_2d, K, dist)
gt_mat[:3, :3] = R.from_rotvec(rvecs.T).as_matrix()
gt_mat[:3, 3:] = tvecs  

输入：

世界坐标系 corners_3d: N×3，数据类型np.float32，图像坐标系 corners_2d: N×2，内参矩阵 K: 3×3，畸变 dist一般设置为0

输出：

rvecs: 旋转向量，tvecs: 位移向量（这里我理解应该是和旋转矩阵中的位移保持一致的），然后转换为旋转矩阵的形式。

记 Ro = gt_mat[:3, :3]，to = gt_mat[:3, 3:] 

cameraParams = cameraParameters('IntrinsicMatrix', K'); 
[worldOrientation,worldLocation] = estimateWorldCameraPose(corners_2d,corners_3d,cameraParams);

输入：

世界坐标系 corners_3d: N×3，数据类型double，图像坐标系 corners_2d: N×2，数据类型double

输出：

worldOrientation: 旋转矩阵，worldLocation：位移。

记 Rm = worldOrientation，tm = worldLocation

有：Ro = Rm，to = -Rm·tm