import numpy as np
import cv2
import matplotlib.pyplot as plt
from scipy.linalg import lstsq
def fit_camera_poly_model_3d(world_points, image_points, order):
"""
拟合多项式模型(包含XYZ坐标):世界坐标 -> 图像坐标
:param world_points: N×3数组,世界坐标(X,Y,Z)
:param image_points: N×2数组,图像坐标(u,v)
:param order: 多项式阶数(2或3)
:return: 两个函数 (modelU, modelV) 分别预测u和v坐标
"""
X = world_points[:, 0]
Y = world_points[:, 1]
Z = world_points[:, 2]
if order == 2:
# 二阶多项式: 1, x, y, z, x², xy, xz, y², yz, z²
A = np.column_stack([
np.ones_like(X), X, Y, Z,
X ** 2, X * Y, X * Z, Y ** 2, Y * Z, Z ** 2
])
elif order == 3:
# 三阶多项式: 1, x, y, z, x², xy, xz, y², yz, z²,
# x³, x²y, x²z, xy², xyz, xz², y³, y²z, yz², z³
A = np.column_stack([
np.ones_like(X), X, Y, Z,
X ** 2, X * Y, X * Z, Y ** 2, Y * Z, Z ** 2,
X ** 3, X ** 2 * Y, X ** 2 * Z, X * Y ** 2, X * Y * Z, X * Z ** 2,
Y ** 3, Y ** 2 * Z, Y * Z ** 2, Z ** 3
])
else:
raise ValueError("多项式阶数必须是2或3")
# 拟合u坐标
u = image_points[:, 0]
coeffs_u, _, _, _ = lstsq(A, u)
# 拟合v坐标
v = image_points[:, 1]
coeffs_v, _, _, _ = lstsq(A, v)
# 创建预测函数
def modelU(x, y, z):
return poly_val_3d(x, y, z, coeffs_u, order)
def modelV(x, y, z):
return poly_val_3d(x, y, z, coeffs_v, order)
return modelU, modelV
def poly_val_3d(x, y, z, coeffs, order):
"""
三维多项式求值
:param x: x坐标值(标量或数组)
:param y: y坐标值(标量或数组)
:param z: z坐标值(标量或数组)
:param coeffs: 多项式系数
:param order: 多项式阶数
:return: 多项式计算结果
"""
if order == 2:
terms = np.column_stack([
np.ones_like(x), x, y, z,
x ** 2, x * y, x * z, y ** 2, y * z, z ** 2
])
else: # order == 3
terms = np.column_stack([
np.ones_like(x), x, y, z,
x ** 2, x * y, x * z, y ** 2, y * z, z ** 2,
x ** 3, x ** 2 * y, x ** 2 * z, x * y ** 2, x * y * z, x * z ** 2,
y ** 3, y ** 2 * z, y * z ** 2, z ** 3
])
return np.dot(terms, coeffs)
def ortho_rectification_fixed_z(org_image, ortho_image_x, ortho_image_y, modelU, modelV, fixed_z):
"""
正射纠正(固定Z值)
:param org_image: 原始图像 (H, W, C)
:param ortho_image_x: 正射影像网格的X坐标 (H, W)
:param ortho_image_y: 正射影像网格的Y坐标 (H, W)
:param modelU: u坐标预测函数 (需要xyz三个参数)
:param modelV: v坐标预测函数 (需要xyz三个参数)
:param fixed_z: 固定的Z值
:return: 正射纠正后的图像
"""
height, width = ortho_image_x.shape
channels = 1 if len(org_image.shape) == 2 else org_image.shape[2]
# 展平网格坐标以提高效率
X_flat = ortho_image_x.ravel()
Y_flat = ortho_image_y.ravel()
Z_flat = np.full_like(X_flat, fixed_z) # 使用固定Z值
# 批量预测图像坐标
u_flat = modelU(X_flat, Y_flat, Z_flat)
v_flat = modelV(X_flat, Y_flat, Z_flat)
# 重塑为网格
u_map = u_flat.reshape((height, width))
v_map = v_flat.reshape((height, width))
# 创建映射矩阵(OpenCV需要xy坐标)
map_x = u_map.astype(np.float32)
map_y = v_map.astype(np.float32)
# 使用remap进行重映射(包含插值)
if channels == 1:
ortho_image = cv2.remap(org_image, map_x, map_y, cv2.INTER_LINEAR)
else:
# 分通道处理彩色图像
ortho_image = np.zeros((height, width, channels), dtype=org_image.dtype)
for c in range(channels):
ortho_image[:, :, c] = cv2.remap(org_image[:, :, c], map_x, map_y, cv2.INTER_LINEAR)
return ortho_image
def calculate_resolution(world_points, image_points):
"""
计算图像分辨率(像素/世界单位)
:param world_points: N×3数组,世界坐标
:param image_points: N×2数组,图像坐标
:return: 平均分辨率
"""
# 计算相邻点间的世界距离(考虑XYZ)
world_diffs = np.diff(world_points, axis=0)
world_dists = np.sqrt(np.sum(world_diffs ** 2, axis=1))
# 计算相邻点间的图像距离
image_diffs = np.diff(image_points, axis=0)
image_dists = np.sqrt(np.sum(image_diffs ** 2, axis=1))
# 计算每像素代表的世界单位数
resolutions = world_dists / (image_dists + 1e-9) # 避免除零
return np.nanmean(resolutions[np.isfinite(resolutions)])
def calculate_optimal_z(world_points):
"""
计算最优的固定Z值(取所有控制点Z值的中位数)
:param world_points: N×3数组,世界坐标
:return: 最优固定Z值
"""
return np.median(world_points[:, 2])
# ===================== 使用真实数据 =====================
if __name__ == "__main__":
# 加载真实数据(替换为您的实际数据)
# worldPoints3D: N×3数组 [X, Y, Z]
# imagePoints: N×2数组 [u, v]
# org_image: 原始图像
# 示例数据结构(实际使用时替换为您的数据)
worldPoints3D = np.array( [
[4262845.751, 644463.054, 64.289], [4262844.149, 644468.755, 63.701]
, [4262846.106, 644470.001, 63.621]
, [4262844.678, 644473.34, 63.613], [4262838.693, 644473.677, 63.565]
, [4262833.235, 644478.754, 63.722]
, [4262853.252, 644455.049, 64.177]
, [4262851.546, 644451.282, 64.875], [4262837.259, 644438.863, 67.325]
, [4262841.986, 644437.241, 65.751], [4262846.305, 644431.715, 66.42]
, [4262851.769, 644434.948, 64.503]
, [4262850.889, 644423.337, 68.04], [4262858.526, 644426.641, 64.81]
, [4262860.791, 644431.732, 63.853], [4262865.608, 644419.362, 66.008]
, [4262865.978, 644423.624, 64.7]
, [4262857.58, 644470.911, 63.636], [4262854.209, 644478.653, 63.629]
, [4262858.634, 644475.401, 63.64]
])
imagePoints = np.array([
[1058, 614], [855, 680]
, [901, 711]
, [727, 739], [480, 689]
, [140, 694]
, [1452, 599]
, [1422, 545], [1439, 421]
, [1269, 467], [1401, 447]
, [1515, 513]
, [1534, 409], [1677, 494]
, [1729, 533], [1819, 471]
, [1828, 506]
, [1532, 816], [1100, 983]
, [1557, 940]
])
# 加载原始图像
org_image = cv2.imread(r'D:\shuiwei\2.jpg') # 替换为您的图像路径
# 设置参数
polyOrder = 3 # 多项式阶数(2或3)
# 1. 拟合多项式模型(包含XYZ坐标)
modelU, modelV = fit_camera_poly_model_3d(worldPoints3D, imagePoints, polyOrder)
# 2. 确定正射影像范围和分辨率
x_min, x_max = worldPoints3D[:, 0].min(), worldPoints3D[:, 0].max()
y_min, y_max = worldPoints3D[:, 1].min(), worldPoints3D[:, 1].max()
# 计算分辨率(像素/世界单位)
imageRes = calculate_resolution(worldPoints3D, imagePoints)
# 3. 计算最优固定Z值(或手动指定)
#fixed_z = calculate_optimal_z(worldPoints3D) # 自动计算最优Z值
fixed_z = 64.3 # 或者手动指定固定Z值
print(f"使用固定Z值: {fixed_z:.2f}")
# 4. 生成正射影像网格
x_grid = np.arange(x_min, x_max, imageRes)
y_grid = np.arange(y_min, y_max, imageRes)
orthoImageX, orthoImageY = np.meshgrid(x_grid, y_grid)
# 5. 执行正射纠正(使用固定Z值)
orthoImage = ortho_rectification_fixed_z(
org_image, orthoImageX, orthoImageY, modelU, modelV, fixed_z
)
# 6. 保存和显示结果
cv2.imwrite(r'D:\shuiwei\current_view11.jpg', orthoImage)
plt.figure(figsize=(12, 6))
plt.subplot(121)
plt.title("Original Image")
plt.imshow(cv2.cvtColor(org_image, cv2.COLOR_BGR2RGB))
plt.subplot(122)
plt.title("Orthorectified Image")
plt.imshow(cv2.cvtColor(orthoImage, cv2.COLOR_BGR2RGB))
plt.tight_layout()
plt.show()
print(f"分辨率:{imageRes}")
生成的图像最后横向和纵向是搞反了吗?输入的图片横向像素比纵向多,但是输出的图片看上去是横纵交换了?该怎么解决呢?
最新发布