diff --git a/src/AI_Drone_Face_Recognition_Human_Tracking/detection_module.py b/src/AI_Drone_Face_Recognition_Human_Tracking/detection_module.py deleted file mode 100644 index 73199c593c..0000000000 --- a/src/AI_Drone_Face_Recognition_Human_Tracking/detection_module.py +++ /dev/null @@ -1,259 +0,0 @@ -'''检测模块''' -# -*- coding: utf-8 -*- -""" -无人机检测模块 -功能: -1. 独立运行:模拟检测无人机状态、环境障碍物 -2. 被主程序导入:提供检测接口,返回预警信息 -""" -import numpy as np -import random -from enum import Enum -import time - - -# ===================== 检测类型枚举(便于主程序调用) ===================== -class DetectionType(Enum): - OBSTACLE = "障碍物检测" - BATTERY = "电量检测" - POSITION = "位置检测" - COLLISION = "碰撞预警" - STATE = "状态检测" - - -# ===================== 核心检测类 ===================== -class DroneDetection: - def __init__(self, drone=None): - """ - 初始化检测模块 - :param drone: 可选,传入虚拟无人机对象(主程序调用时传) - """ - self.drone = drone # 关联无人机对象 - self.obstacle_list = self._generate_obstacles() # 生成模拟障碍物 - self.warning_threshold = { - "battery": 20.0, # 电量预警阈值(%) - "height": 0.5, # 高度过低预警阈值(m) - "distance": 1.0 # 障碍物距离预警阈值(m) - } - - def _generate_obstacles(self): - """生成模拟环境障碍物(随机坐标)""" - obstacles = [] - for _ in range(10): # 生成10个随机障碍物 - x = random.uniform(-10, 10) - y = random.uniform(-10, 10) - z = random.uniform(0, 8) - obstacles.append(np.array([x, y, z])) - return obstacles - - def detect_obstacle(self): - """ - 障碍物检测:计算无人机与最近障碍物的距离 - :return: dict - 检测结果(距离、是否预警、最近障碍物坐标) - """ - if self.drone is None: - return { - "type": DetectionType.OBSTACLE.value, - "status": "未关联无人机", - "distance": None, - "warning": False, - "nearest_obstacle": None - } - - # 计算无人机与每个障碍物的欧氏距离 - drone_pos = self.drone.position - distances = [np.linalg.norm(drone_pos - obs) for obs in self.obstacle_list] - min_distance = min(distances) - nearest_idx = np.argmin(distances) - nearest_obs = self.obstacle_list[nearest_idx] - - # 判断是否触发预警 - warning = min_distance < self.warning_threshold["distance"] - - return { - "type": DetectionType.OBSTACLE.value, - "status": "检测完成", - "distance": round(min_distance, 2), - "warning": warning, - "nearest_obstacle": nearest_obs.round(2), - "message": f"⚠️ 距离障碍物仅{min_distance:.2f}m,请注意避让!" if warning else "✅ 无障碍物风险" - } - - def detect_battery(self): - """ - 电量检测:判断是否低于预警阈值 - :return: dict - 检测结果 - """ - if self.drone is None: - return { - "type": DetectionType.BATTERY.value, - "status": "未关联无人机", - "battery": None, - "warning": False, - "message": "未关联无人机,无法检测电量" - } - - battery = self.drone.battery - warning = battery < self.warning_threshold["battery"] - - return { - "type": DetectionType.BATTERY.value, - "status": "检测完成", - "battery": round(battery, 2), - "warning": warning, - "message": f"⚠️ 电量低({battery:.1f}%),请尽快返航!" if warning else f"✅ 电量充足({battery:.1f}%)" - } - - def detect_position(self): - """ - 位置检测:判断高度是否过低/超出边界 - :return: dict - 检测结果 - """ - if self.drone is None: - return { - "type": DetectionType.POSITION.value, - "status": "未关联无人机", - "position": None, - "warning": False, - "message": "未关联无人机,无法检测位置" - } - - pos = self.drone.position - # 兼容主程序枚举状态和独立运行字符串状态 - drone_state = self.drone.state.value if hasattr(self.drone.state, "value") else self.drone.state - height_warning = pos[2] < self.warning_threshold["height"] and drone_state == "FLYING" - boundary_warning = abs(pos[0]) > 15 or abs(pos[1]) > 15 # 水平边界±15m - - warning = height_warning or boundary_warning - messages = [] - if height_warning: - messages.append(f"⚠️ 飞行高度过低({pos[2]:.1f}m),请注意!") - if boundary_warning: - messages.append(f"⚠️ 超出安全边界(坐标:{pos[:2].round(1)}),请返航!") - if not warning: - messages.append(f"✅ 位置正常({pos.round(1)})") - - return { - "type": DetectionType.POSITION.value, - "status": "检测完成", - "position": pos.round(2), - "warning": warning, - "message": " | ".join(messages) - } - - def detect_collision(self): - """ - 碰撞预警:预测未来1秒是否有碰撞风险 - :return: dict - 检测结果 - """ - if self.drone is None: - return { - "type": DetectionType.COLLISION.value, - "status": "未关联无人机", - "risk": False, - "message": "未关联无人机,无法预测碰撞风险" - } - - # 预测1秒后无人机位置(基于当前速度) - future_pos = self.drone.position + self.drone.velocity * 1.0 - # 计算与障碍物的距离 - distances = [np.linalg.norm(future_pos - obs) for obs in self.obstacle_list] - min_future_dist = min(distances) - risk = min_future_dist < 0.5 # 距离<0.5m判定为碰撞风险 - - return { - "type": DetectionType.COLLISION.value, - "status": "检测完成", - "risk": risk, - "future_position": future_pos.round(2), - "message": "🚨 1秒后有碰撞风险!请立即调整方向!" if risk else "✅ 无碰撞风险" - } - - def detect_state(self): - """ - 状态检测:判断无人机当前状态是否正常 - :return: dict - 检测结果 - """ - if self.drone is None: - return { - "type": DetectionType.STATE.value, - "status": "未关联无人机", - "drone_state": None, - "message": "未关联无人机,无法检测状态" - } - - # 兼容主程序枚举状态(DroneState)和独立运行字符串状态 - state = self.drone.state.value if hasattr(self.drone.state, "value") else self.drone.state - if state == "LANDED": - message = "✅ 无人机处于落地状态,状态正常" - elif state == "FLYING" and self.drone.battery > 10: - message = "✅ 无人机处于飞行状态,状态正常" - else: - message = f"⚠️ 无人机飞行状态异常(电量{self.drone.battery:.1f}%)" - - return { - "type": DetectionType.STATE.value, - "status": "检测完成", - "drone_state": state, - "warning": state == "FLYING" and self.drone.battery <= 10, - "message": message - } - - def full_detection(self): - """ - 全量检测:执行所有检测项 - :return: list - 所有检测结果 - """ - results = [ - self.detect_state(), - self.detect_battery(), - self.detect_position(), - self.detect_obstacle(), - self.detect_collision() - ] - return results - - -# ===================== 独立运行测试代码 ===================== -def main(): - """模块独立运行时的测试逻辑""" - print("=" * 60) - print("📊 无人机检测模块 - 独立测试模式") - print("=" * 60) - - # 模拟无人机对象(独立运行时使用,简化状态为字符串,避免枚举类错误) - class MockDrone: - def __init__(self): - self.position = np.array([2.0, 1.5, 1.0]) # 模拟位置 - self.velocity = np.array([0.8, 0.5, 0.0]) # 模拟速度 - self.state = "FLYING" # 直接用字符串标识状态,替代枚举类 - self.battery = 18.5 # 模拟电量 - - # 初始化检测模块 - mock_drone = MockDrone() - detector = DroneDetection(drone=mock_drone) - - # 循环检测(模拟实时监测) - try: - while True: - print(f"\n⏰ 检测时间:{time.strftime('%Y-%m-%d %H:%M:%S')}") - # 执行全量检测 - all_results = detector.full_detection() - for res in all_results: - print(f"[{res['type']}] {res['message']}") - - # 模拟无人机位置变化 - mock_drone.position += mock_drone.velocity * 0.5 - mock_drone.battery -= 0.5 # 模拟电量消耗 - # 模拟飞行状态切换(每10秒落地一次) - if mock_drone.battery < 10: - mock_drone.state = "LANDED" - time.sleep(2) # 每2秒检测一次 - - except KeyboardInterrupt: - print("\n\n🛑 检测模块独立测试已停止") - - -# 仅当模块独立运行时执行测试 -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/src/AI_Drone_Face_Recognition_Human_Tracking/drone_control.py b/src/AI_Drone_Face_Recognition_Human_Tracking/drone_control.py deleted file mode 100644 index 6f01baec22..0000000000 --- a/src/AI_Drone_Face_Recognition_Human_Tracking/drone_control.py +++ /dev/null @@ -1,296 +0,0 @@ -'''无人机控制模块''' -import pyvista as pv -import pygame -import numpy as np -import sys -from enum import Enum -import warnings - -# 忽略PyVista版本兼容警告(提升运行体验) -warnings.filterwarnings("ignore") - - -# ===================== 无人机状态枚举 ===================== -class DroneState(Enum): - LANDED = "Landed" - FLYING = "Flying" - - -# ===================== 虚拟无人机类 ===================== -class VirtualDrone: - def __init__(self): - # 无人机物理参数 - self.position = np.array([0.0, 0.0, 0.0]) # 三维坐标 (x, y, z) - self.velocity = np.array([0.0, 0.0, 0.0]) # 速度 (x, y, z) - self.yaw = 0.0 # 偏航角(绕z轴旋转,°) - self.speed = 0.5 # 飞行速度(m/s) - self.state = DroneState.LANDED # 初始状态:落地 - self.battery = 100.0 # 电量(%) - self.max_height = 10.0 # 最大飞行高度(m) - self.min_height = 0.0 # 最小高度(m) - - def get_battery(self): - """新增:获取当前电量(适配主程序调用)""" - return self.battery - - def takeoff(self): - """起飞(仅落地状态可执行)""" - if self.state == DroneState.LANDED and self.battery > 20: - self.state = DroneState.FLYING - self.position[2] = 1.0 # 起飞到1m高度 - self.battery -= 0.5 # 起飞消耗电量 - print(f"✅ 虚拟无人机起飞 | 当前高度: {self.position[2]:.1f}m") - elif self.battery <= 20: - print("⚠️ 电量不足20%,禁止起飞") - else: - print("⚠️ 无人机已处于飞行状态") - - def land(self): - """降落(仅飞行状态可执行)""" - if self.state == DroneState.FLYING: - self.state = DroneState.LANDED - self.position[2] = 0.0 # 落地 - self.velocity = np.zeros(3) # 速度清零 - self.battery -= 0.2 # 降落消耗电量 - print(f"✅ 虚拟无人机降落 | 最终位置: {self.position[:2]}") - else: - print("⚠️ 无人机已处于落地状态") - - def move(self, direction): - """ - 控制无人机移动 - :param direction: 移动方向(forward/back/left/right/up/down) - """ - if self.state != DroneState.FLYING: - print("⚠️ 无人机未起飞,无法移动") - return - if self.battery <= 0: - print("⚠️ 电量耗尽,无法移动") - return - - # 基于偏航角计算实际移动方向(考虑朝向) - rad_yaw = np.radians(self.yaw) - cos_yaw = np.cos(rad_yaw) - sin_yaw = np.sin(rad_yaw) - - # 重置速度 - self.velocity = np.zeros(3) - - # 方向映射(x: 前后, y: 左右, z: 上下) - if direction == "forward": - self.velocity[0] = self.speed * cos_yaw - self.velocity[1] = self.speed * sin_yaw - elif direction == "back": - self.velocity[0] = -self.speed * cos_yaw - self.velocity[1] = -self.speed * sin_yaw - elif direction == "left": - self.velocity[0] = self.speed * sin_yaw - self.velocity[1] = -self.speed * cos_yaw - elif direction == "right": - self.velocity[0] = -self.speed * sin_yaw - self.velocity[1] = self.speed * cos_yaw - elif direction == "up": - self.velocity[2] = self.speed - elif direction == "down": - self.velocity[2] = -self.speed - - # 更新位置 - new_pos = self.position + self.velocity - # 高度限制 - new_pos[2] = np.clip(new_pos[2], self.min_height, self.max_height) - self.position = new_pos - - # 消耗电量 - self.battery -= 0.1 - self.battery = max(self.battery, 0.0) - - print(f"🔹 移动 {direction} | 位置: {self.position.round(1)} | 电量: {self.battery:.1f}%") - - def rotate(self, direction): - """ - 旋转无人机(偏航角) - :param direction: left/right - """ - if self.state != DroneState.FLYING: - print("⚠️ 无人机未起飞,无法旋转") - return - if direction == "left": - self.yaw += 10.0 # 左转10° - elif direction == "right": - self.yaw -= 10.0 # 右转10° - self.yaw %= 360 # 限制在0-360° - print(f"🔄 旋转 {direction} | 偏航角: {self.yaw:.0f}°") - - # 可选:补充主程序可能用到的扩展方法 - def get_position(self): - """获取当前位置(方便主程序调用)""" - return self.position - - def get_state(self): - """获取当前状态(返回字符串,适配主程序)""" - return self.state.value - - -# ===================== 3D可视化+交互控制 ===================== -class DroneSimulator: - def __init__(self): - # 初始化虚拟无人机 - self.drone = VirtualDrone() - # 初始化Pygame(键盘交互) - pygame.init() - # PyCharm适配:禁用Pygame音频(避免无音频设备报错) - pygame.mixer.quit() - self.screen = pygame.display.set_mode((400, 200)) - pygame.display.set_caption("虚拟无人机控制面板") - self.clock = pygame.time.Clock() - - # 初始化PyVista(3D可视化) - self.plotter = pv.Plotter(window_size=(800, 600)) - self.plotter.set_background("lightgray") - # 创建无人机3D模型 - self._create_drone_model() - # 添加地面网格 - self._add_ground_plane() - - def _create_drone_model(self): - """创建简化的无人机3D模型(适配PyCharm+全版本PyVista)""" - # 机身(立方体) - body = pv.Cube(center=(0, 0, 0), x_length=0.5, y_length=0.5, z_length=0.2) - # 螺旋桨(4个圆柱体) - prop1 = pv.Cylinder(center=(0.3, 0.3, 0.1), direction=(1, 0, 0), radius=0.2, height=0.05) - prop2 = pv.Cylinder(center=(-0.3, 0.3, 0.1), direction=(-1, 0, 0), radius=0.2, height=0.05) - prop3 = pv.Cylinder(center=(0.3, -0.3, 0.1), direction=(0, 1, 0), radius=0.2, height=0.05) - prop4 = pv.Cylinder(center=(-0.3, -0.3, 0.1), direction=(0, -1, 0), radius=0.2, height=0.05) - props = pv.MultiBlock([prop1, prop2, prop3, prop4]) - - # 组合无人机模型:适配所有PyVista版本的颜色设置 - self.drone_actor = self.plotter.add_mesh(body, color="darkblue") # 机身颜色 - self.props_actor = self.plotter.add_mesh(props, color="gray") # 螺旋桨颜色 - - # 初始化状态标签(保存actor引用,用于后续删除) - self.label_actor = self.plotter.add_text( - f"Position: (0.0, 0.0, 0.0) | State: Landed | Battery: 100%", - position="upper_left", - font_size=12, - color="black" - ) - - def _add_ground_plane(self): - """添加地面网格""" - ground = pv.Plane(center=(0, 0, 0), direction=(0, 0, 1), i_size=50, j_size=50) - ground.rotate_z(45) # 旋转45°,网格更明显 - self.plotter.add_mesh(ground, color="lightgreen", opacity=0.5) - # 添加坐标轴(PyCharm中更清晰) - self.plotter.add_axes(line_width=2, labels_off=False) - - def _update_3d_view(self): - """更新3D视图(完全适配PyCharm+旧版PyVista)""" - try: - # 更新无人机位置 - self.drone_actor.SetPosition(self.drone.position) - self.props_actor.SetPosition(self.drone.position) - - # 更新无人机旋转(偏航角) - self.drone_actor.RotateZ(self.drone.yaw) - self.props_actor.RotateZ(self.drone.yaw) - - # 更新状态标签:先删旧标签,再加新标签(兼容所有版本) - self.plotter.remove_actor(self.label_actor) - new_label_text = ( - f"Position: {self.drone.position.round(1)} | " - f"State: {self.drone.state.value} | " - f"Battery: {self.drone.battery:.1f}%" - ) - self.label_actor = self.plotter.add_text( - new_label_text, - position="upper_left", - font_size=12, - color="black" - ) - - # 强制刷新视图 - self.plotter.render() - except Exception as e: - print(f"视图更新小警告(不影响使用):{str(e)}") - - def _handle_keyboard(self): - """处理键盘输入(PyCharm焦点适配)""" - for event in pygame.event.get(): - if event.type == pygame.QUIT: - self.cleanup() - sys.exit() - if event.type == pygame.KEYDOWN: - # 基础控制 - if event.key == pygame.K_t: - self.drone.takeoff() - elif event.key == pygame.K_l: - self.drone.land() - # 移动控制 - elif event.key == pygame.K_w: - self.drone.move("forward") - elif event.key == pygame.K_s: - self.drone.move("back") - elif event.key == pygame.K_a: - self.drone.move("left") - elif event.key == pygame.K_d: - self.drone.move("right") - elif event.key == pygame.K_UP: - self.drone.move("up") - elif event.key == pygame.K_DOWN: - self.drone.move("down") - # 旋转控制 - elif event.key == pygame.K_q: - self.drone.rotate("left") - elif event.key == pygame.K_e: - self.drone.rotate("right") - # 退出(PyCharm中优雅退出) - elif event.key == pygame.K_ESCAPE: - self.cleanup() - sys.exit() - - def cleanup(self): - """PyCharm优雅退出(释放资源)""" - pygame.quit() - self.plotter.close() - print("\n👋 模拟器已优雅退出") - - def run(self): - """运行模拟器(PyCharm专用优化)""" - print("=" * 60) - print("🎮 PyCharm 2025.2.3 虚拟无人机模拟器") - print("=" * 60) - print("操作说明:") - print(" T → 起飞 | L → 降落 | ESC → 退出") - print(" W/S/A/D → 前/后/左/右 | ↑/↓ → 上升/下降") - print(" Q/E → 左转/右转(偏航角)") - print("⚠️ 注意:先点击Pygame窗口获取键盘焦点") - print("=" * 60) - - # PyVista窗口显示(适配PyCharm的交互模式) - self.plotter.show(interactive_update=True, auto_close=False) - - # 主循环(PyCharm帧率优化) - while True: - self._handle_keyboard() - self._update_3d_view() - self.clock.tick(30) # 稳定30FPS,避免PyCharm卡顿 - - -# ===================== PyCharm 一键运行入口 ===================== -if __name__ == "__main__": - # 第一步:安装依赖(复制到PyCharm终端执行) - # pip install pyvista pygame numpy -i https://pypi.tuna.tsinghua.edu.cn/simple/ - - # 第二步:运行程序 - try: - simulator = DroneSimulator() - simulator.run() - except Exception as e: - print(f"\n❌ 程序运行异常:{str(e)}") - print("\n💡 解决方案:") - print("1. 确保已安装依赖:pip install pyvista pygame numpy -i 清华镜像") - print("2. 关闭PyCharm的\"Power Save Mode\"(省电模式)") - print("3. 以管理员身份运行PyCharm") - # 强制清理资源 - pygame.quit() - sys.exit(1) \ No newline at end of file diff --git a/src/AI_Drone_Face_Recognition_Human_Tracking/drone_model.py b/src/AI_Drone_Face_Recognition_Human_Tracking/drone_model.py new file mode 100644 index 0000000000..1159cd1da8 --- /dev/null +++ b/src/AI_Drone_Face_Recognition_Human_Tracking/drone_model.py @@ -0,0 +1,459 @@ +# drone_model.py - 完全干净版本 +import pygame +from pygame.locals import * +from OpenGL.GL import * +from OpenGL.GLU import * +import math +import sys +import time + + +# === 无人机类 === +class Drone: + def __init__(self): + self.x = 0.0 + self.y = 0.0 + self.z = 1.0 + self.yaw = 0.0 + self.pitch = 0.0 + self.roll = 0.0 + self.propeller_angle = 0.0 + self.is_flying = False + + self.move_forward = False + self.move_backward = False + self.move_left = False + self.move_right = False + self.rotate_left = False + self.rotate_right = False + + self.speed = 2.5 + self.rotate_speed = 150.0 + self.strafe_speed = 1.8 + + print("无人机初始化完成") + + def update(self, delta_time): + if not self.is_flying: + return + + self.propeller_angle += delta_time * 500.0 + if self.propeller_angle > 360: + self.propeller_angle -= 360 + + move_amount = self.speed * delta_time + strafe_amount = self.strafe_speed * delta_time + rotate_amount = self.rotate_speed * delta_time + yaw_rad = math.radians(self.yaw) + + if self.move_forward: + self.x += math.sin(yaw_rad) * move_amount + self.y += -math.cos(yaw_rad) * move_amount + self.pitch = -8.0 + + if self.move_backward: + self.x += -math.sin(yaw_rad) * move_amount + self.y += math.cos(yaw_rad) * move_amount + self.pitch = 8.0 + + if self.move_left: + self.x += -math.cos(yaw_rad) * strafe_amount + self.y += -math.sin(yaw_rad) * strafe_amount + self.roll = -8.0 + + if self.move_right: + self.x += math.cos(yaw_rad) * strafe_amount + self.y += math.sin(yaw_rad) * strafe_amount + self.roll = 8.0 + + if self.rotate_left: + self.yaw += rotate_amount + if self.rotate_right: + self.yaw -= rotate_amount + + self.yaw %= 360.0 + self.pitch *= 0.9 + self.roll *= 0.9 + + if self.z < 0.5: + self.z = 0.5 + + def render(self): + glPushMatrix() + glTranslatef(self.x, self.y, self.z) + glRotatef(self.yaw, 0, 0, 1) + glRotatef(self.pitch, 1, 0, 0) + glRotatef(self.roll, 0, 1, 0) + + glColor3f(0.3, 0.3, 0.3) + self._draw_cube(0.2, 0.15, 0.1) + + arm_positions = [ + (0.25, 0.25, 0), + (-0.25, 0.25, 0), + (0.25, -0.25, 0), + (-0.25, -0.25, 0) + ] + + for i, (ax, ay, az) in enumerate(arm_positions): + glPushMatrix() + glTranslatef(ax, ay, az) + glRotatef(45, 0, 0, 1) + glColor3f(0.4, 0.4, 0.4) + self._draw_cylinder(0.015, 0.18) + glPopMatrix() + + glPushMatrix() + glTranslatef(ax, ay, 0.08) + glColor3f(0.6, 0.6, 0.6) + self._draw_cylinder(0.025, 0.05) + glPopMatrix() + + glPushMatrix() + glTranslatef(ax, ay, 0.13) + glColor3f(0.9, 0.1, 0.1) + glRotatef(self.propeller_angle + i * 90, 0, 0, 1) + self._draw_propeller(0.1) + glPopMatrix() + + glPushMatrix() + glTranslatef(0, 0, 0.08) + if self.is_flying: + blink = int(time.time() * 5) % 2 == 0 + glColor3f(0.0, 1.0 if blink else 0.5, 0.0) + else: + glColor3f(1.0, 0.0, 0.0) + self._draw_sphere(0.02) + glPopMatrix() + + glPushMatrix() + glTranslatef(0, 0.2, 0.05) + glColor3f(1.0, 1.0, 0.0) + self._draw_sphere(0.018) + glPopMatrix() + + glDisable(GL_LIGHTING) + glColor3f(1.0, 1.0, 0.0) + glLineWidth(3.0) + + glBegin(GL_LINES) + glVertex3f(0, 0.1, 0.08) + glVertex3f(0, 0.3, 0.08) + glVertex3f(0, 0.3, 0.08) + glVertex3f(-0.05, 0.22, 0.08) + glVertex3f(0, 0.3, 0.08) + glVertex3f(0.05, 0.22, 0.08) + glVertex3f(-0.05, 0.22, 0.08) + glVertex3f(0.05, 0.22, 0.08) + glEnd() + + glEnable(GL_LIGHTING) + glPopMatrix() + + def _draw_cube(self, width, height, depth): + w, h, d = width / 2, height / 2, depth / 2 + vertices = [ + [-w, -h, -d], [w, -h, -d], [w, h, -d], [-w, h, -d], + [-w, -h, d], [w, -h, d], [w, h, d], [-w, h, d] + ] + faces = [ + [0, 1, 2, 3], [4, 5, 6, 7], + [0, 3, 7, 4], [1, 2, 6, 5], + [0, 1, 5, 4], [2, 3, 7, 6] + ] + + glBegin(GL_QUADS) + for face in faces: + for vertex in face: + glVertex3fv(vertices[vertex]) + glEnd() + + def _draw_cylinder(self, radius, height): + quadric = gluNewQuadric() + gluCylinder(quadric, radius, radius, height, 16, 1) + + glPushMatrix() + glTranslatef(0, 0, height) + gluDisk(quadric, 0, radius, 16, 1) + glPopMatrix() + + glPushMatrix() + glRotatef(180, 1, 0, 0) + gluDisk(quadric, 0, radius, 16, 1) + glPopMatrix() + + def _draw_sphere(self, radius): + quadric = gluNewQuadric() + gluSphere(quadric, radius, 16, 16) + + def _draw_propeller(self, radius): + glBegin(GL_TRIANGLES) + for i in range(4): + angle = i * math.pi / 2 + glVertex3f(0, 0, 0.01) + glVertex3f(radius * math.cos(angle), radius * math.sin(angle), 0.01) + glVertex3f(radius * 0.7 * math.cos(angle + 0.3), radius * 0.7 * math.sin(angle + 0.3), 0.01) + glEnd() + + def reset(self): + self.x = 0.0 + self.y = 0.0 + self.z = 1.0 + self.yaw = 0.0 + self.pitch = 0.0 + self.roll = 0.0 + print("无人机位置已重置") + + +# === 无人机查看器类 === +class DroneViewer: + def __init__(self, width=800, height=600): + pygame.init() + self.width = width + self.height = height + + try: + self.screen = pygame.display.set_mode((width, height), DOUBLEBUF | OPENGL) + except pygame.error as e: + print(f"无法创建OpenGL窗口: {e}") + print("尝试使用标准窗口模式...") + self.screen = pygame.display.set_mode((width, height)) + + pygame.display.set_caption("3D无人机查看器") + + glEnable(GL_DEPTH_TEST) + glEnable(GL_LIGHTING) + glEnable(GL_LIGHT0) + glEnable(GL_COLOR_MATERIAL) + + glLightfv(GL_LIGHT0, GL_POSITION, [5.0, 5.0, 5.0, 1.0]) + glLightfv(GL_LIGHT0, GL_DIFFUSE, [1.0, 1.0, 1.0, 1.0]) + glLightfv(GL_LIGHT0, GL_AMBIENT, [0.2, 0.2, 0.2, 1.0]) + + self.drone = Drone() + self.camera_distance = 8.0 + self.camera_angle_x = 30.0 + self.camera_angle_y = -45.0 + self.mouse_dragging = False + self.last_mouse_x = 0 + self.last_mouse_y = 0 + self.running = True + self.path = [] + self.max_path_length = 200 + + print("=" * 50) + print("3D无人机查看器已启动") + print("控制说明:") + print(" 空格键: 起飞/降落") + print(" W键: 前进 (朝黄色箭头方向)") # W显示为前进 + print(" S键: 后退") # S显示为后退 + print(" A键: 向左平移") + print(" D键: 向右平移") + print(" Q键: 左转") + print(" E键: 右转") + print(" R键: 重置位置") + print(" ESC键: 退出") + print(" 鼠标左键拖拽: 旋转视角") + print(" 鼠标滚轮: 缩放") + print("=" * 50) + + def handle_events(self): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + self.running = False + + elif event.type == pygame.KEYDOWN: + if event.key == pygame.K_ESCAPE: + self.running = False + elif event.key == pygame.K_SPACE: + self.drone.is_flying = not self.drone.is_flying + status = "起飞" if self.drone.is_flying else "降落" + print(f"无人机: {status}") + elif event.key == pygame.K_r: + self.drone.reset() + self.path = [] + print("位置和路径已重置") + elif event.key == pygame.K_s: # S键 - 物理逻辑向前 + self.drone.move_forward = True + print("S键按下: 后退") # 只改打印信息:显示W键前进 + elif event.key == pygame.K_w: # W键 - 物理逻辑向后 + self.drone.move_backward = True + print("W键按下: 前进") # 只改打印信息:显示S键后退 + elif event.key == pygame.K_a: + self.drone.move_left = True + print("A键按下: 向左平移") + elif event.key == pygame.K_d: + self.drone.move_right = True + print("D键按下: 向右平移") + elif event.key == pygame.K_q: + self.drone.rotate_left = True + print("Q键按下: 左转") + elif event.key == pygame.K_e: + self.drone.rotate_right = True + print("E键按下: 右转") + + elif event.type == pygame.KEYUP: + if event.key == pygame.K_s: # S键释放 + self.drone.move_forward = False + elif event.key == pygame.K_w: # W键释放 + self.drone.move_backward = False + elif event.key == pygame.K_a: + self.drone.move_left = False + elif event.key == pygame.K_d: + self.drone.move_right = False + elif event.key == pygame.K_q: + self.drone.rotate_left = False + elif event.key == pygame.K_e: + self.drone.rotate_right = False + + elif event.type == pygame.MOUSEBUTTONDOWN: + if event.button == 1: + self.mouse_dragging = True + self.last_mouse_x, self.last_mouse_y = event.pos + elif event.button == 4: + self.camera_distance = max(3.0, self.camera_distance - 0.5) + elif event.button == 5: + self.camera_distance = min(20.0, self.camera_distance + 0.5) + + elif event.type == pygame.MOUSEBUTTONUP: + if event.button == 1: + self.mouse_dragging = False + + elif event.type == pygame.MOUSEMOTION: + if self.mouse_dragging: + x, y = event.pos + dx = x - self.last_mouse_x + dy = y - self.last_mouse_y + + self.camera_angle_y += dx * 0.5 + self.camera_angle_x -= dy * 0.5 + self.camera_angle_x = max(-85.0, min(85.0, self.camera_angle_x)) + + self.last_mouse_x, self.last_mouse_y = x, y + + def update(self, delta_time): + self.drone.update(delta_time) + + if self.drone.is_flying: + self.path.append((self.drone.x, self.drone.y, self.drone.z)) + if len(self.path) > self.max_path_length: + self.path.pop(0) + + def _draw_grid(self): + glDisable(GL_LIGHTING) + glColor3f(0.4, 0.4, 0.4) + + glBegin(GL_LINES) + for i in range(-15, 16): + glVertex3f(i, -15, 0) + glVertex3f(i, 15, 0) + glVertex3f(-15, i, 0) + glVertex3f(15, i, 0) + glEnd() + + glEnable(GL_LIGHTING) + + def _draw_path(self): + if len(self.path) > 1: + glDisable(GL_LIGHTING) + glColor3f(1.0, 0.5, 0.0) + glLineWidth(2.0) + + glBegin(GL_LINE_STRIP) + for point in self.path: + glVertex3f(point[0], point[1], point[2] + 0.05) + glEnd() + + glEnable(GL_LIGHTING) + + def _draw_axes(self): + glDisable(GL_LIGHTING) + glLineWidth(2.0) + + glBegin(GL_LINES) + glColor3f(1.0, 0.0, 0.0) + glVertex3f(0, 0, 0) + glVertex3f(3, 0, 0) + + glColor3f(0.0, 1.0, 0.0) + glVertex3f(0, 0, 0) + glVertex3f(0, 3, 0) + + glColor3f(0.0, 0.0, 1.0) + glVertex3f(0, 0, 0) + glVertex3f(0, 0, 3) + glEnd() + + glEnable(GL_LIGHTING) + + def render(self): + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) + glClearColor(0.08, 0.12, 0.18, 1.0) + + glMatrixMode(GL_PROJECTION) + glLoadIdentity() + gluPerspective(45, self.width / self.height, 0.1, 100.0) + + glMatrixMode(GL_MODELVIEW) + glLoadIdentity() + + cam_x = self.camera_distance * math.sin(math.radians(self.camera_angle_y)) * math.cos( + math.radians(self.camera_angle_x)) + cam_y = self.camera_distance * math.cos(math.radians(self.camera_angle_y)) * math.cos( + math.radians(self.camera_angle_x)) + cam_z = self.camera_distance * math.sin(math.radians(self.camera_angle_x)) + + gluLookAt(cam_x, cam_y, cam_z, 0, 0, 0, 0, 0, 1) + + self._draw_grid() + self._draw_axes() + self._draw_path() + self.drone.render() + + pygame.display.flip() + + def run(self): + clock = pygame.time.Clock() + last_time = pygame.time.get_ticks() + + print("\n操作流程:") + print("1. 按空格键起飞") + print("2. 按W键前进 (朝黄色箭头方向)") # W显示为前进 + print("3. 按S键后退") # S显示为后退 + print("4. 按A/D键左右平移") + print("5. 按Q/E键旋转") + print("6. 鼠标拖拽旋转视角") + print("7. 滚轮缩放\n") + + try: + while self.running: + current_time = pygame.time.get_ticks() + delta_time = (current_time - last_time) / 1000.0 + last_time = current_time + + self.handle_events() + self.update(delta_time) + self.render() + + clock.tick(60) + except KeyboardInterrupt: + print("\n用户中断程序") + except Exception as e: + print(f"运行时错误: {e}") + import traceback + traceback.print_exc() + finally: + pygame.quit() + print("程序已退出") + + +# === 主程序 === +def main(): + print("启动3D无人机查看器...") + + viewer = DroneViewer() + viewer.run() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/AI_Drone_Face_Recognition_Human_Tracking/face_database.py b/src/AI_Drone_Face_Recognition_Human_Tracking/face_database.py index 51ff11fe20..da3360e506 100644 --- a/src/AI_Drone_Face_Recognition_Human_Tracking/face_database.py +++ b/src/AI_Drone_Face_Recognition_Human_Tracking/face_database.py @@ -333,4 +333,3 @@ def draw_detection_box(frame: np.ndarray, # 释放资源 cap.release() - cv2.destroyAllWindows() \ No newline at end of file diff --git a/src/AI_Drone_Face_Recognition_Human_Tracking/map_overlay.py b/src/AI_Drone_Face_Recognition_Human_Tracking/map_overlay.py deleted file mode 100644 index 37bdfc34f9..0000000000 --- a/src/AI_Drone_Face_Recognition_Human_Tracking/map_overlay.py +++ /dev/null @@ -1,150 +0,0 @@ -'''地图叠加模块''' -import cv2 -import numpy as np - - -class MapOverlay: - def __init__(self, map_path="map.png", alpha=0.3): - self.map_path = map_path - self.alpha = np.clip(alpha, 0.0, 1.0) - self.map_img = self.load_map() - - def load_map(self): - """加载地图,失败则生成默认地图""" - try: - map_img = cv2.imread(self.map_path) - if map_img is None: - raise ValueError("地图图片读取失败") - return map_img - except Exception as e: - print(f"加载地图失败:{e},使用默认地图") - # 生成默认地图(带网格的简易地图) - default_map = np.ones((200, 300, 3), dtype=np.uint8) * 240 - # 绘制网格 - for x in range(0, 300, 30): - cv2.line(default_map, (x, 0), (x, 200), (200, 200, 200), 1) - for y in range(0, 200, 20): - cv2.line(default_map, (0, y), (300, y), (200, 200, 200), 1) - cv2.putText(default_map, "DEFAULT MAP", (20, 100), - cv2.FONT_HERSHEY_SIMPLEX, 0.8, (50, 50, 50), 2) - return default_map - - def overlay(self, frame): - """将地图叠加到帧的右上角""" - if frame is None or self.map_img is None: - return frame - - h, w = frame.shape[:2] - map_h, map_w = self.map_img.shape[:2] - - # 缩放地图(占视频宽度的25%) - target_w = int(w * 0.25) - scale = target_w / map_w - target_h = int(map_h * scale) - resized_map = cv2.resize(self.map_img, (target_w, target_h)) - - # 计算叠加位置(右上角,留10像素边距) - x_start = w - target_w - 10 - y_start = 10 - x_end = x_start + target_w - y_end = y_start + target_h - - # 边界校验(防止地图超出视频帧范围) - x_end = min(x_end, w) - y_end = min(y_end, h) - resized_map = resized_map[:y_end - y_start, :x_end - x_start] - - # 透明度混合 - roi = frame[y_start:y_end, x_start:x_end] - blended = cv2.addWeighted(resized_map, self.alpha, roi, 1 - self.alpha, 0) - frame[y_start:y_end, x_start:x_end] = blended - - # 绘制无人机位置标记(地图中心) - map_center_x = x_start + (x_end - x_start) // 2 - map_center_y = y_start + (y_end - y_start) // 2 - cv2.circle(frame, (map_center_x, map_center_y), 5, (0, 0, 255), -1) - cv2.putText(frame, "Drone", (map_center_x + 10, map_center_y), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1) - - return frame - - def adjust_alpha(self, delta): - """调整地图透明度""" - old_alpha = self.alpha - self.alpha = np.clip(self.alpha + delta, 0.0, 1.0) - if old_alpha != self.alpha: # 仅在透明度变化时打印 - print(f"✅ 地图透明度调整为:{self.alpha:.1f}") - else: - print(f"⚠️ 透明度已达极限({self.alpha:.1f}),无法继续调整") - - -def main(): - # 初始化地图叠加器 - map_overlay = MapOverlay(alpha=0.4) - - # 选择视频源:0=摄像头,也可以替换为视频文件路径(如"test.mp4") - video_source = 0 - cap = cv2.VideoCapture(video_source) - - # 检查视频源是否打开 - if not cap.isOpened(): - print(f"❌ 无法打开视频源:{video_source}") - return - - # 设置窗口可调整大小(方便操作) - cv2.namedWindow("Map Overlay Demo", cv2.WINDOW_NORMAL) - - print("=" * 60) - print("📢 操作说明:") - print(" W / 上方向键:增加地图透明度(+0.1)") - print(" S / 下方向键:降低地图透明度(-0.1)") - print(" Q / ESC键 :退出程序") - print("=" * 60) - - # 主循环 - while True: - # 读取视频帧 - ret, frame = cap.read() - if not ret: - print("❌ 视频流已结束或读取失败") - break - - # 叠加地图 - frame_with_map = map_overlay.overlay(frame) - - # 显示操作提示和当前透明度 - cv2.putText(frame_with_map, "W/S: Adjust Alpha | Q/ESC: Quit", (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) - cv2.putText(frame_with_map, f"Alpha: {map_overlay.alpha:.1f}", (10, 70), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 0), 2) - - # 显示结果 - cv2.imshow("Map Overlay Demo", frame_with_map) - - # 键盘交互(优化跨平台按键检测,等待时间调整为30ms提高响应性) - key = cv2.waitKey(30) & 0xFF - - # 退出逻辑(Q键 / ESC键) - if key == ord('q') or key == ord('Q') or key == 27: - print("📤 退出程序") - break - - # 增加透明度(W键 / 上方向键) - elif key == ord('w') or key == ord('W') or key == 82 or key == 104: - map_overlay.adjust_alpha(0.1) - - # 降低透明度(S键 / 下方向键) - elif key == ord('s') or key == ord('S') or key == 84 or key == 101: - map_overlay.adjust_alpha(-0.1) - - # 调试:打印未知按键编码(方便排查问题) - elif key != 255: - print(f"🔍 检测到未映射按键,编码:{key}") - - # 释放资源 - cap.release() - cv2.destroyAllWindows() - - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/src/AI_Drone_Face_Recognition_Human_Tracking/utils.py b/src/AI_Drone_Face_Recognition_Human_Tracking/utils.py deleted file mode 100644 index 1dca43f414..0000000000 --- a/src/AI_Drone_Face_Recognition_Human_Tracking/utils.py +++ /dev/null @@ -1,34 +0,0 @@ -'''工具模块''' -import cv2 -import numpy as np - -def preprocess_face(face_img): - """人脸预处理:灰度化+均衡化+归一化""" - # 灰度化 - if len(face_img.shape) == 3: - face_gray = cv2.cvtColor(face_img, cv2.COLOR_BGR2GRAY) - else: - face_gray = face_img - # 直方图均衡化 - face_eq = cv2.equalizeHist(face_gray) - # 归一化尺寸 - face_resized = cv2.resize(face_eq, (100, 100)) - # 归一化像素值 - face_normalized = face_resized / 255.0 - return face_normalized - -def calculate_similarity(face1, face2): - """计算人脸相似度(余弦相似度)""" - try: - # 展平为一维向量 - vec1 = face1.flatten() - vec2 = face2.flatten() - # 余弦相似度 - dot = np.dot(vec1, vec2) - norm1 = np.linalg.norm(vec1) - norm2 = np.linalg.norm(vec2) - sim = dot / (norm1 * norm2) if (norm1 * norm2) != 0 else 0.0 - return sim - except Exception as e: - print(f"计算相似度失败:{e}") - return 0.0 \ No newline at end of file