背景

已知相機(jī)參數(shù)（傳感器寬度和高度、圖像寬度和高度、焦距、相對(duì)航高、像主點(diǎn)坐標(biāo) ），在給定像素坐標(biāo)的前提下，求世界坐標(biāo)，大部分通過AI來實(shí)現(xiàn)，不知道哪個(gè)步驟有問題，望大家指正

腳本

import numpy as np
import cv2# 畸變校正
def undistort_pixel(pixel_x, pixel_y, sym_dist, dec_dist):k0,k1,k2,k3=sym_dist# k1, k2, p1, p2, k3 = sym_distp1,p2,p3=dec_distfx = focal_length_mmfy = focal_length_mmcx = xpoff_pxcy = ypoff_pxdistCoeffs = np.array([k1, k2, p1, p2,k3])cameraMatrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])distorted_points = np.array([[pixel_x, pixel_y]], dtype=np.float32)undistorted_points = cv2.undistortPoints(distorted_points, cameraMatrix, distCoeffs)#################################################### 4\對(duì)圖像去畸變img = cv2.imread('./images/100_0004_0001.JPG')img_undistored = cv2.undistort(img, cameraMatrix, distCoeffs)cv2.imwrite('./images/100_0004_00011.JPG', img_undistored)return undistorted_points[0][0][0], undistorted_points[0][0][1]# 相機(jī)坐標(biāo)轉(zhuǎn)世界坐標(biāo)
def camera_to_world_coordinates(cam_coords, pos):# 獲取相機(jī)到世界的轉(zhuǎn)換參數(shù)pos_x, pos_y, pos_z, roll, pitch, yaw = pos# 將角度轉(zhuǎn)換為弧度roll = np.radians(roll)pitch = np.radians(pitch)yaw = np.radians(yaw)# 計(jì)算旋轉(zhuǎn)矩陣R_roll = np.array([[1, 0, 0],[0, np.cos(roll), -np.sin(roll)],[0, np.sin(roll), np.cos(roll)]])R_pitch = np.array([[np.cos(pitch), 0, np.sin(pitch)],[0, 1, 0],[-np.sin(pitch), 0, np.cos(pitch)]])R_yaw = np.array([[np.cos(yaw), -np.sin(yaw), 0],[np.sin(yaw), np.cos(yaw), 0],[0, 0, 1]])R = R_yaw @ R_pitch @ R_roll# 相機(jī)坐標(biāo)轉(zhuǎn)換到世界坐標(biāo)cam_coords_homogeneous = np.array([cam_coords[0], cam_coords[1], -H, 1])world_coords = R @ cam_coords_homogeneous[:3] + np.array([pos_x, pos_y, pos_z])return world_coordsif __name__ == "__main__":####################################################基本參數(shù)# 傳感器寬度和高度（毫米）sensor_width_mm = 12.83331744000000007588sensor_height_mm = 8.55554496000000064271# 圖像寬度和高度（像素）image_width_px = 5472image_height_px = 3648# 焦距（毫米）focal_length_mm = 8.69244671863242679422# 焦距（米）focal_length_m = 8.69244671863242679422/1000# 相對(duì)航高（米）H=86.93#像主點(diǎn)坐標(biāo) （像素）xpoff_px=20.88973563438230485190ypoff_px=50.51977022866981315019#################################################### 1\計(jì)算空間分辨率# 傳感器尺寸轉(zhuǎn)換為米sensor_width_m = sensor_width_mm / 1000sensor_height_m = sensor_height_mm / 1000# 計(jì)算水平和垂直的 GSDGSD_x = (sensor_width_m/image_width_px) * (H / focal_length_m )GSD_y = (sensor_height_m /image_height_px) * (H / focal_length_m)# 水平和垂直方向的 GSDprint("水平方向的 GSD:", GSD_x, "米/像素")print("垂直方向的 GSD:", GSD_y, "米/像素")#################################################### 2\給定像素坐標(biāo),計(jì)算相機(jī)坐標(biāo)# 像素坐標(biāo)oripixel_x = image_width_pxoripixel_y = image_height_px# oripixel_x = image_width_px/2# oripixel_y = image_height_px/2# oripixel_x = 0# oripixel_y = 0pixel_x=oripixel_x-xpoff_px-image_width_px/2pixel_y=oripixel_y-ypoff_px-image_height_px/2# 計(jì)算相機(jī)坐標(biāo)（假設(shè)無畸變）camera_x = pixel_x * GSD_xcamera_y = pixel_y * GSD_yprint("像素坐標(biāo) (", oripixel_x, ",", oripixel_y, ") 對(duì)應(yīng)的相機(jī)坐標(biāo) (x, y): (", camera_x, "米, ", camera_y, "米)")#################################################### 3\計(jì)算畸變后坐標(biāo)# 對(duì)稱畸變系數(shù)sym_dist = [0, -0.00043396118129128110, 0.00000262222711982075, -0.00000001047488706013]# 徑向畸變dec_dist = [0.00000205885592671873, -0.00000321714140091248, 0]# 進(jìn)行畸變校正undistorted_camera_x, undistorted_camera_y = undistort_pixel(pixel_x, pixel_y, sym_dist, dec_dist)print("畸變校正后像素坐標(biāo) (", oripixel_x, ",", oripixel_y, ") 對(duì)應(yīng)的相機(jī)坐標(biāo) (x, y): (", undistorted_camera_x, "米, ", undistorted_camera_y, "米)")#################################################### 4\計(jì)算世界坐標(biāo)# POS數(shù)據(jù)pos = [433452.054688, 2881728.519704, 183.789696, 0.648220, -0.226028, 14.490357]# 計(jì)算世界坐標(biāo)world_coords = camera_to_world_coordinates((undistorted_camera_x, undistorted_camera_y), pos)print("旋轉(zhuǎn)平移變換后像素坐標(biāo) (", oripixel_x, ",", oripixel_y, ") 對(duì)應(yīng)的世界坐標(biāo) (x, y): (", world_coords[0], "米, ", world_coords[1], "米)")