cv2.solvePnP 报错 求相机位姿

目录

报错信息及解决：

cv2.solvePnP 使用例子：

设置初始值效果也不好

cv2.projectPoints 函数效果不好

---

报错信息及解决：

File "/shared_disk/users/lbg/project/human_4d/nlf_pose/render_demo_pkl2_cal.py", line 236, in <module> success, rotation_vector, translation_vector = cv2.solvePnP(vertices, vertices2d, camera_matrix, dist_coeffs) cv2.error: OpenCV(4.10.0) /io/opencv/modules/calib3d/src/solvepnp.cpp:823: error: (-215:Assertion failed) ( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess) || (npoints >= 3 && flags == SOLVEPNP_SQPNP) ) && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) in function 'solvePnPGeneric'

解决方法：

把所有数据都astype(np.float32)

cv2.solvePnP 使用例子：

import cv2
import numpy as np# 三维物体点在世界坐标系中的坐标
object_points = np.array([[0, 0, 0],[0, 1, 0],[1, 1, 0],[1, 0, 0]
]).astype(np.float32)# 这些三维点在图像平面上对应的二维像素坐标
image_points = np.array([[100, 100],[100, 200],[200, 200],[200, 100]
], dtype=np.float64)# 相机的内参矩阵
camera_matrix = np.array([[1000, 0, 320],[0, 1000, 240],[0, 0, 1]
], dtype=np.float64)# 相机的畸变系数
dist_coeffs = np.zeros((5, 1), dtype=np.float32)# 求解PnP问题
success, rotation_vector, translation_vector = cv2.solvePnP(object_points, image_points, camera_matrix, dist_coeffs)if success:print("旋转向量:")print(rotation_vector)print("平移向量:")print(translation_vector)
else:print("求解失败")

设置初始值效果也不好

import cv2
import numpy as np# 假设 vertices 和 vertices2d 已经准备好了
# vertices 是 3D 点集合 (Nx3)
# vertices2d 是对应的 2D 点集合 (Nx2)
# camera_matrix 是相机内参矩阵
# dist_coeffs 是畸变系数（如果有）# 相机的内参矩阵
f_x = 1000  # 焦距 fx
f_y = 1000  # 焦距 fy
c_x = 640   # 主点 cx
c_y = 360   # 主点 cy
camera_matrix = np.array([[f_x, 0, c_x],[0, f_y, c_y],[0, 0, 1]
])# 畸变系数（假设无畸变）
dist_coeffs = np.zeros(5)# 初始旋转向量（设为零）
rvec_init = np.zeros(3)  # 初始旋转为零（单位向量）
tvec_init = np.zeros(3)  # 初始平移为零# 使用 solvePnP 计算平移并强制旋转为零
success, rvec, tvec = cv2.solvePnP(vertices,        # 3D 点vertices2d,      # 对应的 2D 点camera_matrix,   # 相机内参矩阵dist_coeffs,     # 畸变系数rvec_init,       # 初始旋转向量（零）tvec_init,       # 初始平移向量（零）useExtrinsicGuess=True  # 使用提供的初始旋转和平移
)# 输出计算结果
print(f"旋转向量 (rvec): {rvec}")
print(f"平移向量 (tvec): {tvec}")

cv2.projectPoints 函数效果不好

import cv2
import numpy as np
from scipy.optimize import least_squaresdef project_without_rotation(t, object_points, camera_matrix, dist_coeffs):rvec = np.zeros((3, 1))  # 零旋转tvec = t.reshape(3, 1)projected, _ = cv2.projectPoints(object_points, rvec, tvec, camera_matrix, dist_coeffs)return projected.reshape(-1, 2)def residual(t, object_points, image_points, camera_matrix, dist_coeffs):projected = project_without_rotation(t, object_points, camera_matrix, dist_coeffs)return (projected - image_points).ravel()# 输入数据：3D点、2D点、相机矩阵、畸变系数
vertices = np.array([...], dtype=np.float32)  # 替换为实际3D点
vertices2d = np.array([...], dtype=np.float32)  # 替换为实际2D点
camera_matrix = np.array([...], dtype=np.float32)  # 替换为实际相机矩阵
dist_coeffs = np.array([...], dtype=np.float32)  # 替换为实际畸变系数，可为None# 初始猜测，例如零平移
t_initial = np.zeros(3)# 可选：使用线性解法获取更好的初始值（见注释部分）# 非线性优化
result = least_squares(residual, t_initial, args=(vertices, vertices2d, camera_matrix, dist_coeffs))
t_opt = result.xprint("优化后的平移向量:", t_opt)