```
def pnp(points_3d, points_2d, camera_matrix, method=cv2.SOLVEPNP_ITERATIVE):
pic = np.zeros(shape=[9, 2])
try:
dist_coeffs = pnp.dist_coeffs # 畸变系数
except:
# dist_coeffs = np.zeros(shape=[8, 1], dtype='float64')
pic[:, 0] = points_2d[:, 0] - 240
pic[:, 1] = points_2d[:, 1] - 160
assert points_3d.shape[0] == points_2d.shape[0], 'points 3D and points 2D must have same number of vertices'
# R1 = np.dot(points_2d.T, numpy.linalg.pinv(points_3d).T)
# model = numpy.linalg.pinv(points_3d)
# R = np.dot(numpy.linalg.pinv(camera_matrix), R1)
t = np.array([[0],
[0],
[0]])
# 输入数据（示例）
proj = camera_matrix # 2x3
model = points_3d.T # 3x9
pic = pic.T # 2x9
# 构造线性方程组 A * vec(R) = b
n_points = model.shape[1]
A = np.zeros((2 * n_points, 9))
b = np.zeros(2 * n_points)
for i in range(n_points):
x, y, z = model[:, i]
p0 = proj[0, :]
p1 = proj[1, :]
# 第一行方程系数 (对应 proj 的第一行)
A[2 * i] = [
p0[0] * x, p0[0] * y, p0[0] * z,
p0[1] * x, p0[1] * y, p0[1] * z,
p0[2] * x, p0[2] * y, p0[2] * z
]
# 第二行方程系数 (对应 proj 的第二行)
A[2 * i + 1] = [
p1[0] * x, p1[0] * y, p1[0] * z,
p1[1] * x, p1[1] * y, p1[1] * z,
p1[2] * x, p1[2] * y, p1[2] * z
]
# 观测值
b[2 * i] = pic[0, i]
b[2 * i + 1] = pic[1, i]
# 最小二乘求解
vec_R, _, _, _ = np.linalg.lstsq(A, b, rcond=None)
R_est = vec_R.reshape(3, 3)
# 对 R_est 进行 SVD 正交化
U, S, Vt = np.linalg.svd(R_est)
R_ortho = U @ Vt
# 确保行列式为 1
if np.linalg.det(R_ortho) < 0:
Vt[-1, :] *= -1
R_ortho = U @ Vt
# print("Estimated Rotation Matrix R:")
# print(R_ortho)
R = R_ortho
projected = proj @ R_ortho @ model
error = np.linalg.norm(projected - pic)
# print('projected is ', projected)
# print(f"Projection Error: {error}")
# 已知矩阵
# proj = camera_matrix # 2x3
# model = points_3d.T # 3x9
# pic = points_2d.T # 2x9
# print('model is ', model)
# print('pic is ', pic)
# # 构造Kronecker积矩阵和向量
# K = np.kron(model.T, proj) # 18x9
# b = pic.T.reshape(-1, 1) # 18x1
#
# # 最小二乘求解
# vec_R, residuals, rank, _ = np.linalg.lstsq(K, b, rcond=None)
# R_initial = vec_R.reshape(3, 3).T
#
# # SVD投影到SO(3)
# U, S, Vt = np.linalg.svd(R_initial)
# D = np.eye(3)
# D[2, 2] = np.linalg.det(U @ Vt)
# R = U @ D @ Vt
#
# # 验证
# pic_reconstructed = proj @ R @ model
# error = np.linalg.norm(pic - pic_reconstructed)
# print('pic_reconstructed is ', pic_reconstructed)
# print(f"投影误差：{error}")
# print('R is ', R)
# print('t is ', t)
return np.concatenate([R, t], axis=-1)```这个代码输出的R不是正交矩阵
最新发布
扫一扫
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
