使用python调用opencv中的函数solvepnp

先上代码

# -*- coding: utf-8 -*-
# 测试使用opencv中的函数solvepnp
import cv2
import numpy as np
tag_size = 0.05
tag_size_half = 0.025
fx = 610.32366943
fy = 610.5026245
cx = 313.3859558
cy = 237.2507269
K = np.array([[fx, 0, cx],
              [0, fy, cy],
              [0, 0, 1]], dtype=np.float64)
objPoints = np.array([[-tag_size_half, -tag_size_half, 0],
                      [tag_size_half, -tag_size_half, 0],
                      [tag_size_half, tag_size_half, 0],
                      [-tag_size_half, tag_size_half, 0]], dtype=np.float64)
imgPoints = np.array([[608, 167], [514, 167], [518, 69], [611, 71]], dtype=np.float64)
cameraMatrix = K
distCoeffs = None
retval,rvec,tvec  = cv2.solvePnP(objPoints, imgPoints, cameraMatrix, distCoeffs)
# cv2.Rodrigues()
print retval, rvec, tvec

碰到过的问题：

OpenCV Error: Assertion failed (( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess) ) && npoints == std::max(ipoints.checkVector(2, 5), ipoints.checkVector(2, 6))) in solvePnP, file /tmp/binarydeb/ros-kinetic-opencv3-3.3.1/modules/calib3d/src/solvepnp.cpp, line 65
Traceback (most recent call last):
  File "/home/sujie/ROS/catkin_ws/src/optimize_pose/src/opencv_test/pnp_test.py", line 21, in
    retval,rvec,tvec  = cv2.solvePnP(objPoints, imgPoints, cameraMatrix, distCoeffs)
cv2.error: /tmp/binarydeb/ros-kinetic-opencv3-3.3.1/modules/calib3d/src/solvepnp.cpp:65: error: (-215) ( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess) ) && npoints == std::max(ipoints.checkVector(2, 5), ipoints.checkVector(2, 6)) in function solvePnP
 

因为在np.array([ ] , dtype=np.float64)缺少了后面的,dtype=np.float64类型

 

参考　https://www.cnblogs.com/aoru45/p/9781540.html