import numpy as np
# 保留原始代码中的函数不变
def normalize_coordinates(points):
"""坐标归一化处理"""
points_arr = np.array(points)
mean = np.mean(points_arr, axis=0)
std = np.std(points_arr, axis=0)
# 防止零标准差
scale = np.where(std < 1e-8, 1.0, std)
normalized_points = (points_arr - mean) / scale
return normalized_points, mean, scale
def build_design_matrix(normalized_points):
"""构建新的多项式设计矩阵：XY三次多项式 + Z二次多项式"""
A = []
for (X, Y, Z) in normalized_points:
# XY三次多项式特征：包括X^3, Y^3, X^2Y, XY^2等
# Z二次多项式特征：Z, Z^2 (不包括Z^3及以上)
row = [
1, X, Y, Z, # 常数项和线性项
X ** 2, Y ** 2, XY := X * Y, # 二次项（不包括Z^2）
X * Z, Y * Z, # 交叉项（Z线性）
X ** 3, Y ** 3, X ** 2 * Y, X * Y ** 2, # XY三次项
X ** 2 * Z, Y ** 2 * Z, XY * Z, # 带Z的二次项
X * Z ** 2, Y * Z ** 2, # 带Z^2的项（二次项）
Z ** 2 # Z二次项（单独）
]
A.append(row)
return np.array(A)
def compute_reprojection_errors(coeff_u, coeff_v, mean, scale, points, pixels):
"""计算重投影误差"""
errors = []
for i, point in enumerate(points):
# 预测像素坐标
u_pred, v_pred = world_to_pixel(coeff_u, coeff_v, mean, scale, point)
# 实际像素坐标
u_obs, v_obs = pixels[i]
# 计算欧氏距离误差
error = np.sqrt((u_pred - u_obs) ** 2 + (v_pred - v_obs) ** 2)
errors.append(error)
# 计算平均误差
mean_error = np.mean(errors)
return errors, mean_error
def world_to_pixel(coeff_u, coeff_v, mean, scale, point):
"""世界坐标转像素坐标（考虑归一化）"""
# 归一化处理
X_norm = (point[0] - mean[0]) / scale[0]
Y_norm = (point[1] - mean[1]) / scale[1]
Z_norm = (point[2] - mean[2]) / scale[2]
# 构建新的特征向量：XY三次多项式 + Z二次多项式
features = np.array([
1, X_norm, Y_norm, Z_norm,
X_norm ** 2, Y_norm ** 2, XY := X_norm * Y_norm,
X_norm * Z_norm, Y_norm * Z_norm,
X_norm ** 3, Y_norm ** 3, X_norm ** 2 * Y_norm, X_norm * Y_norm ** 2,
X_norm ** 2 * Z_norm, Y_norm ** 2 * Z_norm, XY * Z_norm,
X_norm * Z_norm ** 2, Y_norm * Z_norm ** 2,
Z_norm ** 2
])
u = np.dot(features, coeff_u)
v = np.dot(features, coeff_v)
return u, v
def solve(points, pixels):
"""根据点的数量自动选择求解方法（特征增加到18项）"""
n = len(points)
min_points = 18 # 新模型需要至少18个点（18个系数）
if n < min_points:
raise ValueError(f"至少需要{min_points}个点来求解系数，当前只有{n}个点")
# 检查点与像素数量是否匹配
if len(pixels) != n:
raise ValueError(f"世界坐标点数量({n})与像素坐标点数量({len(pixels)})不匹配")
# 坐标归一化
norm_points, mean, scale = normalize_coordinates(points)
A = build_design_matrix(norm_points)
u_obs = np.array([p[0] for p in pixels])
v_obs = np.array([p[1] for p in pixels])
# 根据点数选择求解方法
if n == min_points:
# 正定情况求解
coeff_u = np.linalg.solve(A, u_obs)
coeff_v = np.linalg.solve(A, v_obs)
solver_type = f"正定求解 (点数={min_points})"
else:
# 超定情况求解
coeff_u, _, _, _ = np.linalg.lstsq(A, u_obs, rcond=None)
coeff_v, _, _, _ = np.linalg.lstsq(A, v_obs, rcond=None)
solver_type = f"超定求解 (点数={n}>{min_points})"
# 计算重投影误差
errors, mean_error = compute_reprojection_errors(
coeff_u, coeff_v, mean, scale, points, pixels)
return coeff_u, coeff_v, mean, scale, errors, mean_error, solver_type
# 新增函数：从像素坐标和已知XY求解Z值
def solve_z_from_xy(coeff_u, coeff_v, mean, scale, X, Y, u_obs, v_obs, max_iter=100, tol=1e-6):
"""
从像素坐标和已知XY求解Z值
使用牛顿迭代法求解非线性方程
参数:
coeff_u, coeff_v: 多项式系数
mean, scale: 归一化参数
X, Y: 已知的世界坐标XY值
u_obs, v_obs: 观测到的像素坐标
max_iter: 最大迭代次数
tol: 收敛阈值
返回:
Z: 求解的世界坐标Z值
"""
# 归一化已知的XY坐标
X_norm = (X - mean[0]) / scale[0]
Y_norm = (Y - mean[1]) / scale[1]
# 初始Z值（使用世界坐标Z的均值）
Z = mean[2]
for i in range(max_iter):
# 归一化当前Z值
Z_norm = (Z - mean[2]) / scale[2]
# 构建特征向量 (18维)
features = np.array([
1, X_norm, Y_norm, Z_norm,
X_norm ** 2, Y_norm ** 2, X_norm * Y_norm,
X_norm * Z_norm, Y_norm * Z_norm,
X_norm ** 3, Y_norm ** 3, X_norm ** 2 * Y_norm, X_norm * Y_norm ** 2,
X_norm ** 2 * Z_norm, Y_norm ** 2 * Z_norm, (X_norm * Y_norm) * Z_norm,
X_norm * Z_norm ** 2, Y_norm * Z_norm ** 2,
Z_norm ** 2
])
# 计算预测的像素坐标
u_pred = np.dot(features, coeff_u)
v_pred = np.dot(features, coeff_v)
# 计算残差
res_u = u_pred - u_obs
res_v = v_pred - v_obs
residual = np.sqrt(res_u ** 2 + res_v ** 2)
# 检查是否收敛
if residual < tol:
break
# 计算特征向量对Z的导数 (用于牛顿法)
df_dz = np.array([
0, 0, 0, 1 / scale[2],
0, 0, 0,
X_norm / scale[2], Y_norm / scale[2],
0, 0, 0, 0,
X_norm ** 2 / scale[2], Y_norm ** 2 / scale[2], (X_norm * Y_norm) / scale[2],
2 * X_norm * Z_norm / scale[2], 2 * Y_norm * Z_norm / scale[2],
2 * Z_norm / scale[2]
])
# 计算像素坐标对Z的导数
du_dz = np.dot(df_dz, coeff_u)
dv_dz = np.dot(df_dz, coeff_v)
# 牛顿法更新: ΔZ = -J⁻¹ * F
# 其中F是残差向量 [res_u, res_v], J是雅可比矩阵 [[du_dz], [dv_dz]]
# 使用伪逆处理过定系统
J = np.array([[du_dz], [dv_dz]])
F = np.array([[res_u], [res_v]])
# 计算雅可比矩阵的伪逆
J_pseudo_inv = np.linalg.pinv(J)
# 计算Z的更新量
delta_Z = -J_pseudo_inv.dot(F)[0, 0]
# 更新Z值
Z += delta_Z
# 防止Z值发散
if abs(delta_Z) > 100:
raise RuntimeError(f"迭代发散，当前delta_Z={delta_Z:.4f}")
return Z
if __name__ == "__main__":
# 示例数据（需要至少18个点）
world_points = [
[4262845.751, 644463.054, 63.939], [4262844.149, 644468.755, 63.351],
[4262846.106, 644470.001, 63.271], [4262847.099, 644471.268, 63.415],
[4262844.678, 644473.34, 63.263], [4262838.693, 644473.677, 63.215],
[4262833.235, 644478.754, 63.372], [4262838.601, 644477.972, 63.281],
[4262853.252, 644455.049, 63.827], [4262851.546, 644451.282, 64.525],
[4262837.259, 644438.863, 66.975], [4262841.986, 644437.241, 65.401],
[4262846.305, 644431.715, 66.07], [4262851.769, 644434.948, 64.153],
[4262852.526, 644431.694, 64.434], [4262850.889, 644423.337, 67.69],
[4262858.526, 644426.641, 64.46], [4262860.791, 644431.732, 63.503],
[4262865.608, 644419.362, 65.658], [4262865.978, 644423.624, 64.35],
[4262857.58, 644470.911, 63.286], [4262854.209, 644478.653, 63.279],
[4262858.634, 644475.401, 63.29]
]
pixel_points = [
[1058, 614], [855, 680],
[901, 711], [913, 726],
[727, 739], [480, 689],
[140, 694], [295, 736],
[1452, 599], [1422, 545],
[1439, 421], [1269, 467],
[1401, 447], [1515, 513],
[1542, 497], [1534, 409],
[1677, 494], [1729, 533],
[1819, 471], [1828, 506],
[1532, 816], [1100, 983],
[1557, 940]
]
# 使用所有输入点求解多项式系数
n = len(world_points)
print(f"使用{n}个点进行求解:")
coeff_u, coeff_v, mean, scale, errors, mean_error, solver_type = solve(world_points, pixel_points)
print(f"求解方法: {solver_type}")
print(f"平均重投影误差: {mean_error:.4f}像素")
# 测试点：使用原始数据中的一个点
test_index = 0 # 选择第一个点进行测试
X_known = world_points[test_index][0]
Y_known = world_points[test_index][1]
u_obs, v_obs = pixel_points[test_index]
Z_true = world_points[test_index][2]
print(f"\n测试点信息:")
print(f"已知世界坐标 XY: ({X_known:.3f}, {Y_known:.3f})")
print(f"已知像素坐标 UV: ({u_obs}, {v_obs})")
print(f"真实Z值: {Z_true:.3f}")
# 求解Z值
Z_solution = solve_z_from_xy(
coeff_u, coeff_v, mean, scale,
X_known, Y_known, u_obs, v_obs
)
print(f"\n求解的Z值: {Z_solution:.3f}")
print(f"与真实值的绝对误差: {abs(Z_solution - Z_true):.6f}")
print(f"相对误差: {abs(Z_solution - Z_true) / Z_true * 100:.4f}%")
# 验证：使用求解的Z值预测像素坐标
test_point = (X_known, Y_known, Z_solution)
u_pred, v_pred = world_to_pixel(coeff_u, coeff_v, mean, scale, test_point)
print(f"\n使用求解的Z值预测像素坐标: ({u_pred:.2f}, {v_pred:.2f})")
print(f"与观测值的误差: {np.sqrt((u_pred - u_obs) ** 2 + (v_pred - v_obs) ** 2):.4f}像素")简化这段代码
最新发布
该程序提供了一个计算重投影误差的函数，输入包括RGB图像、对象点、图像点、校准矩阵、旋转矩阵和平移向量。通过矩阵运算和图像处理，它计算并显示了每个点的重投影误差，最终返回平均重投影误差。
最低0.47元/天 解锁文章
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
