cv2.solvePnP 报错 求相机位姿

目录

报错信息及解决：

[cv2.solvePnP 使用例子：](#cv2.solvePnP 使用例子：)

设置初始值效果也不好

[cv2.projectPoints 函数效果不好](#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_squares

def 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.x

print("优化后的平移向量:", t_opt)