【第2篇-3D到2D的坐标转换和投影】airsim+UE 目的是像air2land一样在虚拟环境建立一个自己的仿真数据集方便获取关键点和一些pose情况下的标注数据在半自动化仿真系统里去获取数据air2land这类获取姿态的数据集。零、最终效果First下一个目标是标注四旋翼的四个关键点相对于mesh的刚体坐标系下的四旋翼的点然后投影到相机坐标系获取Keypoints的内容。这一部分实现了3D→2D的坐标映射Airsim是可以直接获取到Air3的静态mesh的3D世界的坐标的在UE里面StaticMesh是一个绝对位置的。其中Air3.fbx文件来自网络down的网盘链接为通过网盘分享的文件DJI Air 3 模型 fbx.zip链接: https://pan.baidu.com/s/1cFF_BUrrdBkBg3J0n1z6MQ?pwdjukq 提取码: jukq--来自百度网盘超级会员v4的分享一、配置文件settings.json位置C:\Users\xxx\Documents\AirSim\settings.jsonSimMode, ApiServerPort要加相机默认的分辨率为256*144更改相机默认的分辨率长宽和视场角{ SettingsVersion: 1.2, SimMode: Multirotor, ClockType: SteppableClock, ApiServerPort: 41451, CameraDefaults: { CaptureSettings: [ { ImageType: 0, Width: 1024, Height: 1024, FOV_Degrees: 90, AutoExposureSpeed: 100, AutoExposureBias: 0, AutoExposureMaxBrightness: 0.64, AutoExposureMinBrightness: 0.03, MotionBlurAmount: 0, TargetGamma: 1.0 } ] }, Vehicles: { SimpleFlight: { VehicleType: SimpleFlight, DefaultVehicleState: Armed } } }二、相机内参的计算通过视场角来计算内参fx f / dx, 焦距是多少个像元----------------主点的含义fx,fy------等效焦距的像素个数代码先放下面# 1. python airsim 从源码安装的 pip install -e . # 遇到问题cursor解决SSL有问题需要跳过有netutil.py里面跳过安装成功的 # usage: C:\Users\liuqi\miniconda3\envs\simple_base\python.exe e:/3D-software/zhr_code/airsim/zhr_pys/airsim_get_objects.py zhr_pys/airsim_get_allObjs.log # 设置玩家出生点为0,0,0 就可以得到*.log的效果现在要明确xyz的放心问题 # C:\Users\liuqi\Documents\AirSim\settings.json import airsim # 导入 AirSim Python API simple_base conda环境中 import numpy as np import cv2 def intrinsics_from_fov(W, H, fov_deg): fx (W / 2.0) / np.tan(np.deg2rad(fov_deg / 2.0)) fy fx # AirSim 用同一 FOV宽高比由 proj_mat 里的 multiplier 处理 cx W / 2.0 cy H / 2.0 K np.array([ [fx, 0, cx], [ 0, fy, cy], [ 0, 0, 1] ]) return K client airsim.MultirotorClient() # 有载具模式 ComputerVision纯视觉模式 client.confirmConnection() # 确认与 AirSim 的连接 all_objects client.simListSceneObjects() # 获取场景中所有对象的名称列表 print(all_objects) # 打印所有对象的名称列表 cylinders client.simListSceneObjects(Cylinder.*) # 获取场景中所有对象的名称列表过滤出包含 Cylinder 的对象 print(cylinders) # 打印所有对象的名称列表过滤出包含 Cylinder 的对象 for name in cylinders: pose client.simGetObjectPose(name) if not pose.containsNan(): p pose.position print(f{name}: ({p.x_val}, {p.y_val}, {p.z_val})) # 这个是OK的对应起来的。 org_ball client.simGetObjectPose(OrangeBall) # 获取名为 OrangeBall 的对象的位姿 if not org_ball.containsNan(): p org_ball.position print(fOrangeBall: ({p.x_val}, {p.y_val}, {p.z_val})) # 100*100*100cm 1m*1m*1m 但放大倍率为10,105 # 真实长宽高为长10m宽10m高5m 然后知道中心点的位置x,y,z旋转角度为0 cube_148 client.simGetObjectPose(TemplateCube_Rounded_148) # 获取名为 Cube_148 的对象的位姿 if not cube_148.containsNan(): p cube_148.position print(fCube_148: ({p.x_val}, {p.y_val}, {p.z_val})) # 需要知道TemplateCube_Rounded_148的长宽高尺寸 # 相机为0,0,0的时候从图像中一个像素点来获取位置然后换算为3D的坐标 camera_info client.simGetCameraInfo(0) # 获取相机信息 print(camera_info) # 打印相机信息 print(f相机位置: {camera_info.pose.position}) response client.simGetImages([airsim.ImageRequest(0, airsim.ImageType.Scene, False, False)]) # 获取相机图像 W response[0].width H response[0].height print(f图像大小: {W} x {H}) img1d np.frombuffer(response[0].image_data_uint8, dtypenp.uint8) img_rgb img1d.reshape(H, W, 3) # 图像另存为 # cv2.imwrite(airsim_image.png, cv2.cvtColor(img_rgb, cv2.COLOR_RGB2BGR)) cv2.imwrite(airsim_image.png, img_rgb) project_matrix camera_info.proj_mat # 获取相机投影矩阵 print(f投影矩阵:\n {project_matrix.matrix}) P np.array(project_matrix.matrix).reshape(4, 4) # 转换为4x4矩阵 fov camera_info.fov # 获取相机的视场角 print(f视场角: {fov} 弧度) K intrinsics_from_fov(W, H, fov) print(f******10) print(f视场角计算的内参矩阵: \n{K}) def intrinsics_from_proj_mat(P, W, H): fx abs(P[0, 0]) * W / 2.0 fy abs(P[1, 1]) * H / 2.0 cx W / 2.0 cy H / 2.0 K np.array([ [fx, 0, cx], [ 0, fy, cy], [ 0, 0, 1] ]) return K # K2 intrinsics_from_proj_mat(P, W, H) # print(f投影矩阵计算的内参矩阵: \n{K2}) # 先从世界坐标系→相机坐标系→图像坐标系→像素坐标系 # K [fx 0 cx; 0 fy cy; 0 0 1]fxfyfocal_lengthcxwidth/2cyheight/2 T_cw np.array([camera_info.pose.position.y_val, camera_info.pose.position.z_val, camera_info.pose.position.x_val]) # 相机原点在世界坐标系的位置 T_wc -T_cw # 世界坐标系原点在相机坐标系的位置 # 如果使用K2来作为相机的内参的话那么我们的148方块在相机的像素坐标为 # Cube_148: (85.0, -17.5, -13.5) x,y,z 前方左侧下方 # 10,10,5 真实大小100cm*100cm*100cm-1m*m*1m-倍率为10,10,5---- 10m*10m*5m # 中心点-w/2,h/2,l/2就是左下脚的坐标那么就是左下角 pos_148_x cube_148.position.x_val - 10/2 pos_148_y cube_148.position.y_val - 10/2 pos_148_z cube_148.position.z_val - 5/2 # obj np.array([org_ball.position.x_val, org_ball.position.y_val, org_ball.position.z_val]) obj np.array([org_ball.position.y_val, org_ball.position.z_val, org_ball.position.x_val]) T_wc # 相机坐标系下的坐标 obj obj / org_ball.position.x_val # 归一化为像素坐标 Zc 前向的深度 print(fOrangeBall在世界坐标系下的坐标: {obj}) # obj np.array([pos_148_x, pos_148_y, pos_148_z]) # pos obj - Transition_cw pos obj # print(f相机原点在世界坐标系下的坐标: {Transition_cw}) pos_148_cam K pos # 计算相机坐标系下的坐标 print(fCube_148在相机坐标系下的坐标: {pos_148_cam}) # # print(fCube_148/pos_148_cam[2]在相机坐标系下的坐标: {pos_148_cam / pos_148_cam[2]}) # 归一化为像素坐标 # # print的精度为两位小数不要是科学计数法 # np.set_printoptions(precision2, suppressTrue) # print(fCube_148/pos_148_x在相机坐标系下的坐标: {pos_148_cam / org_ball.position.x_val}) # 归一化为像素坐标 fx K[0, 0] fy K[1, 1] cx K[0, 2] cy K[1, 2] u fx * org_ball.position.y_val / org_ball.position.x_val cx v fy * org_ball.position.z_val / org_ball.position.x_val cy print(fOrangeBall在相机像素坐标系下的坐标: ({u}, {v})) # ---------------------------方法二应该用四元素的思维来做--------------------- # 给相机发送一个pose角度AT值方位和俯仰要发生一点变化 position airsim.Vector3r(0, 0, 0) # 固定位置 pose airsim.Pose(position, airsim.to_quaternion(10./180.0 * np.pi, 0, 0)) # 初始姿态: pitch0, roll0, yaw0 client.simSetCameraPose(0, pose) # 设置相机位置和姿态 # 重新获取相机信息 camera_info client.simGetCameraInfo(0) # 获取相机信息 T_wc np.array([camera_info.pose.position.y_val, camera_info.pose.position.z_val, camera_info.pose.position.x_val]) # 相机原点在世界坐标系的位置 P_w np.array([org_ball.position.y_val, org_ball.position.z_val, org_ball.position.x_val]) # 世界坐标系下的点 K np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) # 相机内参矩阵 # 相机要增加AE值方位和俯仰会有角度会有Pose camera_pose camera_info.pose # 将四元数转换为旋转矩阵 from scipy.spatial.transform import Rotation as R def quaternion_to_rotation_matrix(quaternion): r R.from_quat([quaternion.x_val, quaternion.y_val, quaternion.z_val, quaternion.w_val]) return r.as_matrix() R_cw quaternion_to_rotation_matrix(camera_pose.orientation) from airsim.utils import to_eularian_angles # 直接从四元数对象获取欧拉角返回值为 (pitch, roll, yaw) pitch, roll, yaw to_eularian_angles(camera_pose.orientation) print(f相机欧拉角(from airsim): pitch{np.rad2deg(pitch):.2f}°, roll{np.rad2deg(roll):.2f}°, yaw{np.rad2deg(yaw):.2f}°) rotation R.from_euler(zyx, [yaw, pitch, roll]) rotation_matrix rotation.as_matrix() R_cw np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) # 世界坐标系到相机坐标系的旋转矩阵 print(f相机四元数: {camera_pose.orientation}) print(f相机旋转矩阵: \n{R_cw}) # 1. 定义坐标变换矩阵 P P np.array([ [0, 1, 0], # 将 Y 轴映射到 X 轴 [0, 0, 1], # 将 Z 轴映射到 Y 轴 [1, 0, 0] # 将 X 轴映射到 Z 轴 ]) # 2. 假设你有一个旧的旋转矩阵 R_old (3x3) # R_old ... # 3. 计算新的旋转矩阵 R_new # R_cw P R_cw P.T print(f转换后的相机旋转矩阵: \n{R_cw}) R_wc R_cw.T # 世界坐标系到相机坐标系的旋转矩阵 P_c R_wc P_w T_wc # 相机坐标系下的点 # Zc P_c[2] # 深度 P_N P_c / P_c[2] # 归一化为像素坐标 P_uv K P_N # 像素坐标系下的点 print(fOrangeBall在相机像素坐标系下的坐标: ({P_uv[0]}, {P_uv[1]})) # import numpy as np # from scipy.spatial.transform import Rotation as R # 1. 设置相机位姿 position airsim.Vector3r(0, 0, 0) pitch_rad 10.0 / 180.0 * np.pi # 正俯仰角 向下看 pose airsim.Pose(position, airsim.to_quaternion(pitch_rad, 0, 0)) client.simSetCameraPose(0, pose) # 2. 获取相机信息 camera_info client.simGetCameraInfo(0) camera_pose camera_info.pose # 3. 从四元数获取旋转矩阵 # 注意AirSim 返回的四元数是 (w, x, y, z) 顺序 q camera_pose.orientation r R.from_quat([q.x_val, q.y_val, q.z_val, q.w_val]) # SciPy 需要 (x, y, z, w) R_cam_to_world r.as_matrix() # 相机坐标系 - 世界坐标系 # 4. 世界坐标系下的点 P_w在 AirSim 坐标系中 P_w np.array([org_ball.position.x_val, org_ball.position.y_val, org_ball.position.z_val]) # 5. 相机在世界坐标系中的位置 T_wc np.array([camera_pose.position.x_val, camera_pose.position.y_val, camera_pose.position.z_val]) # 6. 世界坐标系 - 相机坐标系 # 点相对于相机的位置在世界坐标系中 P_rel_w P_w - T_wc # 转换到相机坐标系R_wc R_cam_to_world.T R_wc R_cam_to_world.T P_c R_wc P_rel_w # 相机坐标系下的点 # 7. 投影到像素平面 P_n P_c / P_c[0] # 归一化坐标 (注意Z 轴指向相机前方) P_uv K P_n # 像素坐标 print(f像素坐标: ({P_uv[0]}, {P_uv[1]}))三、3D-2D坐标计算3.1 手动计算正确的形式如下Zc的计算时机以及容易出错的地方3.2 转为矩阵形式obj_pose client.simGetObjectPose(name) 可以获得目标的xyz世界坐标uv_k project_world_to_pixel_k(p_w, camera_info, K)uv_p project_world_to_pixel_proj_mat(p_w, camera_info, proj_mat, width, height)最后的Pc,Pw坐标系转换的关系公式不一样容易陷入误区3.3 叠加上坐标轴的旋转3.4 叠加上相机的位姿变化——pitch和yaw变化坐标轴的旋转暂时只是在XYZ-(YZX)的一个变化前向表示了深度在转化为相机坐标系的时候给一个深度的意义的变化深度变化到第三个维度而已。法二airsim官方proj_mat投影同caputre_ir_segementation.py一致 AirSim 3D - 2D 投影支持相机位置 姿态 坐标约定AirSim API / NED: 世界系: X前(北), Y右(东), Z下 成像相机系: x右(Y_w), yZ_w, z前(X_w) 用法: C:\\Users\\liuqi\\miniconda3\\envs\\simple_base\\python.exe e:/3D-software/zhr_code/airsim/zhr_pys/airsim_project_3d_to_2d.py import os import sys import airsim import cv2 import numpy as np from scipy.spatial.transform import Rotation as R # AirSim 世界 - 成像相机系 (Y, Z, X) (右, Z, 前) P_PERM np.array([ [0.0, 1.0, 0.0], # Y [0.0, 0.0, 1.0], # Z [1.0, 0.0, 0.0], # X ], dtypenp.float64) def intrinsics_from_fov(width: int, height: int, fov_deg: float) - np.ndarray: 由 FOV 和分辨率计算 3x3 内参矩阵 K。 fx (width / 2.0) / np.tan(np.deg2rad(fov_deg / 2.0)) fy fx cx width / 2.0 cy height / 2.0 return np.array([ [fx, 0.0, cx], [0.0, fy, cy], [0.0, 0.0, 1.0], ], dtypenp.float64) def pose_to_position(pose) - np.ndarray: AirSim Pose - [X, Y, Z]。 return np.array([ pose.position.x_val, pose.position.y_val, pose.position.z_val, ], dtypenp.float64) def quaternion_to_rotation_cw(quaternion) - np.ndarray: 四元数 - 旋转矩阵 R_cw相机 body 坐标 - 世界坐标。 SciPy 四元数顺序: (x, y, z, w) q quaternion return R.from_quat([q.x_val, q.y_val, q.z_val, q.w_val]).as_matrix() def rotation_matrix_from_airsim_euler(pitch: float, roll: float, yaw: float) - np.ndarray: 与 AirSim 官方 capture_ir_segmentation.py 一致: Roll 先应用, 再 Pitch, 再 Yaw R Rz Ry Rx cp, sp np.cos(pitch), np.sin(pitch) cr, sr np.cos(roll), np.sin(roll) cy, sy np.cos(yaw), np.sin(yaw) rx np.array([[1.0, 0.0, 0.0], [0.0, cr, -sr], [0.0, sr, cr]], dtypenp.float64) ry np.array([[cp, 0.0, sp], [0.0, 1.0, 0.0], [-sp, 0.0, cp]], dtypenp.float64) rz np.array([[cy, -sy, 0.0], [sy, cy, 0.0], [0.0, 0.0, 1.0]], dtypenp.float64) return rz ry rx def world_to_camera(P_w: np.ndarray, camera_info) - np.ndarray: 世界坐标 - 成像相机坐标。 P_cam P_perm R_cw^T (P_w - t_c) t_c pose_to_position(camera_info.pose) r_cw quaternion_to_rotation_cw(camera_info.pose.orientation) r_wc r_cw.T return P_PERM r_wc (P_w - t_c) def project_world_to_pixel_k(P_w: np.ndarray, camera_info, K: np.ndarray): 方法 A: K 外参四元数 返回 (u, v) 或 None点在相机后方 p_cam world_to_camera(P_w, camera_info) depth p_cam[2] if depth 1e-6: return None fx, fy K[0, 0], K[1, 1] cx, cy K[0, 2], K[1, 2] u fx * p_cam[0] / depth cx v fy * p_cam[1] / depth cy return float(u), float(v) def project_world_to_pixel_proj_mat( P_w: np.ndarray, camera_info, proj_mat_4x4, width: int, height: int, ): 方法 B: AirSim 官方 proj_mat 投影与 capture_ir_segmentation.py 一致 返回 (u, v) 或 None pitch, roll, yaw airsim.utils.to_eularian_angles(camera_info.pose.orientation) rot rotation_matrix_from_airsim_euler(pitch, roll, yaw) cam_pos pose_to_position(camera_info.pose) p_rel (P_w - cam_pos).reshape(3, 1) p_cam rot.T p_rel p_h np.vstack([p_cam, [[1.0]]]) P np.array(proj_mat_4x4, dtypenp.float64) xyzw P p_h if abs(xyzw[3, 0]) 1e-12: return None xyzw xyzw / xyzw[3, 0] u width * (1.0 - xyzw[0, 0]) / 2.0 v height * (1.0 xyzw[1, 0]) / 2.0 return float(u), float(v) def draw_point(img: np.ndarray, uv, color(0, 255, 0), radius6, label: str ): 在 RGB 图像上画投影点。 if uv is None: return u, v int(round(uv[0])), int(round(uv[1])) h, w img.shape[:2] if not (0 u w and 0 v h): return cv2.circle(img, (u, v), radius, color, -1) if label: cv2.putText(img, label, (u 8, v - 8), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1, cv2.LINE_AA) def get_scene_image(client, camera_name: str 0): 获取 Scene 图像及尺寸。 response client.simGetImages([ airsim.ImageRequest(camera_name, airsim.ImageType.Scene, False, False) ])[0] width, height response.width, response.height img np.frombuffer(response.image_data_uint8, dtypenp.uint8).reshape(height, width, 3) return img.copy(), width, height def demo(): client airsim.MultirotorClient() client.confirmConnection() # 设置相机位姿: 位置 俯仰角正 pitch 向下看AirSim NED 约定 pitch_deg 10.0 roll_deg 0.0 yaw_deg 0.0 cam_position airsim.Vector3r(0.0, 0.0, 0.0) cam_orientation airsim.to_quaternion( np.deg2rad(pitch_deg), np.deg2rad(roll_deg), np.deg2rad(yaw_deg), ) client.simSetCameraPose(0, airsim.Pose(cam_position, cam_orientation)) camera_info client.simGetCameraInfo(0) img, width, height get_scene_image(client, 0) K intrinsics_from_fov(width, height, camera_info.fov) proj_mat np.array(camera_info.proj_mat.matrix, dtypenp.float64) pitch, roll, yaw airsim.utils.to_eularian_angles(camera_info.pose.orientation) print(f图像尺寸: {width} x {height}) print(f相机位置: {pose_to_position(camera_info.pose)}) print( f相机欧拉角(deg): pitch{np.rad2deg(pitch):.2f}, froll{np.rad2deg(roll):.2f}, yaw{np.rad2deg(yaw):.2f} ) print(f内参 K:\n{K}) targets [OrangeBall, TemplateCube_Rounded_148] for name in targets: obj_pose client.simGetObjectPose(name) if obj_pose.containsNan(): print(f[跳过] {name}: 未找到) continue p_w pose_to_position(obj_pose) uv_k project_world_to_pixel_k(p_w, camera_info, K) uv_p project_world_to_pixel_proj_mat(p_w, camera_info, proj_mat, width, height) print(f\n{name}) print(f 世界坐标: ({p_w[0]:.3f}, {p_w[1]:.3f}, {p_w[2]:.3f})) print(f 相机系 P_cam: {world_to_camera(p_w, camera_info)}) print(f 像素(K法): {uv_k}) print(f 像素(proj_mat): {uv_p}) draw_point(img, uv_k, color(0, 255, 0), labelf{name}_K) draw_point(img, uv_p, color(255, 0, 0), radius4, labelf{name}_P) out_dir os.path.dirname(os.path.abspath(__file__)) out_path os.path.join(out_dir, project_3d_to_2d.png) cv2.imwrite(out_path, img) print(f\n结果图像已保存: {out_path}) print(绿色K法, 红色proj_mat法) if __name__ __main__: demo()Connected! Client Ver:1 (Min Req: 1), Server Ver:1 (Min Req: 1) 图像尺寸: 1024 x 1024 相机位置: [ 0.45999998 0. -0.14649588] 相机欧拉角(deg): pitch10.00, roll0.00, yaw0.00 内参 K: [[512.86193602 0. 512. ] [ 0. 512.86193602 512. ] [ 0. 0. 1. ]] OrangeBall 世界坐标: (91.150, 32.100, -5.705) 相机系 P_cam: [32.09999847 10.27452452 90.27736486] 像素(K法): (694.3587494912042, 570.3691443104426) 像素(proj_mat): (694.0522703257684, 570.271050533337) TemplateCube_Rounded_148 世界坐标: (85.000, -17.500, -13.500) 相机系 P_cam: [-17.5 1.5295787 85.57445919] 像素(K法): (407.1195956633861, 521.1670190062523) 像素(proj_mat): (407.29586179807393, 521.1516162170883) 结果图像已保存: e:\3D-software\zhr_code\airsim\zhr_pys\project_3d_to_2d.png 绿色K法, 红色proj_mat法3.5 刚体坐标系-世界坐标系-相机坐标系-像素坐标系原因需要将目标的关键点投影到像素上所以需要已知机体重心坐标系的情况下去知道机体的关键点坐标系