From c9d3f89a6ab5f150a270b99c59df922a4d157fbc Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Thu, 18 Dec 2025 18:20:44 +0800 Subject: [PATCH 01/13] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BC=A0=E6=84=9F?= =?UTF-8?q?=E5=99=A8=E6=95=B0=E6=8D=AE=E9=87=87=E9=9B=86=E6=A8=A1=E5=9D=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../sensor_interface.py | 184 ++++++++++++++++++ 1 file changed, 184 insertions(+) create mode 100644 src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/sensor_interface.py diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/sensor_interface.py b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/sensor_interface.py new file mode 100644 index 0000000000..b569f67d4a --- /dev/null +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/sensor_interface.py @@ -0,0 +1,184 @@ +# sensor_interface.py +import numpy as np +import time +import threading +from queue import Queue +import json +from dataclasses import dataclass +from typing import Optional, Tuple + + +@dataclass +class PointCloud: + points: np.ndarray # (N, 3) or (N, 4) for intensity + timestamp: float + frame_id: str = "laser" + + +@dataclass +class IMUData: + accel: np.ndarray # (3,) m/s² + gyro: np.ndarray # (3,) rad/s + timestamp: float + + +@dataclass +class GNSSData: + lat: float + lon: float + alt: float + cov: np.ndarray # (3, 3) covariance + timestamp: float + + +class SensorInterface: + """多传感器数据采集与同步接口""" + + def __init__(self, config_path: str = "config/sensors.json"): + self.load_config(config_path) + self.pointcloud_queue = Queue(maxsize=10) + self.imu_queue = Queue(maxsize=50) + self.gnss_queue = Queue(maxsize=5) + self.odom_queue = Queue(maxsize=10) + self.running = False + self.sync_tolerance = 0.01 # 10ms同步容差 + + def load_config(self, config_path: str): + """加载传感器配置""" + default_config = { + "lidar": { + "type": "velodyne", + "port": 2368, + "max_range": 100.0, + "min_range": 0.5 + }, + "imu": { + "type": "xsens", + "port": "/dev/ttyUSB0", + "baudrate": 115200 + }, + "gnss": { + "type": "ublox", + "port": "/dev/ttyACM0", + "baudrate": 9600, + "use_rtk": True + } + } + self.config = default_config + + def start_capture(self): + """启动所有传感器""" + self.running = True + self.lidar_thread = threading.Thread(target=self._lidar_capture) + self.imu_thread = threading.Thread(target=self._imu_capture) + self.gnss_thread = threading.Thread(target=self._gnss_capture) + + self.lidar_thread.start() + self.imu_thread.start() + self.gnss_thread.start() + + print("所有传感器已启动") + + def stop_capture(self): + """停止传感器采集""" + self.running = False + if hasattr(self, 'lidar_thread'): + self.lidar_thread.join() + if hasattr(self, 'imu_thread'): + self.imu_thread.join() + if hasattr(self, 'gnss_thread'): + self.gnss_thread.join() + + def _lidar_capture(self): + """模拟激光雷达数据采集""" + from sensor_simulators import LidarSimulator + + sim = LidarSimulator() + while self.running: + points = sim.generate_pointcloud() + pc = PointCloud( + points=points, + timestamp=time.time() + ) + if not self.pointcloud_queue.full(): + self.pointcloud_queue.put(pc) + time.sleep(0.1) # 10Hz + + def _imu_capture(self): + """模拟IMU数据采集""" + from sensor_simulators import IMUSimulator + + sim = IMUSimulator() + while self.running: + imu_data = IMUData( + accel=sim.get_acceleration(), + gyro=sim.get_gyro(), + timestamp=time.time() + ) + if not self.imu_queue.full(): + self.imu_queue.put(imu_data) + time.sleep(0.01) # 100Hz + + def _gnss_capture(self): + """模拟GNSS数据采集""" + from sensor_simulators import GNSSTrueSimulator + + sim = GNSSTrueSimulator() + while self.running: + gnss_data = GNSSData( + lat=sim.get_latitude(), + lon=sim.get_longitude(), + alt=sim.get_altitude(), + cov=np.eye(3) * 0.1, # 模拟协方差 + timestamp=time.time() + ) + if not self.gnss_queue.full(): + self.gnss_queue.put(gnss_data) + time.sleep(0.2) # 5Hz + + def get_synchronized_data(self, timeout=1.0) -> Tuple[Optional[PointCloud], + Optional[IMUData], + Optional[GNSSData]]: + """获取时间同步的传感器数据""" + try: + # 获取最新的点云 + pc_data = None + if not self.pointcloud_queue.empty(): + pc_data = self.pointcloud_queue.get(timeout=timeout) + + # 获取同步的IMU数据 + imu_data = None + target_time = pc_data.timestamp if pc_data else time.time() + + # 从队列中寻找时间最接近的IMU数据 + closest_imu = None + min_diff = float('inf') + + temp_imu_list = [] + while not self.imu_queue.empty(): + imu = self.imu_queue.get(timeout=0.01) + temp_imu_list.append(imu) + + for imu in temp_imu_list: + time_diff = abs(imu.timestamp - target_time) + if time_diff < min_diff and time_diff < self.sync_tolerance: + min_diff = time_diff + closest_imu = imu + + # 放回未使用的数据 + for imu in temp_imu_list: + if imu is not closest_imu and not self.imu_queue.full(): + self.imu_queue.put(imu) + + imu_data = closest_imu + + # 获取同步的GNSS数据 + gnss_data = None + if not self.gnss_queue.empty(): + gnss_data = self.gnss_queue.get(timeout=0.1) + + return pc_data, imu_data, gnss_data + + except Exception as e: + print(f"数据同步失败: {e}") + return None, None, None \ No newline at end of file From 0d5b42a9cb1e926b8e5fa2212416706a5c8d18a2 Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Thu, 18 Dec 2025 20:07:24 +0800 Subject: [PATCH 02/13] =?UTF-8?q?=E6=A8=A1=E5=9D=97=E5=8C=96PID=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../object_detection.py | 106 ++++++++++++------ 1 file changed, 70 insertions(+), 36 deletions(-) diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py index 97cba38106..919f3f96c9 100644 --- a/src/Object_Tracking_Planning/object_detection.py +++ b/src/Object_Tracking_Planning/object_detection.py @@ -1178,7 +1178,7 @@ def _pid_control(self, target_speed, current_speed): return np.clip((self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) -class PIDLateralController(): +class PIDLateralController: """ PIDLateralController implements lateral control using a PID. """ @@ -1187,64 +1187,98 @@ def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=0.0, dt=0.03): """ Constructor method. - :param vehicle: actor to apply to local planner logic onto - :param K_P: Proportional term - :param K_D: Differential term - :param K_I: Integral term - :param dt: time differential in seconds + Args: + vehicle: Actor to apply local planner logic onto + K_P: Proportional term coefficient (default: 1.0) + K_D: Differential term coefficient (default: 0.0) + K_I: Integral term coefficient (default: 0.0) + dt: Time differential in seconds (default: 0.03) """ self._vehicle = vehicle self._k_p = K_P self._k_d = K_D self._k_i = K_I self._dt = dt - self._e_buffer = deque(maxlen=10) + self._error_buffer = deque(maxlen=10) def run_step(self, waypoint): """ - Execute one step of lateral control to steer - the vehicle towards a certain waypoin. + Execute one step of lateral control to steer the vehicle towards a waypoint. - :param waypoint: target waypoint - :return: steering control in the range [-1, 1] where: - -1 maximum steering to left - +1 maximum steering to right + Args: + waypoint: Target waypoint + + Returns: + float: Steering control in range [-1, 1] where: + -1 = maximum steering to left + +1 = maximum steering to right """ return self._pid_control(waypoint, self._vehicle.get_transform()) def _pid_control(self, waypoint, vehicle_transform): """ - Estimate the steering angle of the vehicle based on the PID equations + Estimate steering angle based on PID equations. - :param waypoint: target waypoint - :param vehicle_transform: current transform of the vehicle - :return: steering control in the range [-1, 1] + Args: + waypoint: Target waypoint + vehicle_transform: Current transform of the vehicle + + Returns: + float: Steering control in range [-1, 1] """ + # Calculate vehicle direction vector v_begin = vehicle_transform.location - v_end = v_begin + carla.Location(x=math.cos(math.radians(vehicle_transform.rotation.yaw)), - y=math.sin(math.radians(vehicle_transform.rotation.yaw))) + yaw_rad = math.radians(vehicle_transform.rotation.yaw) + v_end = v_begin + carla.Location( + x=math.cos(yaw_rad), + y=math.sin(yaw_rad) + ) + # Calculate vectors v_vec = np.array([v_end.x - v_begin.x, v_end.y - v_begin.y, 0.0]) - w_vec = np.array([waypoint.transform.location.x - - v_begin.x, waypoint.transform.location.y - - v_begin.y, 0.0]) - _dot = math.acos(np.clip(np.dot(w_vec, v_vec) / - (np.linalg.norm(w_vec) * np.linalg.norm(v_vec)), -1.0, 1.0)) - - _cross = np.cross(v_vec, w_vec) + w_vec = np.array([ + waypoint.transform.location.x - v_begin.x, + waypoint.transform.location.y - v_begin.y, + 0.0 + ]) + + # Calculate angle difference + v_norm = np.linalg.norm(v_vec) + w_norm = np.linalg.norm(w_vec) + dot_product = np.dot(w_vec, v_vec) / (v_norm * w_norm) + dot_product_clipped = np.clip(dot_product, -1.0, 1.0) + angle_diff = math.acos(dot_product_clipped) + + # Determine sign based on cross product + cross_product = np.cross(v_vec, w_vec) + if cross_product[2] < 0: + angle_diff *= -1.0 + + # Update error buffer and calculate PID terms + self._error_buffer.append(angle_diff) + + # Calculate derivative and integral terms + derivative_error = self._calculate_derivative_error() + integral_error = self._calculate_integral_error() + + # PID calculation with clamping + pid_output = ( + self._k_p * angle_diff + + self._k_d * derivative_error + + self._k_i * integral_error + ) - if _cross[2] < 0: - _dot *= -1.0 + return np.clip(pid_output, -1.0, 1.0) - self._e_buffer.append(_dot) - if len(self._e_buffer) >= 2: - _de = (self._e_buffer[-1] - self._e_buffer[-2]) / self._dt - _ie = sum(self._e_buffer) * self._dt - else: - _de = 0.0 - _ie = 0.0 + def _calculate_derivative_error(self): + """Calculate derivative error term.""" + if len(self._error_buffer) >= 2: + return (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt + return 0.0 - return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) + def _calculate_integral_error(self): + """Calculate integral error term.""" + return sum(self._error_buffer) * self._dt # ============================================================================== From 737c4bdf0d281f4a3f383d205052db61377de670 Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Thu, 18 Dec 2025 20:18:52 +0800 Subject: [PATCH 03/13] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BC=A0=E6=84=9F?= =?UTF-8?q?=E5=99=A8=E6=95=B0=E6=8D=AE=E9=87=87=E9=9B=86=E6=A8=A1=E5=9E=8B?= =?UTF-8?q?=E5=8F=8A=E8=BF=90=E8=A1=8C=E6=95=88=E6=9E=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../sensor_interface.py | 161 +++++++++++++++++- 1 file changed, 154 insertions(+), 7 deletions(-) diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/sensor_interface.py b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/sensor_interface.py index b569f67d4a..16a6d6c236 100644 --- a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/sensor_interface.py +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/sensor_interface.py @@ -6,8 +6,55 @@ import json from dataclasses import dataclass from typing import Optional, Tuple +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import random +# ====================== 传感器模拟器部分(原缺失的依赖) ====================== +class LidarSimulator: + def generate_pointcloud(self): + """模拟生成(1000, 3)的点云数据(x,y,z)""" + n_points = 1000 + x = np.random.uniform(-50, 50, n_points) + y = np.random.uniform(-50, 50, n_points) + z = np.random.uniform(-5, 5, n_points) + return np.column_stack((x, y, z)) + + +class IMUSimulator: + def get_acceleration(self): + """模拟加速度(含重力+微小噪声)""" + return np.array([0, 0, 9.81 + random.uniform(-0.1, 0.1)]) + + def get_gyro(self): + """模拟陀螺仪数据(微小噪声)""" + return np.array([random.uniform(-0.01, 0.01) for _ in range(3)]) + + +class GNSSTrueSimulator: + def __init__(self): + self.lat = 39.9042 # 基准纬度(如北京天安门) + self.lon = 116.4074 # 基准经度 + self.alt = 50.0 # 基准海拔 + + def get_latitude(self): + """模拟纬度缓慢变化""" + self.lat += random.uniform(-1e-6, 1e-6) + return self.lat + + def get_longitude(self): + """模拟经度缓慢变化""" + self.lon += random.uniform(-1e-6, 1e-6) + return self.lon + + def get_altitude(self): + """模拟海拔缓慢变化""" + self.alt += random.uniform(-0.01, 0.01) + return self.alt + + +# ====================== 原代码核心部分(无任何修改) ====================== @dataclass class PointCloud: points: np.ndarray # (N, 3) or (N, 4) for intensity @@ -91,8 +138,7 @@ def stop_capture(self): def _lidar_capture(self): """模拟激光雷达数据采集""" - from sensor_simulators import LidarSimulator - + # 直接使用上方定义的模拟器(不再需要外部导入) sim = LidarSimulator() while self.running: points = sim.generate_pointcloud() @@ -106,8 +152,7 @@ def _lidar_capture(self): def _imu_capture(self): """模拟IMU数据采集""" - from sensor_simulators import IMUSimulator - + # 直接使用上方定义的模拟器 sim = IMUSimulator() while self.running: imu_data = IMUData( @@ -121,8 +166,7 @@ def _imu_capture(self): def _gnss_capture(self): """模拟GNSS数据采集""" - from sensor_simulators import GNSSTrueSimulator - + # 直接使用上方定义的模拟器 sim = GNSSTrueSimulator() while self.running: gnss_data = GNSSData( @@ -181,4 +225,107 @@ def get_synchronized_data(self, timeout=1.0) -> Tuple[Optional[PointCloud], except Exception as e: print(f"数据同步失败: {e}") - return None, None, None \ No newline at end of file + return None, None, None + + +# ====================== 可视化与运行逻辑(外部扩展整合进来) ====================== +def visualize_synchronized_data(pc, imu, gnss, fig, axes): + """可视化同步数据""" + ax_pc, ax_imu_accel, ax_imu_gyro, ax_gnss = axes + # 清空画布 + ax_pc.clear() + ax_imu_accel.clear() + ax_imu_gyro.clear() + ax_gnss.clear() + + # 1. 绘制点云 + if pc is not None: + points = pc.points + ax_pc.scatter(points[:, 0], points[:, 1], points[:, 2], s=1, c='b') + ax_pc.set_xlabel('X (m)') + ax_pc.set_ylabel('Y (m)') + ax_pc.set_zlabel('Z (m)') + ax_pc.set_title(f'Point Cloud (time: {pc.timestamp:.2f})') + else: + ax_pc.set_title('Point Cloud: No Data') + + # 2. 绘制IMU数据 + if imu is not None: + # 加速度 + ax_imu_accel.bar(['X', 'Y', 'Z'], imu.accel, color=['r', 'g', 'b']) + ax_imu_accel.set_ylim(9.7, 9.9) + ax_imu_accel.set_title(f'IMU Accel (time: {imu.timestamp:.2f})') + ax_imu_accel.set_ylabel('Accel (m/s²)') + # 陀螺仪 + ax_imu_gyro.bar(['X', 'Y', 'Z'], imu.gyro, color=['r', 'g', 'b']) + ax_imu_gyro.set_ylim(-0.01, 0.01) + ax_imu_gyro.set_title('IMU Gyro (rad/s)') + ax_imu_gyro.set_ylabel('Gyro (rad/s)') + else: + ax_imu_accel.set_title('IMU Accel: No Data') + ax_imu_gyro.set_title('IMU Gyro: No Data') + + # 3. 绘制GNSS数据 + if gnss is not None: + ax_gnss.text(0.1, 0.8, f'Lat: {gnss.lat:.6f}', transform=ax_gnss.transAxes) + ax_gnss.text(0.1, 0.6, f'Lon: {gnss.lon:.6f}', transform=ax_gnss.transAxes) + ax_gnss.text(0.1, 0.4, f'Alt: {gnss.alt:.2f} m', transform=ax_gnss.transAxes) + ax_gnss.text(0.1, 0.2, f'Time: {gnss.timestamp:.2f}', transform=ax_gnss.transAxes) + ax_gnss.set_xlim(0, 1) + ax_gnss.set_ylim(0, 1) + ax_gnss.axis('off') + else: + ax_gnss.set_title('GNSS: No Data') + ax_gnss.axis('off') + + # 实时更新 + plt.draw() + plt.pause(0.001) + + +def main(): + """主运行函数""" + # 1. 实例化传感器接口 + sensor_if = SensorInterface() + + # 2. 初始化可视化画布 + fig = plt.figure(figsize=(12, 8)) + ax_pc = fig.add_subplot(221, projection='3d') + ax_imu_accel = fig.add_subplot(222) + ax_imu_gyro = fig.add_subplot(223) + ax_gnss = fig.add_subplot(224) + axes = (ax_pc, ax_imu_accel, ax_imu_gyro, ax_gnss) + plt.ion() # 交互模式 + + try: + # 3. 启动传感器采集 + sensor_if.start_capture() + + # 4. 循环获取同步数据并展示 + while True: + pc_data, imu_data, gnss_data = sensor_if.get_synchronized_data() + + # 文本输出 + print("\n=== 同步数据摘要 ===") + print(f"点云: {'有数据' if pc_data else '无数据'} (数量: {pc_data.points.shape[0] if pc_data else 0})") + print(f"IMU: {'有数据' if imu_data else '无数据'} (加速度: {imu_data.accel if imu_data else 'None'})") + print( + f"GNSS: {'有数据' if gnss_data else '无数据'} (经纬度: {f'{gnss_data.lat:.6f}, {gnss_data.lon:.6f}' if gnss_data else 'None'})") + + # 可视化输出 + visualize_synchronized_data(pc_data, imu_data, gnss_data, fig, axes) + + time.sleep(0.1) # 与点云频率同步 + + except KeyboardInterrupt: + # 停止采集 + print("\n停止传感器采集...") + sensor_if.stop_capture() + plt.ioff() + plt.show() + print("采集已停止") + + +# 程序入口 +if __name__ == "__main__": + main() \ No newline at end of file From c15f438c79c0d990a53cb5ec9381a52a1092965b Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Fri, 19 Dec 2025 00:39:19 +0800 Subject: [PATCH 04/13] =?UTF-8?q?=E4=BF=AE=E6=94=B9readme=E6=96=87?= =?UTF-8?q?=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../README.md | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md index c7c8ba177a..202373667b 100644 --- a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md @@ -3,6 +3,7 @@ # 2.可靠的全局定位 # 3.无人工干预的自主路径规划与导航 + # 构建目录结构 ```text @@ -21,4 +22,11 @@ outdoor_ws/src/robot_description/ ├── config/ │ ├── controllers.yaml # ros2_control配置 │ └── robot_control.yaml # 控制参数 -└── meshes/ # 可选:3D模型文件 \ No newline at end of file +└── meshes/ # 可选:3D模型文件 +``` +## 核心功能 +1. **多传感器数据采集、同步与统一接口管理** + - 封装激光雷达、IMU、GNSS和轮式编码器的数据采集 + - 实现多传感器时间同步(10ms容差) + - 提供线程安全的数据队列 + - 包含传感器配置管理和模拟器(用于测试) \ No newline at end of file From 10fb8195e9f80cf4972a623e970f81e860d87d4c Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Fri, 19 Dec 2025 12:21:25 +0800 Subject: [PATCH 05/13] =?UTF-8?q?=E6=B7=BB=E5=8A=A0LiDAR-IMU=E7=B4=A7?= =?UTF-8?q?=E8=80=A6=E5=90=88=E9=87=8C=E7=A8=8B=E8=AE=A1=E6=A8=A1=E5=9D=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../lio_odometry.py | 665 ++++++++++++++++++ 1 file changed, 665 insertions(+) create mode 100644 src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/lio_odometry.py diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/lio_odometry.py b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/lio_odometry.py new file mode 100644 index 0000000000..d920f30885 --- /dev/null +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/lio_odometry.py @@ -0,0 +1,665 @@ +# lio_odometry.py +import numpy as np +from scipy.spatial.transform import Rotation as R +from scipy.spatial import KDTree +from collections import deque +import time +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import warnings + +warnings.filterwarnings('ignore') + + +class LIOdometry: + """LiDAR-IMU紧耦合里程计""" + + def __init__(self, config=None): + self.config = config or self._default_config() + + # 状态变量 + self.position = np.zeros(3) # 位置 (x, y, z) + self.velocity = np.zeros(3) # 速度 + self.orientation = np.eye(3) # 旋转矩阵 + self.gravity = np.array([0, 0, -9.81]) # 重力向量 + + # 偏差 + self.gyro_bias = np.zeros(3) + self.accel_bias = np.zeros(3) + + # 缓冲区 + self.imu_buffer = deque(maxlen=100) + self.last_lidar_time = None + + # 协方差矩阵 + self.covariance = np.eye(15) * 0.1 + + # 预积分 + self.delta_p = np.zeros(3) # 位置增量 + self.delta_v = np.zeros(3) # 速度增量 + self.delta_r = np.eye(3) # 旋转增量 + + # === 新增:可视化相关变量 === + self.trajectory = [] # 轨迹历史 + self.features_history = [] # 特征点历史 + self.max_history = 1000 # 最大历史记录数 + # ========================== + + def _default_config(self): + return { + "imu_noise": { + "gyro": 0.01, # rad/s + "accel": 0.1, # m/s² + "gyro_bias": 1e-5, + "accel_bias": 1e-4 + }, + "lidar_noise": { + "rotation": 0.001, # rad + "translation": 0.01 # m + }, + "mapping": { + "voxel_size": 0.2, + "max_range": 50.0, + "min_range": 1.0 + } + } + + def preintegrate_imu(self, imu_data_list): + """IMU预积分""" + if not imu_data_list: + return + + # 重置预积分量 + self.delta_p = np.zeros(3) + self.delta_v = np.zeros(3) + self.delta_r = np.eye(3) + + last_time = imu_data_list[0].timestamp + + for i in range(1, len(imu_data_list)): + current_imu = imu_data_list[i] + dt = current_imu.timestamp - last_time + + if dt <= 0: + continue + + # 去除偏差后的测量值 + accel = current_imu.accel - self.accel_bias + gyro = current_imu.gyro - self.gyro_bias + + # 中值积分 + accel_0 = imu_data_list[i - 1].accel - self.accel_bias + gyro_0 = imu_data_list[i - 1].gyro - self.gyro_bias + + accel_hat = 0.5 * (accel_0 + accel) + gyro_hat = 0.5 * (gyro_0 + gyro) + + # 旋转更新 + delta_theta = gyro_hat * dt + delta_rotation = R.from_rotvec(delta_theta).as_matrix() + self.delta_r = self.delta_r @ delta_rotation + + # 速度更新 + accel_world = self.delta_r @ accel_hat + self.delta_v += (accel_world + self.gravity) * dt + + # 位置更新 + self.delta_p += self.delta_v * dt + 0.5 * (accel_world + self.gravity) * dt * dt + + last_time = current_imu.timestamp + + def extract_features(self, point_cloud): + """从点云中提取几何特征""" + if point_cloud.shape[0] < 100: + return [], [] + + # 地面分割 + ground_points, non_ground = self._ground_segmentation(point_cloud) + + # 提取平面特征(来自地面点) + plane_features = self._extract_plane_features(ground_points) + + # 提取边缘特征(来自非地面点) + edge_features = self._extract_edge_features(non_ground) + + return plane_features, edge_features + + def _ground_segmentation(self, points): + """简单的地面分割""" + if points.shape[0] == 0: + return np.zeros((0, 3)), np.zeros((0, 3)) + + # 基于高度的简单分割 + z_values = points[:, 2] + ground_mask = z_values < 0.2 # 假设地面高度阈值 + ground_points = points[ground_mask] + non_ground = points[~ground_mask] + + return ground_points, non_ground + + def _extract_plane_features(self, points, num_features=20): + """提取平面特征点""" + if points.shape[0] < num_features: + return points + + # 使用曲率选择最平坦的点 + curvatures = self._compute_curvature(points) + flat_indices = np.argsort(curvatures)[:num_features] + + return points[flat_indices] + + def _extract_edge_features(self, points, num_features=20): + """提取边缘特征点""" + if points.shape[0] < num_features: + return points + + # 使用曲率选择最尖锐的点 + curvatures = self._compute_curvature(points) + edge_indices = np.argsort(curvatures)[-num_features:] + + return points[edge_indices] + + def _compute_curvature(self, points, k=5): + """计算点云曲率""" + from sklearn.neighbors import NearestNeighbors + + if points.shape[0] < k: + return np.zeros(points.shape[0]) + + nbrs = NearestNeighbors(n_neighbors=k + 1, algorithm='kd_tree').fit(points) + distances, indices = nbrs.kneighbors(points) + + curvatures = [] + for i in range(points.shape[0]): + neighbor_points = points[indices[i][1:]] # 排除自身 + centered = neighbor_points - neighbor_points.mean(axis=0) + cov = centered.T @ centered + eigenvalues = np.linalg.eigvalsh(cov) + curvature = eigenvalues[0] / (eigenvalues.sum() + 1e-6) + curvatures.append(curvature) + + return np.array(curvatures) + + def scan_to_map_matching(self, current_features, map_features): + """当前扫描与地图匹配""" + if len(current_features) == 0 or len(map_features) == 0: + return np.eye(4) + + # 使用ICP进行配准 + transformation = self._icp_registration(current_features, map_features) + + return transformation + + def _icp_registration(self, source, target, max_iterations=20): + """迭代最近点算法""" + if len(source) < 3 or len(target) < 3: + return np.eye(4) + + # 构建KD树加速最近邻搜索 + tree = KDTree(target) + + # 初始变换矩阵 + transformation = np.eye(4) + + for iteration in range(max_iterations): + # 找到最近邻对应点 + distances, indices = tree.query(source) + + # 过滤掉距离太远的点对 + valid_mask = distances < 1.0 # 1米阈值 + if np.sum(valid_mask) < 3: + break + + source_valid = source[valid_mask] + target_valid = target[indices[valid_mask]] + + # 计算最优刚体变换 + R_mat, t_vec = self._compute_rigid_transform(source_valid, target_valid) + + # 更新变换 + delta_transform = np.eye(4) + delta_transform[:3, :3] = R_mat + delta_transform[:3, 3] = t_vec + + transformation = delta_transform @ transformation + + # 更新源点云 + source = (R_mat @ source.T + t_vec.reshape(3, 1)).T + + # 检查收敛 + if np.linalg.norm(t_vec) < 0.001 and np.linalg.norm(R_mat - np.eye(3)) < 0.001: + break + + return transformation + + def _compute_rigid_transform(self, A, B): + """计算点集A到点集B的最优刚体变换""" + centroid_A = np.mean(A, axis=0) + centroid_B = np.mean(B, axis=0) + + AA = A - centroid_A + BB = B - centroid_B + + H = AA.T @ BB + U, S, Vt = np.linalg.svd(H) + + R = Vt.T @ U.T + + # 处理反射情况 + if np.linalg.det(R) < 0: + Vt[-1, :] *= -1 + R = Vt.T @ U.T + + t = centroid_B - R @ centroid_A + + return R, t + + def process_frame(self, point_cloud, imu_data_list, gnss_data=None): + """处理一帧数据""" + # IMU预积分 + self.preintegrate_imu(imu_data_list) + + # 提取特征 + plane_features, edge_features = self.extract_features(point_cloud) + all_features = np.vstack([plane_features, edge_features]) if len(plane_features) > 0 else edge_features + + # 运动估计(这里简化为使用IMU预积分) + # 在实际系统中,这里会进行扫描匹配 + delta_pose = np.eye(4) + delta_pose[:3, :3] = self.delta_r + delta_pose[:3, 3] = self.delta_p + + # 更新状态 + self.orientation = self.orientation @ self.delta_r + self.position += self.orientation @ self.delta_p + self.velocity += self.orientation @ self.delta_v + + # 如果有GNSS数据,可以在这里融合 + if gnss_data: + self._fuse_gnss(gnss_data) + + # 构建当前位姿 + pose = np.eye(4) + pose[:3, :3] = self.orientation + pose[:3, 3] = self.position + + # === 新增:记录轨迹和特征用于可视化 === + self._record_for_visualization(pose, all_features) + # ================================= + + return pose, all_features + + def _fuse_gnss(self, gnss_data): + """融合GNSS数据(简化版本)""" + # 将经纬度转换为局部坐标(需要知道原点) + # 这里简化为直接信任GNSS(在实际系统中应使用卡尔曼滤波) + local_pos = self._llh_to_local(gnss_data.lat, gnss_data.lon, gnss_data.alt) + + # 简单的加权融合 + gnss_weight = 0.1 # 信任度 + self.position = (1 - gnss_weight) * self.position + gnss_weight * local_pos + + def _llh_to_local(self, lat, lon, alt): + """经纬度转局部坐标(简化版)""" + # 实际实现中需要知道原点的经纬度 + return np.array([lon * 111320.0, lat * 110540.0, alt]) # 近似转换 + + # ============================================= + # === 以下是新增的可视化方法,原功能完全不变 === + # ============================================= + + def _record_for_visualization(self, pose, features): + """记录数据用于可视化(内部方法)""" + # 记录轨迹 + self.trajectory.append(pose[:3, 3].copy()) + + # 记录特征点 + if isinstance(features, np.ndarray) and features.shape[0] > 0: + self.features_history.append(features.copy()) + else: + self.features_history.append(np.zeros((0, 3))) + + # 限制历史数据长度 + if len(self.trajectory) > self.max_history: + self.trajectory = self.trajectory[-self.max_history:] + if len(self.features_history) > self.max_history: + self.features_history = self.features_history[-self.max_history:] + + def show_trajectory(self): + """显示轨迹(2D和3D)""" + if not self.trajectory: + print("No trajectory data to visualize") + return + + trajectory_array = np.array(self.trajectory) + + fig = plt.figure(figsize=(15, 5)) + + # 1. 3D轨迹 + ax1 = fig.add_subplot(131, projection='3d') + ax1.plot(trajectory_array[:, 0], trajectory_array[:, 1], trajectory_array[:, 2], + 'b-', linewidth=2, label='Trajectory') + if len(trajectory_array) > 1: + ax1.scatter(trajectory_array[0, 0], trajectory_array[0, 1], trajectory_array[0, 2], + c='g', s=100, marker='o', label='Start') + ax1.scatter(trajectory_array[-1, 0], trajectory_array[-1, 1], trajectory_array[-1, 2], + c='r', s=100, marker='s', label='End') + ax1.set_xlabel('X (m)') + ax1.set_ylabel('Y (m)') + ax1.set_zlabel('Z (m)') + ax1.set_title('3D Trajectory') + ax1.legend() + ax1.grid(True) + + # 2. 2D轨迹(XY平面) + ax2 = fig.add_subplot(132) + ax2.plot(trajectory_array[:, 0], trajectory_array[:, 1], 'b-', linewidth=2) + if len(trajectory_array) > 1: + ax2.scatter(trajectory_array[0, 0], trajectory_array[0, 1], c='g', s=100) + ax2.scatter(trajectory_array[-1, 0], trajectory_array[-1, 1], c='r', s=100) + ax2.set_xlabel('X (m)') + ax2.set_ylabel('Y (m)') + ax2.set_title('2D Trajectory (XY Plane)') + ax2.axis('equal') + ax2.grid(True) + + # 3. 高程变化 + ax3 = fig.add_subplot(133) + if len(trajectory_array) > 1: + distances = np.cumsum(np.sqrt(np.sum(np.diff(trajectory_array[:, :2], axis=0) ** 2, axis=1))) + distances = np.insert(distances, 0, 0) + ax3.plot(distances, trajectory_array[:, 2], 'b-', linewidth=2) + ax3.set_xlabel('Distance (m)') + ax3.set_ylabel('Z (m)') + ax3.set_title('Elevation Profile') + ax3.grid(True) + + plt.tight_layout() + plt.show() + return fig + + def show_features(self, point_cloud=None): + """显示特征点和点云""" + if not self.features_history: + print("No feature data to visualize") + return + + recent_features = self.features_history[-1] + + fig = plt.figure(figsize=(15, 5)) + + # 1. 3D特征点 + ax1 = fig.add_subplot(131, projection='3d') + if recent_features.shape[0] > 0: + # 转换特征点到世界坐标系 + if hasattr(self, 'orientation') and hasattr(self, 'position'): + features_homogeneous = np.hstack([recent_features, np.ones((len(recent_features), 1))]) + current_pose = np.eye(4) + current_pose[:3, :3] = self.orientation + current_pose[:3, 3] = self.position + world_features = (current_pose @ features_homogeneous.T).T[:, :3] + + ax1.scatter(world_features[:, 0], world_features[:, 1], world_features[:, 2], + c='red', s=20, label='Features') + + ax1.scatter(self.position[0], self.position[1], self.position[2], + c='blue', s=100, marker='s', label='Current Position') + ax1.set_xlabel('X (m)') + ax1.set_ylabel('Y (m)') + ax1.set_zlabel('Z (m)') + ax1.set_title('Feature Points in World Frame') + ax1.legend() + ax1.grid(True) + + # 2. 特征点统计 + ax2 = fig.add_subplot(132) + feature_counts = [len(f) for f in self.features_history[-20:] if f.shape[0] > 0] + if feature_counts: + ax2.plot(range(len(feature_counts)), feature_counts, 'b-o', linewidth=2, markersize=6) + ax2.set_xlabel('Recent Frames') + ax2.set_ylabel('Number of Features') + ax2.set_title('Feature Count History') + ax2.grid(True) + + # 3. 点云(如果提供) + ax3 = fig.add_subplot(133, projection='3d') + if point_cloud is not None and point_cloud.shape[0] > 0: + # 随机采样显示 + if point_cloud.shape[0] > 1000: + indices = np.random.choice(point_cloud.shape[0], 1000, replace=False) + point_cloud = point_cloud[indices] + + ax3.scatter(point_cloud[:, 0], point_cloud[:, 1], point_cloud[:, 2], + c='gray', s=1, alpha=0.3, label='Point Cloud') + + if recent_features.shape[0] > 0: + ax3.scatter(recent_features[:, 0], recent_features[:, 1], recent_features[:, 2], + c='red', s=20, label='Features') + + ax3.set_xlabel('X') + ax3.set_ylabel('Y') + ax3.set_zlabel('Z') + ax3.set_title('Latest Point Cloud & Features') + ax3.legend() + + plt.tight_layout() + plt.show() + return fig + + def show_comprehensive_analysis(self): + """显示综合分析图表""" + if not self.trajectory: + print("No data for analysis") + return + + trajectory_array = np.array(self.trajectory) + + fig = plt.figure(figsize=(16, 12)) + + # 1. 3D轨迹 + ax1 = fig.add_subplot(331, projection='3d') + ax1.plot(trajectory_array[:, 0], trajectory_array[:, 1], trajectory_array[:, 2], + 'b-', linewidth=2) + ax1.set_xlabel('X (m)') + ax1.set_ylabel('Y (m)') + ax1.set_zlabel('Z (m)') + ax1.set_title('3D Trajectory') + ax1.grid(True) + + # 2. 2D轨迹 + ax2 = fig.add_subplot(332) + ax2.plot(trajectory_array[:, 0], trajectory_array[:, 1], 'b-', linewidth=2) + ax2.set_xlabel('X (m)') + ax2.set_ylabel('Y (m)') + ax2.set_title('2D Trajectory') + ax2.axis('equal') + ax2.grid(True) + + # 3. 位置分量 + ax3 = fig.add_subplot(333) + frames = range(len(trajectory_array)) + ax3.plot(frames, trajectory_array[:, 0], 'r-', label='X', linewidth=1) + ax3.plot(frames, trajectory_array[:, 1], 'g-', label='Y', linewidth=1) + ax3.plot(frames, trajectory_array[:, 2], 'b-', label='Z', linewidth=1) + ax3.set_xlabel('Frame') + ax3.set_ylabel('Position (m)') + ax3.set_title('Position Components') + ax3.legend() + ax3.grid(True) + + # 4. 速度信息 + ax4 = fig.add_subplot(334) + try: + velocity_norm = np.linalg.norm(self.velocity) + ax4.bar(['Speed'], [velocity_norm], color='skyblue') + ax4.set_ylabel('m/s') + ax4.set_title(f'Current Speed: {velocity_norm:.2f} m/s') + ax4.grid(True, alpha=0.3) + except: + pass + + # 5. 特征点统计 + ax5 = fig.add_subplot(335) + if self.features_history: + feature_counts = [len(f) for f in self.features_history[-50:] if f.shape[0] > 0] + if feature_counts: + ax5.plot(range(len(feature_counts)), feature_counts, 'purple', linewidth=2) + ax5.set_xlabel('Recent Frames') + ax5.set_ylabel('Feature Count') + ax5.set_title('Feature Extraction History') + ax5.grid(True) + + # 6. 轨迹长度 + ax6 = fig.add_subplot(336) + if len(trajectory_array) > 1: + total_distance = np.sum(np.sqrt(np.sum(np.diff(trajectory_array, axis=0) ** 2, axis=1))) + ax6.text(0.5, 0.5, f'Total Distance:\n{total_distance:.2f} m', + ha='center', va='center', fontsize=14, transform=ax6.transAxes) + ax6.set_title('Trajectory Statistics') + ax6.axis('off') + + # 7. 最新特征点 + ax7 = fig.add_subplot(337, projection='3d') + if self.features_history and self.features_history[-1].shape[0] > 0: + recent_features = self.features_history[-1] + ax7.scatter(recent_features[:, 0], recent_features[:, 1], recent_features[:, 2], + c='red', s=10, alpha=0.6) + ax7.set_xlabel('X') + ax7.set_ylabel('Y') + ax7.set_zlabel('Z') + ax7.set_title('Latest Features (Local)') + + # 8. 状态信息 + ax8 = fig.add_subplot(338) + info_text = f"Frames: {len(self.trajectory)}\n" + info_text += f"Position: ({self.position[0]:.2f}, {self.position[1]:.2f}, {self.position[2]:.2f})\n" + info_text += f"Velocity: {np.linalg.norm(self.velocity):.2f} m/s" + ax8.text(0.1, 0.5, info_text, ha='left', va='center', fontsize=12, + transform=ax8.transAxes, family='monospace') + ax8.set_title('Current State') + ax8.axis('off') + + # 9. 空图表(预留) + ax9 = fig.add_subplot(339) + ax9.text(0.5, 0.5, 'LIO Odometry\nVisualization', + ha='center', va='center', fontsize=16, transform=ax9.transAxes) + ax9.axis('off') + + plt.tight_layout() + plt.show() + return fig + + def export_trajectory(self, filename="trajectory.txt", format="kitti"): + """导出轨迹到文件""" + if not self.trajectory: + print("No trajectory to export") + return + + import os + os.makedirs(os.path.dirname(filename) if os.path.dirname(filename) else '.', exist_ok=True) + + with open(filename, 'w') as f: + if format.lower() == "kitti": + # KITTI格式 + for pos in self.trajectory: + # 使用当前方向或单位矩阵 + rot_matrix = self.orientation if hasattr(self, 'orientation') else np.eye(3) + line = ' '.join([f'{val:.6f}' for val in rot_matrix.flatten()]) + line += f' {pos[0]:.6f} {pos[1]:.6f} {pos[2]:.6f}' + f.write(line + '\n') + print(f"Trajectory exported in KITTI format to {filename}") + + elif format.lower() == "tum": + # TUM格式 + f.write("# timestamp tx ty tz qx qy qz qw\n") + for i, pos in enumerate(self.trajectory): + timestamp = i * 0.1 # 假设时间间隔 + rot = self.orientation if hasattr(self, 'orientation') else np.eye(3) + rotation = R.from_matrix(rot) + quat = rotation.as_quat() # [x, y, z, w] + line = f"{timestamp:.6f} {pos[0]:.6f} {pos[1]:.6f} {pos[2]:.6f} " + line += f"{quat[0]:.6f} {quat[1]:.6f} {quat[2]:.6f} {quat[3]:.6f}" + f.write(line + '\n') + print(f"Trajectory exported in TUM format to {filename}") + else: + print(f"Unknown format: {format}") + + def clear_visualization_data(self): + """清除可视化数据""" + self.trajectory.clear() + self.features_history.clear() + + +# ============================================= +# === 以下是为了测试而添加的模拟代码 === +# ============================================= + +class MockIMUData: + """模拟IMU数据类(仅用于测试)""" + + def __init__(self, timestamp, accel, gyro): + self.timestamp = timestamp + self.accel = accel + self.gyro = gyro + + +class MockGNSSData: + """模拟GNSS数据类(仅用于测试)""" + + def __init__(self, lat, lon, alt): + self.lat = lat + self.lon = lon + self.alt = alt + + +def test_demo(): + """测试演示函数(可以直接在PyCharm中运行)""" + print("Starting LIO Odometry Visualization Demo...") + + # 创建LIOdometry实例 + odom = LIOdometry() + + # 模拟运行10帧 + for i in range(10): + # 生成模拟点云 + n_points = 500 + np.random.randint(0, 500) + point_cloud = np.random.randn(n_points, 3) * 3.0 + point_cloud[:, 2] = np.abs(point_cloud[:, 2]) * 0.5 + + # 生成模拟IMU数据(轻微运动) + imu_data = [ + MockIMUData(0.0, np.array([0, 0, -9.81]), np.array([0, 0, 0])), + MockIMUData(0.01, np.array([0.1 * (i / 10), 0, -9.81]), np.array([0, 0, 0.05 * (i / 10)])) + ] + + # 处理帧(原有功能) + pose, features = odom.process_frame(point_cloud, imu_data) + + print(f"Frame {i + 1}: Position = [{pose[0, 3]:.2f}, {pose[1, 3]:.2f}, {pose[2, 3]:.2f}], " + f"Features = {features.shape[0]}") + + print("\nGenerating visualizations...") + + # 显示轨迹 + odom.show_trajectory() + + # 显示特征点 + # 注意:这里需要最新的点云,我们重新生成一个 + test_point_cloud = np.random.randn(300, 3) * 2.0 + odom.show_features(test_point_cloud) + + # 显示综合分析 + odom.show_comprehensive_analysis() + + # 导出轨迹 + odom.export_trajectory("test_trajectory_kitti.txt", "kitti") + odom.export_trajectory("test_trajectory_tum.txt", "tum") + + print("\nDemo completed!") + print("Close the matplotlib windows to exit.") + + +# 主程序入口 +if __name__ == "__main__": + # 自动运行测试演示 + test_demo() \ No newline at end of file From 837902a843be986dcc615ac4ee2a23b05222a015 Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Fri, 19 Dec 2025 14:57:41 +0800 Subject: [PATCH 06/13] =?UTF-8?q?=E7=AE=80=E6=B4=81=E5=8C=96=E4=B8=8E?= =?UTF-8?q?=E7=8E=AF=E5=A2=83=E7=AE=A1=E7=90=86=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../object_detection.py | 172 ++++++++++++------ 1 file changed, 113 insertions(+), 59 deletions(-) diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py index 919f3f96c9..a05617ff4c 100644 --- a/src/Object_Tracking_Planning/object_detection.py +++ b/src/Object_Tracking_Planning/object_detection.py @@ -105,113 +105,167 @@ def get_actor_display_name(actor, truncate=250): # ============================================================================== class World(object): - """ Class representing the surrounding environment """ + """Class representing the surrounding environment""" + + # 常量提取 + DEFAULT_VEHICLE_MODEL = 'model3' + DEFAULT_VEHICLE_COLOR = '236,102,17' def __init__(self, carla_world, hud, args): """Constructor method""" self.world = carla_world - try: - self.map = self.world.get_map() - except RuntimeError as error: - print('RuntimeError: {}'.format(error)) - print(' The server could not send the OpenDRIVE (.xodr) file:') - print(' Make sure it exists, has the same name of your town, and is correct.') - sys.exit(1) self.hud = hud + self.args = args + + self._initialize_map() + self._weather_presets = find_weather_presets() + self._weather_index = 0 + + # 初始化所有传感器和玩家为None self.player = None self.collision_sensor = None self.lane_invasion_sensor = None self.gnss_sensor = None self.camera_manager = None - self._weather_presets = find_weather_presets() - self._weather_index = 0 - self._actor_filter = args.filter - self._gamma = args.gamma - self.restart(args) - self.world.on_tick(hud.on_world_tick) + self.recording_enabled = False self.recording_start = 0 - def restart(self, args): + self.restart() + self.world.on_tick(hud.on_world_tick) + + def _initialize_map(self): + """初始化地图,处理可能的错误""" + try: + self.map = self.world.get_map() + except RuntimeError as error: + print(f'RuntimeError: {error}') + print('The server could not send the OpenDRIVE (.xodr) file:') + print('Make sure it exists, has the same name of your town, and is correct.') + sys.exit(1) + + def restart(self): """Restart the world""" - # Keep same camera config if the camera manager exists. - cam_index = self.camera_manager.index if self.camera_manager is not None else 0 - cam_pos_id = self.camera_manager.transform_index if self.camera_manager is not None else 0 - # Set the seed if requested by user - if args.seed is not None: - random.seed(args.seed) - - # KK Get a vehicle blueprint - # blueprint = self.world.get_blueprint_library().filter('vehicle.lincoln.mkz2017')[0] # KK - blueprint = self.world.get_blueprint_library().filter('model3')[0] + # 保存相机状态 + cam_index = self.camera_manager.index if self.camera_manager else 0 + cam_pos_id = self.camera_manager.transform_index if self.camera_manager else 0 + + # 设置随机种子 + if self.args.seed is not None: + random.seed(self.args.seed) + + # 创建并配置车辆 + blueprint = self._create_vehicle_blueprint() + + # 生成玩家车辆 + self._spawn_player(blueprint) + + # 设置传感器 + self._setup_sensors(cam_index, cam_pos_id) + + def _create_vehicle_blueprint(self): + """创建并配置车辆蓝图""" + blueprint_lib = self.world.get_blueprint_library() + blueprint = blueprint_lib.filter(self.DEFAULT_VEHICLE_MODEL)[0] blueprint.set_attribute('role_name', 'hero') + if blueprint.has_attribute('color'): - color = '236,102,17' # KK Packt logo color - blueprint.set_attribute('color', color) - # Spawn the player. - print("Spawning the player") + blueprint.set_attribute('color', self.DEFAULT_VEHICLE_COLOR) + + return blueprint + + def _spawn_player(self, blueprint): + """生成玩家车辆""" + # 如果玩家已存在,在当前位置重新生成 if self.player is not None: - spawn_point = self.player.get_transform() - spawn_point.location.z += 2.0 - spawn_point.rotation.roll = 0.0 - spawn_point.rotation.pitch = 0.0 - self.destroy() + spawn_point = self._get_respawn_point() + self.destroy() # 销毁现有actor self.player = self.world.try_spawn_actor(blueprint, spawn_point) - while self.player is None: - if not self.map.get_spawn_points(): - print('There are no spawn points available in your map/town.') - print('Please add some Vehicle Spawn Point to your UE4 scene.') - sys.exit(1) - spawn_points = self.map.get_spawn_points() - spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() - # spawn_point = spawn_points[0] if spawn_points else carla.Transform() # KK + # 首次生成或重新生成失败时 + if self.player is None: + spawn_point = self._get_random_spawn_point() self.player = self.world.try_spawn_actor(blueprint, spawn_point) - # Set up the sensors. + + def _get_respawn_point(self): + """获取重新生成点(当前位置上方)""" + spawn_point = self.player.get_transform() + spawn_point.location.z += 2.0 + spawn_point.rotation.roll = 0.0 + spawn_point.rotation.pitch = 0.0 + return spawn_point + + def _get_random_spawn_point(self): + """获取随机生成点""" + spawn_points = self.map.get_spawn_points() + + if not spawn_points: + print('There are no spawn points available in your map/town.') + print('Please add some Vehicle Spawn Point to your UE4 scene.') + sys.exit(1) + + return random.choice(spawn_points) + + def _setup_sensors(self, cam_index, cam_pos_id): + """设置所有传感器""" self.collision_sensor = CollisionSensor(self.player, self.hud) self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) self.gnss_sensor = GnssSensor(self.player) - self.camera_manager = CameraManager(self.player, self.hud, self._gamma) + + self.camera_manager = CameraManager(self.player, self.hud, self.args.gamma) self.camera_manager.transform_index = cam_pos_id self.camera_manager.set_sensor(cam_index, notify=False) + + # 通知玩家类型 actor_type = get_actor_display_name(self.player) self.hud.notification(actor_type) def next_weather(self, reverse=False): - """Get next weather setting""" + """切换到下一个天气预设""" self._weather_index += -1 if reverse else 1 self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] - self.hud.notification('Weather: %s' % preset[1]) - self.player.get_world().set_weather(preset[0]) + self.hud.notification(f'Weather: {preset[1]}') + self.world.set_weather(preset[0]) def tick(self, clock): - """Method for every tick""" + """每一帧的更新方法""" self.hud.tick(self, clock) def render(self, display): - """Render world""" + """渲染世界""" self.camera_manager.render(display) self.hud.render(display) def destroy_sensors(self): - """Destroy sensors""" - self.camera_manager.sensor.destroy() - self.camera_manager.sensor = None - self.camera_manager.index = None + """销毁所有传感器""" + if self.camera_manager and self.camera_manager.sensor: + self.camera_manager.sensor.destroy() + self.camera_manager.sensor = None + self.camera_manager.index = None def destroy(self): - """Destroys all actors""" + """销毁所有actor""" actors = [ - self.camera_manager.sensor, - self.collision_sensor.sensor, - self.lane_invasion_sensor.sensor, - self.gnss_sensor.sensor, - self.player] + self.camera_manager.sensor if self.camera_manager else None, + self.collision_sensor.sensor if self.collision_sensor else None, + self.lane_invasion_sensor.sensor if self.lane_invasion_sensor else None, + self.gnss_sensor.sensor if self.gnss_sensor else None, + self.player + ] + for actor in actors: if actor is not None: actor.destroy() + # 重置引用 + self.player = None + self.collision_sensor = None + self.lane_invasion_sensor = None + self.gnss_sensor = None + self.camera_manager = None + # ============================================================================== # -- KeyboardControl ----------------------------------------------------------- From a92adb9cc41362edd4888f86fe0fc7b03cc506a1 Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Fri, 19 Dec 2025 15:19:08 +0800 Subject: [PATCH 07/13] =?UTF-8?q?=E6=9B=B4=E6=96=B0readme=E6=96=87?= =?UTF-8?q?=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../README.md | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md index 202373667b..aa571532a4 100644 --- a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md @@ -29,4 +29,10 @@ outdoor_ws/src/robot_description/ - 封装激光雷达、IMU、GNSS和轮式编码器的数据采集 - 实现多传感器时间同步(10ms容差) - 提供线程安全的数据队列 - - 包含传感器配置管理和模拟器(用于测试) \ No newline at end of file + - 包含传感器配置管理和模拟器(用于测试) +2. **实时估计机器人运动位姿** + - IMU预积分补偿激光雷达运动畸变 + - 从点云提取几何特征(地面/平面/边缘点) + - 扫描匹配(ICP)估计相对运动 + - 融合IMU、激光雷达和GNSS数据 + - 输出高频位姿估计(前端里程计) From 3b3f9b8e1d3180be32e65a8f5438a6529e25c98e Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Fri, 19 Dec 2025 15:52:37 +0800 Subject: [PATCH 08/13] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=9B=A0=E5=AD=90?= =?UTF-8?q?=E5=9B=BE=E4=BC=98=E5=8C=96=E5=90=8E=E7=AB=AF=E6=A8=A1=E5=9D=97?= =?UTF-8?q?=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../factor_graph_backend.py | 1194 +++++++++++++++++ 1 file changed, 1194 insertions(+) create mode 100644 src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/factor_graph_backend.py diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/factor_graph_backend.py b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/factor_graph_backend.py new file mode 100644 index 0000000000..bc3ca19d57 --- /dev/null +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/factor_graph_backend.py @@ -0,0 +1,1194 @@ +# factor_graph_backend.py +import numpy as np +from dataclasses import dataclass +from typing import List, Dict, Tuple, Optional, Any +import time +from collections import deque +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import matplotlib.animation as animation +from matplotlib.patches import FancyArrowPatch +from mpl_toolkits.mplot3d import proj3d +# 在 factor_graph_backend.py 文件开头,import语句后面添加 + +import matplotlib.pyplot as plt + +# 设置中文字体 +try: + # Windows系统字体路径 + plt.rcParams['font.sans-serif'] = ['SimHei', 'Microsoft YaHei', 'SimSun'] + # Mac系统字体路径 + # plt.rcParams['font.sans-serif'] = ['Arial Unicode MS', 'STHeiti', 'Heiti SC'] + plt.rcParams['axes.unicode_minus'] = False # 正确显示负号 +except: + print("警告:无法加载中文字体,将使用默认字体") + pass + +# 设置matplotlib默认参数 +plt.rcParams['figure.figsize'] = [10, 8] +plt.rcParams['figure.dpi'] = 100 +plt.rcParams['savefig.dpi'] = 150 +plt.rcParams['font.size'] = 10 +plt.rcParams['legend.fontsize'] = 9 +plt.rcParams['axes.titlesize'] = 12 +plt.rcParams['axes.labelsize'] = 10 + + +@dataclass +class Factor: + """因子基类""" + factor_type: str + nodes: List[int] # 关联的节点ID + measurement: np.ndarray + information: np.ndarray # 信息矩阵 + + +@dataclass +class Node: + """位姿节点""" + node_id: int + timestamp: float + pose: np.ndarray # 4x4变换矩阵 + fixed: bool = False + + +class FactorGraphVisualizer: + """因子图可视化类""" + + def __init__(self, backend: 'FactorGraphBackend'): + self.backend = backend + self.fig = None + self.ax = None + + def plot_trajectory_2d(self, show_factor_graph: bool = True, + show_submaps: bool = True, + show_loop_closures: bool = True, + save_path: Optional[str] = None): + """绘制2D轨迹图""" + if len(self.backend.nodes) == 0: + print("没有节点可显示") + return + + # 创建图形 + self.fig, (self.ax1, self.ax2) = plt.subplots(1, 2, figsize=(15, 7)) + + # 绘制轨迹 + self._plot_trajectory_2d_on_axis(self.ax1, "优化前轨迹", show_factor_graph, show_submaps, show_loop_closures) + + # 如果有优化结果,绘制优化后的轨迹 + if hasattr(self.backend, 'optimized_poses') and self.backend.optimized_poses: + self._plot_optimized_trajectory_2d(self.ax2) + else: + # 否则绘制节点和因子图 + self._plot_trajectory_2d_on_axis(self.ax2, "节点和因子图", True, False, False) + + plt.tight_layout() + + if save_path: + plt.savefig(save_path, dpi=150, bbox_inches='tight') + print(f"轨迹图已保存到 {save_path}") + + plt.show() + + def _plot_trajectory_2d_on_axis(self, ax, title: str, + show_factor_graph: bool, + show_submaps: bool, + show_loop_closures: bool): + """在指定轴上绘制轨迹""" + # 提取轨迹点 + x_coords = [] + y_coords = [] + timestamps = [] + + for node_id in sorted(self.backend.nodes.keys()): + node = self.backend.nodes[node_id] + pos = node.pose[:3, 3] + x_coords.append(pos[0]) + y_coords.append(pos[1]) + timestamps.append(node.timestamp) + + # 绘制轨迹线 + ax.plot(x_coords, y_coords, 'b-', alpha=0.5, linewidth=1, label='轨迹') + + # 绘制节点点 + ax.scatter(x_coords, y_coords, c=timestamps, cmap='viridis', + s=20, alpha=0.7, label='节点') + + # 绘制固定节点(如果有) + fixed_x = [] + fixed_y = [] + for node_id, node in self.backend.nodes.items(): + if node.fixed: + pos = node.pose[:3, 3] + fixed_x.append(pos[0]) + fixed_y.append(pos[1]) + + if fixed_x: + ax.scatter(fixed_x, fixed_y, c='red', s=100, + marker='*', label='固定节点', zorder=5) + + # 绘制因子图连接 + if show_factor_graph: + self._plot_factor_connections_2d(ax) + + # 绘制子图 + if show_submaps: + self._plot_submaps_2d(ax) + + # 绘制闭环 + if show_loop_closures: + self._plot_loop_closures_2d(ax) + + ax.set_xlabel('X (米)') + ax.set_ylabel('Y (米)') + ax.set_title(title) + ax.grid(True, alpha=0.3) + ax.legend() + ax.axis('equal') + + def _plot_factor_connections_2d(self, ax): + """绘制因子连接""" + # 定义颜色映射 + factor_colors = { + 'odometry': 'green', + 'gnss': 'blue', + 'loop_closure': 'red' + } + + # 绘制因子连接 + for factor in self.backend.factors: + if len(factor.nodes) >= 2: + color = factor_colors.get(factor.factor_type, 'gray') + linewidth = 1.0 + alpha = 0.3 + + # 不同类型因子使用不同样式 + if factor.factor_type == 'loop_closure': + linewidth = 1.5 + alpha = 0.8 + + # 获取节点位置 + positions = [] + for node_id in factor.nodes: + if node_id in self.backend.nodes: + pos = self.backend.nodes[node_id].pose[:3, 3] + positions.append(pos[:2]) # 只取XY + + if len(positions) >= 2: + # 绘制连接线 + for i in range(len(positions) - 1): + ax.plot([positions[i][0], positions[i + 1][0]], + [positions[i][1], positions[i + 1][1]], + color=color, linewidth=linewidth, + alpha=alpha, linestyle='--') + + def _plot_submaps_2d(self, ax): + """绘制子图区域""" + for i, submap in enumerate(self.backend.submaps): + if submap["nodes"]: + # 提取子图节点位置 + positions = [] + for node_id in submap["nodes"]: + if node_id in self.backend.nodes: + pos = self.backend.nodes[node_id].pose[:3, 3] + positions.append(pos[:2]) + + if positions: + positions = np.array(positions) + # 计算边界框 + x_min, y_min = positions.min(axis=0) + x_max, y_max = positions.max(axis=0) + + # 绘制矩形框 + rect = plt.Rectangle((x_min, y_min), + x_max - x_min, + y_max - y_min, + fill=False, + edgecolor='orange', + linewidth=1, + alpha=0.5, + linestyle=':') + ax.add_patch(rect) + + # 添加子图编号 + ax.text(x_min, y_min, f'Submap {i}', + fontsize=8, color='orange', + bbox=dict(boxstyle="round,pad=0.3", + facecolor="white", + alpha=0.7)) + + def _plot_loop_closures_2d(self, ax): + """绘制闭环连接""" + # 从因子中提取闭环 + for factor in self.backend.factors: + if factor.factor_type == 'loop_closure' and len(factor.nodes) >= 2: + node_i, node_j = factor.nodes[:2] + if node_i in self.backend.nodes and node_j in self.backend.nodes: + pos_i = self.backend.nodes[node_i].pose[:3, 3][:2] + pos_j = self.backend.nodes[node_j].pose[:3, 3][:2] + + # 绘制闭环连接线 + ax.plot([pos_i[0], pos_j[0]], + [pos_i[1], pos_j[1]], + color='red', linewidth=2, + alpha=0.8, linestyle='-', + label='闭环' if not ax.get_legend() else '') + + def _plot_optimized_trajectory_2d(self, ax): + """绘制优化后的轨迹""" + # 提取优化前后的轨迹 + orig_x, orig_y = [], [] + opt_x, opt_y = [], [] + + for node_id in sorted(self.backend.nodes.keys()): + # 原始位置 + orig_pos = self.backend.nodes[node_id].pose[:3, 3] + orig_x.append(orig_pos[0]) + orig_y.append(orig_pos[1]) + + # 优化后位置 + if node_id in self.backend.optimized_poses: + opt_pos = self.backend.optimized_poses[node_id][:3, 3] + opt_x.append(opt_pos[0]) + opt_y.append(opt_pos[1]) + + # 绘制轨迹 + ax.plot(orig_x, orig_y, 'b-', alpha=0.3, linewidth=1, label='优化前') + ax.plot(opt_x, opt_y, 'r-', alpha=0.8, linewidth=2, label='优化后') + + # 绘制节点 + ax.scatter(orig_x, orig_y, c='blue', s=20, alpha=0.3, label='原节点') + ax.scatter(opt_x, opt_y, c='red', s=30, alpha=0.6, label='优化节点') + + # 绘制修正箭头 + for i in range(len(orig_x)): + if i < len(opt_x): + ax.arrow(orig_x[i], orig_y[i], + opt_x[i] - orig_x[i], + opt_y[i] - orig_y[i], + head_width=0.1, head_length=0.2, + fc='green', ec='green', alpha=0.3) + + ax.set_xlabel('X (米)') + ax.set_ylabel('Y (米)') + ax.set_title('优化前后轨迹对比') + ax.grid(True, alpha=0.3) + ax.legend() + ax.axis('equal') + + def plot_3d_trajectory(self, show_poses: bool = False, + save_path: Optional[str] = None): + """绘制3D轨迹图""" + if len(self.backend.nodes) == 0: + print("没有节点可显示") + return + + self.fig = plt.figure(figsize=(12, 10)) + self.ax = self.fig.add_subplot(111, projection='3d') + + # 提取轨迹点 + x_coords = [] + y_coords = [] + z_coords = [] + timestamps = [] + + for node_id in sorted(self.backend.nodes.keys()): + node = self.backend.nodes[node_id] + pos = node.pose[:3, 3] + x_coords.append(pos[0]) + y_coords.append(pos[1]) + z_coords.append(pos[2]) + timestamps.append(node.timestamp) + + # 绘制轨迹线 + self.ax.plot(x_coords, y_coords, z_coords, 'b-', alpha=0.5, linewidth=1) + + # 绘制节点点 + scatter = self.ax.scatter(x_coords, y_coords, z_coords, + c=timestamps, cmap='viridis', + s=20, alpha=0.7) + + # 绘制位姿方向(可选) + if show_poses: + self._plot_3d_poses() + + # 绘制因子连接 + self._plot_3d_factor_connections() + + # 添加颜色条 + cbar = self.fig.colorbar(scatter, ax=self.ax, shrink=0.5, aspect=5) + cbar.set_label('时间戳') + + self.ax.set_xlabel('X (米)') + self.ax.set_ylabel('Y (米)') + self.ax.set_zlabel('Z (米)') + self.ax.set_title('3D轨迹图') + + # 设置视角 + self.ax.view_init(elev=30, azim=45) + + plt.tight_layout() + + if save_path: + plt.savefig(save_path, dpi=150, bbox_inches='tight') + print(f"3D轨迹图已保存到 {save_path}") + + plt.show() + + def _plot_3d_poses(self): + """在3D图中绘制位姿方向""" + for node_id in sorted(self.backend.nodes.keys()): + if node_id % 5 == 0: # 每5个节点绘制一个方向 + node = self.backend.nodes[node_id] + pose = node.pose + + # 位置 + pos = pose[:3, 3] + + # X方向(红色) + x_dir = pose[:3, 0] + self.ax.quiver(pos[0], pos[1], pos[2], + x_dir[0] * 0.5, x_dir[1] * 0.5, x_dir[2] * 0.5, + color='r', linewidth=1, alpha=0.6) + + # Y方向(绿色) + y_dir = pose[:3, 1] + self.ax.quiver(pos[0], pos[1], pos[2], + y_dir[0] * 0.5, y_dir[1] * 0.5, y_dir[2] * 0.5, + color='g', linewidth=1, alpha=0.6) + + # Z方向(蓝色) + z_dir = pose[:3, 2] + self.ax.quiver(pos[0], pos[1], pos[2], + z_dir[0] * 0.5, z_dir[1] * 0.5, z_dir[2] * 0.5, + color='b', linewidth=1, alpha=0.6) + + def _plot_3d_factor_connections(self): + """在3D图中绘制因子连接""" + factor_colors = { + 'odometry': 'green', + 'gnss': 'blue', + 'loop_closure': 'red' + } + + for factor in self.backend.factors: + if len(factor.nodes) >= 2: + color = factor_colors.get(factor.factor_type, 'gray') + alpha = 0.3 + linewidth = 1 + + if factor.factor_type == 'loop_closure': + alpha = 0.8 + linewidth = 2 + + # 获取节点位置 + positions = [] + for node_id in factor.nodes: + if node_id in self.backend.nodes: + pos = self.backend.nodes[node_id].pose[:3, 3] + positions.append(pos) + + if len(positions) >= 2: + for i in range(len(positions) - 1): + x = [positions[i][0], positions[i + 1][0]] + y = [positions[i][1], positions[i + 1][1]] + z = [positions[i][2], positions[i + 1][2]] + + self.ax.plot(x, y, z, color=color, + linewidth=linewidth, alpha=alpha, + linestyle='--') + + def plot_error_evolution(self, save_path: Optional[str] = None): + """绘制误差演化图""" + if not hasattr(self.backend, 'optimization_history'): + print("没有优化历史数据") + return + + history = self.backend.optimization_history + + self.fig, axes = plt.subplots(2, 2, figsize=(12, 10)) + + # 总误差 + if 'total_errors' in history: + ax = axes[0, 0] + ax.plot(history['total_errors'], 'b-', linewidth=2) + ax.set_xlabel('迭代次数') + ax.set_ylabel('总误差') + ax.set_title('总误差收敛曲线') + ax.grid(True, alpha=0.3) + + # 位置误差 + if 'position_errors' in history: + ax = axes[0, 1] + ax.plot(history['position_errors'], 'g-', linewidth=2) + ax.set_xlabel('迭代次数') + ax.set_ylabel('位置误差 (米)') + ax.set_title('位置误差收敛曲线') + ax.grid(True, alpha=0.3) + + # 旋转误差 + if 'rotation_errors' in history: + ax = axes[1, 0] + ax.plot(history['rotation_errors'], 'r-', linewidth=2) + ax.set_xlabel('迭代次数') + ax.set_ylabel('旋转误差 (弧度)') + ax.set_title('旋转误差收敛曲线') + ax.grid(True, alpha=0.3) + + # 梯度范数 + if 'gradient_norms' in history: + ax = axes[1, 1] + ax.plot(history['gradient_norms'], 'm-', linewidth=2) + ax.set_xlabel('迭代次数') + ax.set_ylabel('梯度范数') + ax.set_title('梯度范数变化') + ax.grid(True, alpha=0.3) + ax.set_yscale('log') + + plt.tight_layout() + + if save_path: + plt.savefig(save_path, dpi=150, bbox_inches='tight') + print(f"误差演化图已保存到 {save_path}") + + plt.show() + + def animate_trajectory(self, interval: float = 100, + save_path: Optional[str] = None): + """创建轨迹动画""" + if len(self.backend.nodes) < 2: + print("节点数量不足,无法创建动画") + return + + self.fig, self.ax = plt.subplots(figsize=(10, 8)) + + # 提取所有位置 + x_coords = [] + y_coords = [] + node_ids = sorted(self.backend.nodes.keys()) + + for node_id in node_ids: + pos = self.backend.nodes[node_id].pose[:3, 3] + x_coords.append(pos[0]) + y_coords.append(pos[1]) + + # 初始化动画元素 + line, = self.ax.plot([], [], 'b-', linewidth=2, alpha=0.8) + point, = self.ax.plot([], [], 'ro', markersize=8, alpha=0.8) + timestamp_text = self.ax.text(0.05, 0.95, '', transform=self.ax.transAxes) + node_text = self.ax.text(0.05, 0.90, '', transform=self.ax.transAxes) + + # 设置图形范围 + margin = 5.0 + self.ax.set_xlim(min(x_coords) - margin, max(x_coords) + margin) + self.ax.set_ylim(min(y_coords) - margin, max(y_coords) + margin) + self.ax.set_xlabel('X (米)') + self.ax.set_ylabel('Y (米)') + self.ax.set_title('轨迹生成动画') + self.ax.grid(True, alpha=0.3) + self.ax.axis('equal') + + def init(): + """初始化动画""" + line.set_data([], []) + point.set_data([], []) + timestamp_text.set_text('') + node_text.set_text('') + return line, point, timestamp_text, node_text + + def animate(i): + """动画更新函数""" + # 更新轨迹线 + line.set_data(x_coords[:i + 1], y_coords[:i + 1]) + + # 更新当前点 + if i < len(x_coords): + point.set_data([x_coords[i]], [y_coords[i]]) + + # 更新文本 + if i < len(node_ids): + node = self.backend.nodes[node_ids[i]] + timestamp_text.set_text(f'时间戳: {node.timestamp:.2f}s') + node_text.set_text(f'节点ID: {node.node_id}') + + return line, point, timestamp_text, node_text + + # 创建动画 + anim = animation.FuncAnimation(self.fig, animate, init_func=init, + frames=len(node_ids), + interval=interval, blit=True) + + plt.tight_layout() + + # 保存动画 + if save_path: + anim.save(save_path, writer='ffmpeg', fps=30) + print(f"轨迹动画已保存到 {save_path}") + + plt.show() + return anim + + +class FactorGraphBackend: + """因子图优化后端(带可视化功能)""" + + def __init__(self, config=None): + self.config = config or self._default_config() + + # 图结构 + self.nodes = {} # node_id -> Node + self.factors = [] # List[Factor] + + # 子图管理 + self.submaps = [] + self.current_submap = None + + # 优化参数 + self.optimization_interval = 10 # 每10帧优化一次 + self.frame_count = 0 + + # GNSS融合 + self.gnss_reference = None # 局部坐标原点 + + # 优化历史 + self.optimization_history = { + 'total_errors': [], + 'position_errors': [], + 'rotation_errors': [], + 'gradient_norms': [] + } + + # 可视化器 + self.visualizer = FactorGraphVisualizer(self) + + # 优化结果缓存 + self.optimized_poses = {} + + def _default_config(self): + return { + "optimization": { + "max_iterations": 50, + "convergence_threshold": 1e-6, + "use_sparse": True + }, + "loop_closure": { + "search_radius": 10.0, # 米 + "min_chain_length": 10, + "score_threshold": 0.5 + }, + "submap": { + "size": 100, # 每100帧一个子图 + "overlap": 20 # 子图间重叠帧数 + }, + "visualization": { + "show_trajectory": True, + "show_factors": True, + "show_submaps": True, + "show_loop_closures": True + } + } + + def add_node(self, pose: np.ndarray, timestamp: float, + node_type: str = "odometry", fixed: bool = False) -> int: + """添加位姿节点""" + node_id = len(self.nodes) + node = Node( + node_id=node_id, + timestamp=timestamp, + pose=pose.copy(), + fixed=fixed + ) + self.nodes[node_id] = node + + # 添加到当前子图 + if self.current_submap is None: + self.current_submap = { + "start_id": node_id, + "end_id": node_id, + "nodes": [node_id], + "features": [] + } + else: + self.current_submap["nodes"].append(node_id) + self.current_submap["end_id"] = node_id + + # 检查子图是否完成 + if len(self.current_submap["nodes"]) >= self.config["submap"]["size"]: + self.submaps.append(self.current_submap) + # 创建新子图,保持重叠 + overlap_nodes = self.current_submap["nodes"][-self.config["submap"]["overlap"]:] + self.current_submap = { + "start_id": overlap_nodes[0], + "end_id": node_id, + "nodes": overlap_nodes.copy(), + "features": [] + } + + return node_id + + def add_odometry_factor(self, node_i: int, node_j: int, + relative_pose: np.ndarray, covariance: np.ndarray): + """添加里程计因子""" + info_matrix = np.linalg.inv(covariance) + + factor = Factor( + factor_type="odometry", + nodes=[node_i, node_j], + measurement=relative_pose, + information=info_matrix + ) + self.factors.append(factor) + + def add_gnss_factor(self, node_id: int, gnss_position: np.ndarray, + covariance: np.ndarray): + """添加GNSS因子""" + if self.gnss_reference is None: + self.gnss_reference = gnss_position.copy() + + # 转换为局部坐标 + local_pos = gnss_position - self.gnss_reference + + # 创建位置测量(只有位置,没有姿态) + measurement = np.eye(4) + measurement[:3, 3] = local_pos + + info_matrix = np.linalg.inv(covariance) + + factor = Factor( + factor_type="gnss", + nodes=[node_id], + measurement=measurement, + information=info_matrix + ) + self.factors.append(factor) + + def add_loop_closure_factor(self, node_i: int, node_j: int, + relative_pose: np.ndarray, covariance: np.ndarray): + """添加闭环因子""" + info_matrix = np.linalg.inv(covariance) + + factor = Factor( + factor_type="loop_closure", + nodes=[node_i, node_j], + measurement=relative_pose, + information=info_matrix + ) + self.factors.append(factor) + + def detect_loop_closure(self, current_node_id: int, features: np.ndarray) -> List[Tuple[int, np.ndarray]]: + """检测闭环""" + if len(self.submaps) < 2: + return [] + + loops = [] + current_node = self.nodes[current_node_id] + current_pos = current_node.pose[:3, 3] + + # 只在历史子图中搜索 + for submap in self.submaps[:-1]: # 排除当前子图 + # 距离检查 + submap_center = self._get_submap_center(submap) + distance = np.linalg.norm(current_pos - submap_center) + + if distance < self.config["loop_closure"]["search_radius"]: + # 特征匹配 + for node_id in submap["nodes"]: + if node_id == current_node_id: + continue + + # 时间间隔检查(避免匹配太近的帧) + time_diff = abs(current_node.timestamp - self.nodes[node_id].timestamp) + if time_diff < 5.0: # 5秒内不闭环 + continue + + # 特征匹配(简化版本) + match_score = self._feature_matching_score(features, submap["features"]) + + if match_score > self.config["loop_closure"]["score_threshold"]: + # 计算相对位姿 + relative_pose = self._compute_relative_pose( + current_node.pose, + self.nodes[node_id].pose + ) + loops.append((node_id, relative_pose)) + + return loops + + def _feature_matching_score(self, features1, features2): + """计算特征匹配分数(简化版)""" + if len(features1) == 0 or len(features2) == 0: + return 0.0 + + # 在实际系统中,这里应该使用更复杂的特征描述子匹配 + # 这里返回一个随机分数作为示例 + return np.random.uniform(0, 1) + + def _compute_relative_pose(self, pose_i, pose_j): + """计算相对位姿""" + return np.linalg.inv(pose_j) @ pose_i + + def _get_submap_center(self, submap): + """获取子图中心位置""" + positions = [] + for node_id in submap["nodes"]: + if node_id in self.nodes: + positions.append(self.nodes[node_id].pose[:3, 3]) + + if positions: + return np.mean(positions, axis=0) + else: + return np.zeros(3) + + def plot_statistics(self): + """绘制统计信息""" + if len(self.nodes) == 0: + print("没有数据可统计") + return + + fig, axes = plt.subplots(2, 3, figsize=(15, 10)) + + # 1. 节点数量统计 + ax = axes[0, 0] + factor_types = {} + for factor in self.factors: + factor_types[factor.factor_type] = factor_types.get(factor.factor_type, 0) + 1 + + if factor_types: + colors = plt.cm.Set3(np.linspace(0, 1, len(factor_types))) + ax.pie(factor_types.values(), labels=factor_types.keys(), + autopct='%1.1f%%', colors=colors, startangle=90) + ax.set_title('因子类型分布') + else: + ax.text(0.5, 0.5, '无因子数据', ha='center', va='center') + ax.set_title('因子类型分布') + + # 2. 轨迹长度统计 + ax = axes[0, 1] + distances = [] + prev_pos = None + for node_id in sorted(self.nodes.keys()): + pos = self.nodes[node_id].pose[:3, 3] + if prev_pos is not None: + distances.append(np.linalg.norm(pos - prev_pos)) + prev_pos = pos + + if distances: + ax.hist(distances, bins=20, alpha=0.7, color='skyblue', edgecolor='black') + ax.set_xlabel('帧间距离 (米)') + ax.set_ylabel('频次') + ax.set_title('帧间距离分布') + else: + ax.text(0.5, 0.5, '无距离数据', ha='center', va='center') + ax.set_title('帧间距离分布') + + # 3. 时间间隔统计 + ax = axes[0, 2] + time_intervals = [] + timestamps = sorted([node.timestamp for node in self.nodes.values()]) + + if len(timestamps) > 1: + for i in range(1, len(timestamps)): + time_intervals.append(timestamps[i] - timestamps[i - 1]) + + ax.plot(time_intervals, 'g-', alpha=0.7) + ax.fill_between(range(len(time_intervals)), 0, time_intervals, + alpha=0.3, color='green') + ax.set_xlabel('帧序号') + ax.set_ylabel('时间间隔 (秒)') + ax.set_title('时间间隔变化') + ax.grid(True, alpha=0.3) + else: + ax.text(0.5, 0.5, '无时间间隔数据', ha='center', va='center') + ax.set_title('时间间隔变化') + + # 4. 节点分布统计 + ax = axes[1, 0] + if len(self.nodes) > 0: + node_ids = list(self.nodes.keys()) + node_types = ['固定' if node.fixed else '非固定' for node in self.nodes.values()] + + fixed_count = sum(1 for node in self.nodes.values() if node.fixed) + non_fixed_count = len(self.nodes) - fixed_count + + ax.bar(['固定节点', '非固定节点'], [fixed_count, non_fixed_count], + color=['red', 'blue'], alpha=0.7) + ax.set_ylabel('数量') + ax.set_title('节点类型分布') + ax.grid(True, alpha=0.3, axis='y') + else: + ax.text(0.5, 0.5, '无节点数据', ha='center', va='center') + ax.set_title('节点类型分布') + + # 5. 子图统计 + ax = axes[1, 1] + if self.submaps: + submap_sizes = [len(submap['nodes']) for submap in self.submaps] + submap_ids = list(range(len(self.submaps))) + + ax.bar(submap_ids, submap_sizes, color='orange', alpha=0.7) + ax.set_xlabel('子图编号') + ax.set_ylabel('节点数量') + ax.set_title('子图大小分布') + ax.grid(True, alpha=0.3, axis='y') + else: + ax.text(0.5, 0.5, '无子图数据', ha='center', va='center') + ax.set_title('子图大小分布') + + # 6. 优化历史统计(如果有) + ax = axes[1, 2] + if hasattr(self, 'optimization_history') and self.optimization_history.get('total_errors'): + iterations = list(range(len(self.optimization_history['total_errors']))) + ax.plot(iterations, self.optimization_history['total_errors'], + 'b-', linewidth=2, label='总误差') + + if self.optimization_history.get('position_errors'): + ax.plot(iterations, self.optimization_history['position_errors'], + 'g-', linewidth=2, label='位置误差', alpha=0.7) + + if self.optimization_history.get('rotation_errors'): + ax.plot(iterations, self.optimization_history['rotation_errors'], + 'r-', linewidth=2, label='旋转误差', alpha=0.7) + + ax.set_xlabel('迭代次数') + ax.set_ylabel('误差') + ax.set_title('优化收敛曲线') + ax.legend() + ax.grid(True, alpha=0.3) + ax.set_yscale('log') + else: + ax.text(0.5, 0.5, '无优化历史数据', ha='center', va='center') + ax.set_title('优化收敛曲线') + + plt.suptitle('因子图统计信息', fontsize=16, fontweight='bold') + plt.tight_layout() + + # 保存图片 + plt.savefig('factor_graph_statistics.png', dpi=150, bbox_inches='tight') + print("统计图已保存为 'factor_graph_statistics.png'") + + plt.show() + def optimize(self): + """执行图优化""" + print(f"开始优化,节点数: {len(self.nodes)},因子数: {len(self.factors)}") + + # 清空历史记录 + self.optimization_history = { + 'total_errors': [], + 'position_errors': [], + 'rotation_errors': [], + 'gradient_norms': [] + } + + # 简化的优化实现 + # 在实际系统中,应该使用g2o、GTSAM或Ceres等优化库 + + # 这里实现一个简单的梯度下降优化 + for iteration in range(self.config["optimization"]["max_iterations"]): + total_error = 0.0 + position_error = 0.0 + rotation_error = 0.0 + + # 计算每个节点的梯度 + gradients = {node_id: np.zeros(6) for node_id in self.nodes} + + for factor in self.factors: + error, jacobians = self._compute_factor_error(factor) + total_error += np.sum(error ** 2) + + # 计算位置和旋转误差 + if factor.factor_type == 'odometry': + position_error += np.linalg.norm(error[3:]) + rotation_error += np.linalg.norm(error[:3]) + + # 更新梯度 + for i, node_id in enumerate(factor.nodes): + if node_id in gradients: + gradients[node_id] += jacobians[i] @ error + + # 记录历史 + self.optimization_history['total_errors'].append(total_error) + self.optimization_history['position_errors'].append(position_error) + self.optimization_history['rotation_errors'].append(rotation_error) + + # 计算平均梯度范数 + grad_norm = 0.0 + for grad in gradients.values(): + grad_norm += np.linalg.norm(grad) + grad_norm /= len(gradients) + self.optimization_history['gradient_norms'].append(grad_norm) + + # 更新节点位姿 + for node_id, gradient in gradients.items(): + if not self.nodes[node_id].fixed: + # 将梯度转换为位姿更新 + delta_pose = self._gradient_to_pose(gradient, 0.01) + self.nodes[node_id].pose = delta_pose @ self.nodes[node_id].pose + + # 检查收敛 + if total_error < self.config["optimization"]["convergence_threshold"]: + print(f"优化在第{iteration}次迭代收敛") + break + + print(f"优化完成,最终误差: {total_error}") + + # 保存优化结果 + self.optimized_poses = {} + for node_id, node in self.nodes.items(): + self.optimized_poses[node_id] = node.pose.copy() + + # 可视化优化结果 + if self.config["visualization"]["show_trajectory"]: + self.visualize() + + return self.optimized_poses + + def _compute_factor_error(self, factor): + """计算因子误差(简化实现)""" + # 这里应该根据因子类型计算不同的误差 + # 简化实现返回零误差和雅可比 + error = np.zeros(6) + jacobians = [np.eye(6) for _ in factor.nodes] + + return error, jacobians + + def _gradient_to_pose(self, gradient, step_size): + """将梯度转换为位姿更新""" + # 将6维梯度转换为SE(3)变换 + delta_pose = np.eye(4) + + # 旋转部分(前3维) + rotation_vec = gradient[:3] * step_size + if np.linalg.norm(rotation_vec) > 0: + from scipy.spatial.transform import Rotation + delta_rotation = Rotation.from_rotvec(rotation_vec).as_matrix() + delta_pose[:3, :3] = delta_rotation + + # 平移部分(后3维) + delta_pose[:3, 3] = gradient[3:] * step_size + + return delta_pose + + def get_optimized_trajectory(self): + """获取优化后的轨迹""" + trajectory = [] + for node_id in sorted(self.nodes.keys()): + node = self.nodes[node_id] + trajectory.append({ + "timestamp": node.timestamp, + "pose": node.pose, + "position": node.pose[:3, 3] + }) + return trajectory + + def save_map(self, filename): + """保存地图""" + import pickle + + map_data = { + "nodes": self.nodes, + "submaps": self.submaps, + "config": self.config, + "gnss_reference": self.gnss_reference, + "optimized_poses": self.optimized_poses, + "optimization_history": self.optimization_history + } + + with open(filename, 'wb') as f: + pickle.dump(map_data, f) + + print(f"地图已保存到 {filename}") + + def load_map(self, filename): + """加载地图""" + import pickle + + with open(filename, 'rb') as f: + map_data = pickle.load(f) + + self.nodes = map_data["nodes"] + self.submaps = map_data["submaps"] + self.config = map_data["config"] + self.gnss_reference = map_data.get("gnss_reference") + self.optimized_poses = map_data.get("optimized_poses", {}) + self.optimization_history = map_data.get("optimization_history", {}) + + print(f"已加载地图,包含 {len(self.nodes)} 个节点") + + # ==================== 可视化方法 ==================== + + def visualize(self, show_2d: bool = True, show_3d: bool = False, + show_errors: bool = True, show_animation: bool = False, + save_prefix: Optional[str] = None): + """综合可视化功能""" + + if show_2d: + save_path = f"{save_prefix}_2d.png" if save_prefix else None + self.visualizer.plot_trajectory_2d( + show_factor_graph=self.config["visualization"]["show_factors"], + show_submaps=self.config["visualization"]["show_submaps"], + show_loop_closures=self.config["visualization"]["show_loop_closures"], + save_path=save_path + ) + + if show_3d: + save_path = f"{save_prefix}_3d.png" if save_prefix else None + self.visualizer.plot_3d_trajectory(save_path=save_path) + + if show_errors and hasattr(self, 'optimization_history'): + save_path = f"{save_prefix}_errors.png" if save_prefix else None + self.visualizer.plot_error_evolution(save_path=save_path) + + if show_animation: + save_path = f"{save_prefix}_animation.mp4" if save_prefix else None + self.visualizer.animate_trajectory(save_path=save_path) + + def plot_statistics(self): + """绘制统计信息""" + if len(self.nodes) == 0: + print("没有数据可统计") + return + + fig, axes = plt.subplots(2, 3, figsize=(15, 10)) + + # 1. 节点数量统计 + ax = axes[0, 0] + factor_types = {} + for factor in self.factors: + factor_types[factor.factor_type] = factor_types.get(factor.factor_type, 0) + 1 + + colors = plt.cm.Set3(np.linspace(0, 1, len(factor_types))) + ax.pie(factor_types.values(), labels=factor_types.keys(), + autopct='%1.1f%%', colors=colors, startangle=90) + ax.set_title('因子类型分布') + + # 2. 轨迹长度统计 + ax = axes[0, 1] + distances = [] + prev_pos = None + for node_id in sorted(self.nodes.keys()): + pos = self.nodes[node_id].pose[:3, 3] + if prev_pos is not None: + distances.append(np.linalg.norm(pos - prev_pos)) + prev_pos = pos + + if distances: + ax.hist(distances, bins=20, alpha=0.7, color='skyblue', edgecolor='black') + ax.set_xlabel('帧间距离 (米)') + ax.set_ylabel('频次') + ax.set_title('帧间距离分布') + + +# 在 factor_graph_backend.py 文件末尾添加以下代码 + +def main(): + """示例演示函数""" + print("因子图后端可视化演示") + + # 创建因子图后端 + backend = FactorGraphBackend() + + # 添加一些示例节点和因子 + import random + + # 生成一个简单的圆形轨迹 + num_nodes = 50 + radius = 10.0 + center_x, center_y = 0, 0 + + for i in range(num_nodes): + # 生成圆形轨迹上的位姿 + angle = 2 * np.pi * i / num_nodes + + # 位置 + x = center_x + radius * np.cos(angle) + y = center_y + radius * np.sin(angle) + z = 0.0 + + # 姿态(朝向切线方向) + from scipy.spatial.transform import Rotation + + # 切线方向 + dx = -np.sin(angle) + dy = np.cos(angle) + + # 创建旋转矩阵 + rotation = Rotation.from_euler('z', angle).as_matrix() + + # 创建位姿矩阵 + pose = np.eye(4) + pose[:3, :3] = rotation + pose[:3, 3] = [x, y, z] + + # 添加节点 + node_id = backend.add_node(pose, timestamp=i) + + # 添加里程计因子 + if i > 0: + # 计算相对位姿 + prev_pose = backend.nodes[i - 1].pose + relative_pose = np.linalg.inv(prev_pose) @ pose + + # 添加里程计因子 + covariance = np.eye(6) * 0.1 # 协方差矩阵 + backend.add_odometry_factor(i - 1, i, relative_pose, covariance) + + # 添加一些GNSS因子 + for i in range(0, num_nodes, 5): + gnss_pos = backend.nodes[i].pose[:3, 3] + np.random.randn(3) * 0.5 + covariance = np.eye(3) * 1.0 + backend.add_gnss_factor(i, gnss_pos, covariance) + + # 添加一个闭环因子(模拟回环检测) + if num_nodes > 20: + # 连接起点和终点附近的一个点 + loop_i = 0 + loop_j = num_nodes - 10 + + pose_i = backend.nodes[loop_i].pose + pose_j = backend.nodes[loop_j].pose + relative_pose = np.linalg.inv(pose_j) @ pose_i + + covariance = np.eye(6) * 0.05 + backend.add_loop_closure_factor(loop_i, loop_j, relative_pose, covariance) + print(f"添加闭环因子: 节点 {loop_i} <-> 节点 {loop_j}") + + # 创建一个子图(模拟子图分割) + backend.submaps.append({ + "start_id": 0, + "end_id": 19, + "nodes": list(range(20)), + "features": [] + }) + + backend.submaps.append({ + "start_id": 20, + "end_id": 39, + "nodes": list(range(20, 40)), + "features": [] + }) + + # 执行优化 + print("执行优化...") + optimized_poses = backend.optimize() + print(f"优化完成,优化了 {len(optimized_poses)} 个位姿") + + # 显示所有可视化 + print("显示可视化...") + + # 1. 2D轨迹图 + backend.visualize(show_2d=True, show_3d=False, show_errors=True, + save_prefix="factor_graph_demo") + + # 2. 3D轨迹图 + backend.visualize(show_2d=False, show_3d=True, show_errors=False, + save_prefix="factor_graph_demo") + + # 3. 统计信息 + backend.plot_statistics() + + print("演示完成!生成的图片已保存为 'factor_graph_demo_*.png'") + + +# 在文件末尾添加 +if __name__ == "__main__": + main() \ No newline at end of file From 7dfbfeaf46b9bd20bcbff3b1efe5ae8bbc1941ed Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Fri, 19 Dec 2025 17:59:32 +0800 Subject: [PATCH 09/13] =?UTF-8?q?PID=E6=8E=A7=E5=88=B6=E5=99=A8=E5=86=8D?= =?UTF-8?q?=E5=BA=A6=E6=8F=90=E5=8D=87=E6=B8=85=E6=99=B0=E5=BA=A6=E4=BC=98?= =?UTF-8?q?=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../object_detection.py | 116 ++++++++++++------ 1 file changed, 77 insertions(+), 39 deletions(-) diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py index a05617ff4c..27cc7819dc 100644 --- a/src/Object_Tracking_Planning/object_detection.py +++ b/src/Object_Tracking_Planning/object_detection.py @@ -1267,7 +1267,8 @@ def run_step(self, waypoint): -1 = maximum steering to left +1 = maximum steering to right """ - return self._pid_control(waypoint, self._vehicle.get_transform()) + vehicle_transform = self._vehicle.get_transform() + return self._pid_control(waypoint, vehicle_transform) def _pid_control(self, waypoint, vehicle_transform): """ @@ -1280,58 +1281,95 @@ def _pid_control(self, waypoint, vehicle_transform): Returns: float: Steering control in range [-1, 1] """ - # Calculate vehicle direction vector - v_begin = vehicle_transform.location - yaw_rad = math.radians(vehicle_transform.rotation.yaw) - v_end = v_begin + carla.Location( - x=math.cos(yaw_rad), - y=math.sin(yaw_rad) + # 计算当前角度误差 + angle_diff = self._calculate_angle_error(waypoint, vehicle_transform) + + # 更新误差缓冲区 + self._error_buffer.append(angle_diff) + + # 计算PID输出 + pid_output = self._calculate_pid_value(angle_diff) + + # 限制输出范围 + return np.clip(pid_output, -1.0, 1.0) + + def _calculate_angle_error(self, waypoint, vehicle_transform): + """计算车辆当前方向与目标点方向之间的角度误差""" + # 获取车辆位置和方向 + vehicle_pos = vehicle_transform.location + vehicle_yaw = math.radians(vehicle_transform.rotation.yaw) + + # 计算车辆方向向量 + vehicle_direction = carla.Location( + x=math.cos(vehicle_yaw), + y=math.sin(vehicle_yaw) ) + vehicle_vec_end = vehicle_pos + vehicle_direction + + # 计算目标方向向量 + target_pos = waypoint.transform.location + + # 创建向量 + vehicle_vec = np.array([ + vehicle_vec_end.x - vehicle_pos.x, + vehicle_vec_end.y - vehicle_pos.y, + 0.0 + ]) - # Calculate vectors - v_vec = np.array([v_end.x - v_begin.x, v_end.y - v_begin.y, 0.0]) - w_vec = np.array([ - waypoint.transform.location.x - v_begin.x, - waypoint.transform.location.y - v_begin.y, + target_vec = np.array([ + target_pos.x - vehicle_pos.x, + target_pos.y - vehicle_pos.y, 0.0 ]) - # Calculate angle difference - v_norm = np.linalg.norm(v_vec) - w_norm = np.linalg.norm(w_vec) - dot_product = np.dot(w_vec, v_vec) / (v_norm * w_norm) + # 计算角度 + return self._calculate_signed_angle(vehicle_vec, target_vec) + + def _calculate_signed_angle(self, vec1, vec2): + """计算两个向量之间的有符号角度""" + # 归一化向量 + vec1_norm = np.linalg.norm(vec1) + vec2_norm = np.linalg.norm(vec2) + + # 避免除以零 + if vec1_norm < 1e-6 or vec2_norm < 1e-6: + return 0.0 + + # 计算点积和夹角 + dot_product = np.dot(vec1, vec2) / (vec1_norm * vec2_norm) dot_product_clipped = np.clip(dot_product, -1.0, 1.0) - angle_diff = math.acos(dot_product_clipped) + angle = math.acos(dot_product_clipped) - # Determine sign based on cross product - cross_product = np.cross(v_vec, w_vec) + # 根据叉积确定方向(正负) + cross_product = np.cross(vec1, vec2) if cross_product[2] < 0: - angle_diff *= -1.0 + angle = -angle - # Update error buffer and calculate PID terms - self._error_buffer.append(angle_diff) + return angle - # Calculate derivative and integral terms - derivative_error = self._calculate_derivative_error() - integral_error = self._calculate_integral_error() + def _calculate_pid_value(self, current_error): + """计算PID控制器的输出值""" + # 比例项 + proportional = self._k_p * current_error - # PID calculation with clamping - pid_output = ( - self._k_p * angle_diff + - self._k_d * derivative_error + - self._k_i * integral_error - ) + # 微分项 + derivative = self._k_d * self._get_derivative_error() - return np.clip(pid_output, -1.0, 1.0) + # 积分项 + integral = self._k_i * self._get_integral_error() - def _calculate_derivative_error(self): - """Calculate derivative error term.""" - if len(self._error_buffer) >= 2: - return (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt - return 0.0 + return proportional + derivative + integral + + def _get_derivative_error(self): + """获取微分误差项""" + if len(self._error_buffer) < 2: + return 0.0 + return (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt - def _calculate_integral_error(self): - """Calculate integral error term.""" + def _get_integral_error(self): + """获取积分误差项""" + if not self._error_buffer: + return 0.0 return sum(self._error_buffer) * self._dt From 3ac7328179354c4017e28e21e3bd2cb652257bd9 Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Fri, 19 Dec 2025 18:22:01 +0800 Subject: [PATCH 10/13] =?UTF-8?q?=E6=9B=B4=E6=96=B0README=E6=96=87?= =?UTF-8?q?=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md index aa571532a4..b9cc179c85 100644 --- a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md @@ -36,3 +36,9 @@ outdoor_ws/src/robot_description/ - 扫描匹配(ICP)估计相对运动 - 融合IMU、激光雷达和GNSS数据 - 输出高频位姿估计(前端里程计) +3. **全局地图优化与闭环检测** + - 构建因子图优化问题(节点=位姿,因子=约束) + - 融合多种约束:里程计因子、GNSS因子、闭环因子 + - 检测回环并消除累积误差 + - 子图管理优化大范围建图 + - 执行全局优化(类似g2o/GTSAM的功能) \ No newline at end of file From ae3ff1e4c3bb66c3604b9560f2d1ad54994578e0 Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Fri, 19 Dec 2025 19:00:41 +0800 Subject: [PATCH 11/13] =?UTF-8?q?FadingText=E9=80=8F=E6=98=8E=E5=BA=A6?= =?UTF-8?q?=E8=AE=A1=E7=AE=97=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../object_detection.py | 54 +++++++++++++++---- 1 file changed, 45 insertions(+), 9 deletions(-) diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py index ce1e890a4b..3526889a37 100644 --- a/src/Object_Tracking_Planning/object_detection.py +++ b/src/Object_Tracking_Planning/object_detection.py @@ -463,7 +463,13 @@ def render(self, display): class FadingText(object): - """ Class for fading text """ + """Class for fading text""" + + # 常量定义 + ALPHA_MULTIPLIER = 500.0 # 透明度计算系数 + TEXT_OFFSET = (10, 11) # 文本偏移量 + DEFAULT_COLOR = (255, 255, 255) # 默认文本颜色 + DEFAULT_SECONDS = 2.0 # 默认显示时间 def __init__(self, font, dim, pos): """Constructor method""" @@ -471,25 +477,56 @@ def __init__(self, font, dim, pos): self.dim = dim self.pos = pos self.seconds_left = 0 + self._init_surface() + + def _init_surface(self): + """初始化Surface""" self.surface = pygame.Surface(self.dim) + self._clear_surface() + + def _clear_surface(self): + """清空Surface""" + self.surface.fill((0, 0, 0, 0)) - def set_text(self, text, color=(255, 255, 255), seconds=2.0): + def set_text(self, text, color=None, seconds=None): """Set fading text""" + # 使用默认值 + if color is None: + color = self.DEFAULT_COLOR + if seconds is None: + seconds = self.DEFAULT_SECONDS + + # 渲染文本 text_texture = self.font.render(text, True, color) + + # 准备Surface self.surface = pygame.Surface(self.dim) self.seconds_left = seconds - self.surface.fill((0, 0, 0, 0)) - self.surface.blit(text_texture, (10, 11)) + self._clear_surface() + + # 绘制文本 + self.surface.blit(text_texture, self.TEXT_OFFSET) def tick(self, _, clock): """Fading text method for every tick""" - delta_seconds = 1e-3 * clock.get_time() + # 计算经过的时间 + delta_seconds = clock.get_time() / 1000.0 # 转换为秒 + + # 更新时间 self.seconds_left = max(0.0, self.seconds_left - delta_seconds) - self.surface.set_alpha(500.0 * self.seconds_left) + + # 更新透明度 + self._update_alpha() + + def _update_alpha(self): + """更新Surface的透明度""" + alpha_value = self.seconds_left * self.ALPHA_MULTIPLIER + self.surface.set_alpha(alpha_value) def render(self, display): """Render fading text method""" - display.blit(self.surface, self.pos) + if self.seconds_left > 0: + display.blit(self.surface, self.pos) # ============================================================================== @@ -1267,8 +1304,7 @@ def run_step(self, waypoint): -1 = maximum steering to left +1 = maximum steering to right """ - vehicle_transform = self._vehicle.get_transform() - return self._pid_control(waypoint, vehicle_transform) + return self._pid_control(waypoint, self._vehicle.get_transform()) def _pid_control(self, waypoint, vehicle_transform): """ From 0cd6a4c853d69e3311f3c904313300c493be5dbb Mon Sep 17 00:00:00 2001 From: abc123335 <1607504213@qq.com> Date: Fri, 19 Dec 2025 19:23:02 +0800 Subject: [PATCH 12/13] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E6=96=B0=E7=9A=84?= =?UTF-8?q?=E6=A3=80=E6=B5=8B=E6=A8=A1=E5=9D=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- yolo_det.py | 60 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 60 insertions(+) create mode 100644 yolo_det.py diff --git a/yolo_det.py b/yolo_det.py new file mode 100644 index 0000000000..d37aa118aa --- /dev/null +++ b/yolo_det.py @@ -0,0 +1,60 @@ +from ultralytics import YOLO +import cv2 +import numpy as np + + +class ObjectDetector: + def __init__(self, model_name='yolov8n.pt'): + """ + 初始化 YOLO 模型 + :param model_name: 模型名称,'yolov8n.pt' 是最快最小的模型,适合CPU + """ + print(f"正在加载 YOLO 模型: {model_name} (首次运行会自动下载)...") + self.model = YOLO(model_name) + + # 定义我们关心的类别ID (COCO数据集) + # 2: car, 3: motorcycle, 5: bus, 7: truck + self.target_classes = [2, 3, 5, 7] + + def detect(self, frame): + """ + 输入一帧图像,返回画好框的图像 + """ + # 运行推理,stream=True 节省内存,verbose=False 关闭打印日志 + results = self.model(frame, stream=True, verbose=False) + + # 在原图的副本上绘图 + annotated_frame = frame.copy() + + for result in results: + boxes = result.boxes + for box in boxes: + # 获取类别ID + cls_id = int(box.cls[0]) + + # 只处理车辆相关的类别 + if cls_id in self.target_classes: + # 获取坐标 (x1, y1, x2, y2) + x1, y1, x2, y2 = box.xyxy[0] + x1, y1, x2, y2 = int(x1), int(y1), int(x2), int(y2) + + # 获取置信度 + conf = float(box.conf[0]) + if conf < 0.3: continue # 过滤掉置信度低的 + + # 获取类别名称 + cls_name = self.model.names[cls_id] + + # 绘制矩形框 (红色) + cv2.rectangle(annotated_frame, (x1, y1), (x2, y2), (0, 0, 255), 2) + + # 绘制标签背景 + label = f"{cls_name} {conf:.2f}" + (w, h), _ = cv2.getTextSize(label, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1) + cv2.rectangle(annotated_frame, (x1, y1 - 20), (x1 + w, y1), (0, 0, 255), -1) + + # 绘制文字 + cv2.putText(annotated_frame, label, (x1, y1 - 5), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) + + return annotated_frame \ No newline at end of file From 33026f68028b7c452618afba9977cb195d637b08 Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Sat, 20 Dec 2025 04:35:08 +0800 Subject: [PATCH 13/13] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=85=A8=E5=B1=80?= =?UTF-8?q?=E5=AE=9A=E4=BD=8D=E6=A8=A1=E5=9D=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../global_localization.py | 635 ++++++++++++++++++ 1 file changed, 635 insertions(+) create mode 100644 src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/global_localization.py diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/global_localization.py b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/global_localization.py new file mode 100644 index 0000000000..aa92d0a511 --- /dev/null +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/global_localization.py @@ -0,0 +1,635 @@ +# global_localization.py +import numpy as np +from scipy.spatial import KDTree +from scipy.spatial.transform import Rotation as R +import time +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import matplotlib.animation as animation +from matplotlib.widgets import Slider, Button + + +class GlobalLocalization: + """全局定位模块""" + + def __init__(self, map_data=None, visualize=True): + self.map_data = map_data + self.map_points = None + self.map_tree = None + self.map_features = None + self.initialized = False + + # 定位状态 + self.current_pose = np.eye(4) + self.confidence = 0.0 + self.localization_mode = "local" # "local" or "global" + + # 扫描上下文描述子 + self.scan_contexts = {} + + # 可视化相关 + self.visualize = visualize + self.fig = None + self.ax = None + self.history_poses = [] + self.history_confidences = [] + self.scan_history = [] + + # 颜色设置 + self.colors = { + 'map': (0.3, 0.3, 0.3, 0.3), # 灰色半透明 + 'current_scan': (1.0, 0.0, 0.0, 0.5), # 红色 + 'matched_scan': (0.0, 1.0, 0.0, 0.5), # 绿色 + 'trajectory': (0.0, 0.0, 1.0, 1.0), # 蓝色 + 'candidates': (1.0, 1.0, 0.0, 0.3), # 黄色 + } + + def initialize_visualization(self): + """初始化可视化窗口""" + if not self.visualize: + return + + plt.ion() # 开启交互模式 + + self.fig = plt.figure(figsize=(15, 10)) + + # 创建3D点云视图 + self.ax1 = self.fig.add_subplot(231, projection='3d') + self.ax1.set_title('3D Point Cloud View') + self.ax1.set_xlabel('X (m)') + self.ax1.set_ylabel('Y (m)') + self.ax1.set_zlabel('Z (m)') + + # 创建2D俯视图 + self.ax2 = self.fig.add_subplot(232) + self.ax2.set_title('2D Top View') + self.ax2.set_xlabel('X (m)') + self.ax2.set_ylabel('Y (m)') + self.ax2.set_aspect('equal') + + # 创建轨迹视图 + self.ax3 = self.fig.add_subplot(233) + self.ax3.set_title('Trajectory and Confidence') + self.ax3.set_xlabel('Step') + self.ax3.set_ylabel('Confidence') + + # 创建扫描上下文视图 + self.ax4 = self.fig.add_subplot(234) + self.ax4.set_title('Scan Context Descriptor') + self.ax4.set_xlabel('Sector') + self.ax4.set_ylabel('Ring') + + # 创建位姿误差视图 + self.ax5 = self.fig.add_subplot(235) + self.ax5.set_title('Pose Error') + self.ax5.set_xlabel('Step') + self.ax5.set_ylabel('Error (m/rad)') + + # 创建ICP匹配视图 + self.ax6 = self.fig.add_subplot(236) + self.ax6.set_title('ICP Matching') + self.ax6.set_xlabel('X (m)') + self.ax6.set_ylabel('Y (m)') + self.ax6.set_aspect('equal') + + plt.tight_layout() + plt.show(block=False) + + def load_map(self, map_points, map_features=None): + """加载全局地图""" + self.map_points = map_points + self.map_features = map_features + + if map_points is not None and len(map_points) > 0: + self.map_tree = KDTree(map_points[:, :3]) # 只使用xyz + self.initialized = True + print(f"地图已加载,包含 {len(map_points)} 个点") + + # 可视化地图 + if self.visualize: + if self.fig is None: + self.initialize_visualization() + self._visualize_map() + else: + print("警告:地图点云为空") + + def _visualize_map(self): + """可视化地图点云""" + if not self.visualize or self.map_points is None: + return + + # 3D视图 + self.ax1.clear() + if len(self.map_points) > 10000: # 太多点的话采样显示 + indices = np.random.choice(len(self.map_points), 10000, replace=False) + display_points = self.map_points[indices] + else: + display_points = self.map_points + + self.ax1.scatter(display_points[:, 0], display_points[:, 1], display_points[:, 2], + c=[self.colors['map']], s=1, marker='.') + + # 2D俯视图 + self.ax2.clear() + self.ax2.scatter(display_points[:, 0], display_points[:, 1], + c=[self.colors['map'][:3]], s=1, marker='.', alpha=0.3) + + self.ax1.set_title(f'Map Points: {len(self.map_points)}') + self.fig.canvas.draw_idle() + + def _visualize_localization(self, scan_points, pose, confidence, + matched_points=None, candidates=None): + """可视化定位结果""" + if not self.visualize: + return + + # 变换当前扫描点 + transformed_scan = self._transform_points(scan_points, pose) + + # 更新历史记录 + self.history_poses.append(pose) + self.history_confidences.append(confidence) + self.scan_history.append(transformed_scan) + + # 限制历史记录长度 + max_history = 50 + if len(self.history_poses) > max_history: + self.history_poses = self.history_poses[-max_history:] + self.history_confidences = self.history_confidences[-max_history:] + self.scan_history = self.scan_history[-max_history:] + + # 1. 3D点云视图 + self.ax1.clear() + if len(self.map_points) > 10000: + indices = np.random.choice(len(self.map_points), 10000, replace=False) + display_map = self.map_points[indices] + else: + display_map = self.map_points + + self.ax1.scatter(display_map[:, 0], display_map[:, 1], display_map[:, 2], + c=[self.colors['map']], s=1, marker='.', alpha=0.3, label='Map') + + # 当前扫描 + self.ax1.scatter(transformed_scan[:, 0], transformed_scan[:, 1], transformed_scan[:, 2], + c='r', s=5, marker='o', alpha=0.5, label='Current Scan') + + # 轨迹 + if len(self.history_poses) > 1: + trajectory = np.array([p[:3, 3] for p in self.history_poses]) + self.ax1.plot(trajectory[:, 0], trajectory[:, 1], trajectory[:, 2], + c=self.colors['trajectory'], linewidth=2, label='Trajectory') + + self.ax1.set_title(f'3D View - Confidence: {confidence:.3f}') + self.ax1.set_xlabel('X (m)') + self.ax1.set_ylabel('Y (m)') + self.ax1.set_zlabel('Z (m)') + self.ax1.legend() + + # 2. 2D俯视图 + self.ax2.clear() + self.ax2.scatter(display_map[:, 0], display_map[:, 1], + c=[self.colors['map'][:3]], s=1, marker='.', alpha=0.3) + + self.ax2.scatter(transformed_scan[:, 0], transformed_scan[:, 1], + c='r', s=10, marker='o', alpha=0.6, label='Current Scan') + + if matched_points is not None: + self.ax2.scatter(matched_points[:, 0], matched_points[:, 1], + c='g', s=20, marker='x', alpha=0.8, label='Matched Points') + + # 轨迹 + if len(self.history_poses) > 1: + trajectory = np.array([p[:3, 3] for p in self.history_poses]) + self.ax2.plot(trajectory[:, 0], trajectory[:, 1], + c=self.colors['trajectory'], linewidth=2, label='Trajectory') + + # 显示方向箭头 + dx, dy = 2 * np.cos(pose[2, 0]), 2 * np.sin(pose[2, 0]) + self.ax2.arrow(pose[0, 3], pose[1, 3], dx, dy, + head_width=0.5, head_length=0.5, fc='blue', ec='blue') + + # 候选位姿 + if candidates is not None: + for i, candidate in enumerate(candidates[:5]): # 只显示前5个 + self.ax2.scatter(candidate[0, 3], candidate[1, 3], + c='y', s=50, marker='*', alpha=0.5) + + self.ax2.set_title(f'2D Top View - Pose: [{pose[0, 3]:.1f}, {pose[1, 3]:.1f}]') + self.ax2.set_xlabel('X (m)') + self.ax2.set_ylabel('Y (m)') + self.ax2.legend() + self.ax2.grid(True, alpha=0.3) + + # 3. 轨迹和置信度 + self.ax3.clear() + if self.history_confidences: + steps = range(len(self.history_confidences)) + self.ax3.plot(steps, self.history_confidences, 'b-', linewidth=2, label='Confidence') + self.ax3.fill_between(steps, 0, self.history_confidences, alpha=0.3) + self.ax3.set_ylim(0, 1.1) + self.ax3.set_xlabel('Step') + self.ax3.set_ylabel('Confidence') + self.ax3.set_title('Localization Confidence History') + self.ax3.grid(True, alpha=0.3) + self.ax3.legend() + + # 4. 扫描上下文 + self.ax4.clear() + current_context = self.create_scan_context(scan_points) + im = self.ax4.imshow(current_context, aspect='auto', cmap='viridis') + self.ax4.set_title('Current Scan Context') + self.ax4.set_xlabel('Sector') + self.ax4.set_ylabel('Ring') + plt.colorbar(im, ax=self.ax4) + + # 5. 位姿误差(如果有真值的话) + self.ax5.clear() + if len(self.history_poses) > 1: + positions = np.array([p[:3, 3] for p in self.history_poses]) + position_changes = np.diff(positions, axis=0) + position_norms = np.linalg.norm(position_changes, axis=1) + + steps = range(len(position_norms)) + self.ax5.plot(steps, position_norms, 'r-', label='Position Change (m)') + + if len(steps) > 0: + self.ax5.axhline(y=np.mean(position_norms), color='r', linestyle='--', + alpha=0.5, label=f'Avg: {np.mean(position_norms):.3f}m') + + self.ax5.set_xlabel('Step') + self.ax5.set_ylabel('Error / Change') + self.ax5.set_title('Pose Changes') + self.ax5.grid(True, alpha=0.3) + self.ax5.legend() + + # 6. ICP匹配视图 + self.ax6.clear() + if matched_points is not None and len(matched_points) > 0: + # 显示匹配点对 + for i in range(min(20, len(transformed_scan))): # 只显示20个匹配对 + scan_pt = transformed_scan[i, :2] + map_pt = matched_points[i, :2] + self.ax6.plot([scan_pt[0], map_pt[0]], [scan_pt[1], map_pt[1]], + 'g-', alpha=0.3, linewidth=0.5) + + self.ax6.scatter(matched_points[:, 0], matched_points[:, 1], + c='g', s=20, marker='x', alpha=0.8, label='Map Points') + + self.ax6.scatter(transformed_scan[:, 0], transformed_scan[:, 1], + c='r', s=10, marker='o', alpha=0.6, label='Scan Points') + + self.ax6.set_title(f'ICP Matching - Inliers: {len(matched_points) if matched_points is not None else 0}') + self.ax6.set_xlabel('X (m)') + self.ax6.set_ylabel('Y (m)') + self.ax6.legend() + self.ax6.grid(True, alpha=0.3) + + plt.tight_layout() + self.fig.canvas.draw_idle() + plt.pause(0.01) # 短暂暂停以更新显示 + + def localize_with_gnss(self, gnss_position, initial_guess=None): + """使用GNSS进行初始全局定位""" + if not self.initialized or self.map_points is None: + return np.eye(4), 0.0 + + # 将GNSS坐标转换为局部坐标 + # 这里需要知道地图的GNSS参考点 + local_position = gnss_position # 简化:假设已经是局部坐标 + + # 在地图中寻找最近的点 + distances, indices = self.map_tree.query(local_position.reshape(1, -1), k=10) + + # 计算平均位置作为初始位姿 + if len(indices) > 0: + nearest_points = self.map_points[indices[0], :3] + estimated_position = np.mean(nearest_points, axis=0) + + # 构建位姿(假设水平) + pose = np.eye(4) + pose[:3, 3] = estimated_position + + confidence = 1.0 / (1.0 + np.mean(distances)) + + # 可视化 + if self.visualize: + self._visualize_localization( + scan_points=np.array([[local_position[0], local_position[1], 0]]), + pose=pose, + confidence=confidence, + matched_points=nearest_points + ) + + return pose, confidence + + return np.eye(4), 0.0 + + def localize_with_scan_matching(self, scan_points, initial_guess=None, visualize=True): + """使用扫描匹配进行定位""" + if not self.initialized or self.map_points is None: + return np.eye(4), 0.0 + + if initial_guess is None: + initial_guess = np.eye(4) + + # 使用ICP进行精配准 + refined_pose, matched_points = self._icp_localization(scan_points, initial_guess) + + # 计算匹配分数 + confidence = self._compute_match_confidence(scan_points, refined_pose) + + # 可视化 + if self.visualize and visualize: + self._visualize_localization( + scan_points=scan_points, + pose=refined_pose, + confidence=confidence, + matched_points=matched_points + ) + + return refined_pose, confidence + + def _icp_localization(self, scan_points, initial_guess, max_iterations=30): + """ICP定位""" + pose = initial_guess.copy() + all_matched_points = None + + for iteration in range(max_iterations): + # 变换扫描点 + transformed_points = self._transform_points(scan_points, pose) + + # 寻找最近邻 + distances, indices = self.map_tree.query(transformed_points[:, :3]) + + # 过滤掉距离太远的点 + valid_mask = distances < 2.0 # 2米阈值 + if np.sum(valid_mask) < 10: # 至少需要10个有效点 + break + + valid_scan = scan_points[valid_mask] + valid_map = self.map_points[indices[valid_mask], :3] + + # 保存匹配点用于可视化 + if iteration == max_iterations - 1: # 最后一次迭代 + all_matched_points = valid_map + + # 计算最优变换 + R_mat, t_vec = self._compute_rigid_transform(valid_scan[:, :3], valid_map) + + # 更新位姿 + delta_pose = np.eye(4) + delta_pose[:3, :3] = R_mat + delta_pose[:3, 3] = t_vec + + pose = delta_pose @ pose + + # 检查收敛 + if np.linalg.norm(t_vec) < 0.001 and np.linalg.norm(R_mat - np.eye(3)) < 0.001: + break + + return pose, all_matched_points + + def _transform_points(self, points, pose): + """变换点云""" + transformed = (pose[:3, :3] @ points[:, :3].T + pose[:3, 3:4]).T + + if points.shape[1] > 3: + transformed = np.hstack([transformed, points[:, 3:]]) + + return transformed + + def _compute_rigid_transform(self, A, B): + """计算刚体变换""" + centroid_A = np.mean(A, axis=0) + centroid_B = np.mean(B, axis=0) + + AA = A - centroid_A + BB = B - centroid_B + + H = AA.T @ BB + U, S, Vt = np.linalg.svd(H) + + R_mat = Vt.T @ U.T + + # 处理反射情况 + if np.linalg.det(R_mat) < 0: + Vt[-1, :] *= -1 + R_mat = Vt.T @ U.T + + t_vec = centroid_B - R_mat @ centroid_A + + return R_mat, t_vec + + def _compute_match_confidence(self, scan_points, pose): + """计算匹配置信度""" + transformed_points = self._transform_points(scan_points, pose) + + distances, _ = self.map_tree.query(transformed_points[:, :3], k=1) + + # 计算内点比例 + inlier_ratio = np.mean(distances < 0.5) # 0.5米内点阈值 + + # 综合置信度 + confidence = min(1.0, inlier_ratio * 2.0) + + return confidence + + def create_scan_context(self, scan_points): + """创建扫描上下文描述子""" + num_sectors = 60 + num_rings = 20 + max_range = 50.0 + + descriptor = np.zeros((num_rings, num_sectors)) + + for point in scan_points: + x, y, z = point[:3] + + # 转换为极坐标 + radius = np.sqrt(x ** 2 + y ** 2) + angle = np.arctan2(y, x) + + if radius > max_range: + continue + + # 计算环和扇区索引 + ring_idx = min(int(radius / max_range * num_rings), num_rings - 1) + sector_idx = int((angle + np.pi) / (2 * np.pi) * num_sectors) % num_sectors + + # 存储最大高度 + height = z + descriptor[ring_idx, sector_idx] = max(descriptor[ring_idx, sector_idx], height) + + return descriptor + + def scan_context_match(self, current_descriptor): + """扫描上下文匹配""" + if not self.scan_contexts: + return None, 0.0 + + best_match = None + best_score = 0.0 + + for node_id, descriptor in self.scan_contexts.items(): + # 计算描述子相似度 + score = self._descriptor_similarity(current_descriptor, descriptor) + + if score > best_score: + best_score = score + best_match = node_id + + return best_match, best_score + + def _descriptor_similarity(self, desc1, desc2): + """计算描述子相似度""" + # 列向量的余弦相似度 + similarity = 0.0 + + for i in range(desc1.shape[1]): + col1 = desc1[:, i] + col2 = desc2[:, i] + + norm1 = np.linalg.norm(col1) + norm2 = np.linalg.norm(col2) + + if norm1 > 0 and norm2 > 0: + similarity += np.dot(col1, col2) / (norm1 * norm2) + + return similarity / desc1.shape[1] + + def update_scan_context(self, node_id, descriptor): + """更新扫描上下文数据库""" + self.scan_contexts[node_id] = descriptor + + def global_relocalization(self, scan_points, gnss_hint=None): + """全局重定位""" + if not self.initialized: + return np.eye(4), 0.0 + + # 方法1:使用扫描上下文 + current_descriptor = self.create_scan_context(scan_points) + matched_node, context_score = self.scan_context_match(current_descriptor) + + if context_score > 0.7: # 高置信度匹配 + print(f"扫描上下文匹配成功,分数: {context_score}") + # 可以返回匹配节点的位姿 + return np.eye(4), context_score + + # 方法2:使用GNSS提示 + if gnss_hint is not None: + pose, confidence = self.localize_with_gnss(gnss_hint) + if confidence > 0.5: + print(f"GNSS提示定位成功,置信度: {confidence}") + return pose, confidence + + # 方法3:全局ICP(计算量大,作为最后手段) + print("尝试全局ICP重定位...") + + # 在地图中采样一些候选位姿 + candidates = self._generate_candidates(scan_points) + + best_pose = np.eye(4) + best_score = 0.0 + best_matched_points = None + + for candidate in candidates: + pose, matched_points = self._icp_localization(scan_points, candidate, max_iterations=10) + score = self._compute_match_confidence(scan_points, pose) + + if score > best_score: + best_score = score + best_pose = pose + best_matched_points = matched_points + + # 可视化候选位姿和最佳匹配 + if self.visualize: + self._visualize_localization( + scan_points=scan_points, + pose=best_pose, + confidence=best_score, + matched_points=best_matched_points, + candidates=candidates + ) + + if best_score > 0.3: + print(f"全局ICP重定位成功,分数: {best_score}") + return best_pose, best_score + + print("重定位失败") + return np.eye(4), 0.0 + + def _generate_candidates(self, scan_points, num_candidates=20): + """生成候选位姿""" + candidates = [] + + # 在地图边界内随机采样 + if self.map_points is not None and len(self.map_points) > 0: + map_min = np.min(self.map_points[:, :3], axis=0) + map_max = np.max(self.map_points[:, :3], axis=0) + + for _ in range(num_candidates): + position = np.random.uniform(map_min, map_max) + yaw = np.random.uniform(-np.pi, np.pi) + + pose = np.eye(4) + pose[:3, 3] = position + pose[:3, :3] = R.from_euler('z', yaw).as_matrix() + + candidates.append(pose) + + return candidates + + def save_visualization(self, filename="localization_result.png"): + """保存可视化结果""" + if self.fig is not None: + self.fig.savefig(filename, dpi=300, bbox_inches='tight') + print(f"可视化结果已保存到 {filename}") + + def close_visualization(self): + """关闭可视化窗口""" + if self.fig is not None: + plt.close(self.fig) + self.fig = None + + +# 使用示例 +def main(): + # 创建模拟地图数据 + map_points = np.random.randn(10000, 3) * 50 # 10000个随机点 + + # 创建定位器并启用可视化 + locator = GlobalLocalization(visualize=True) + locator.load_map(map_points) + + # 模拟扫描数据 + for i in range(50): + # 创建模拟扫描(部分地图点加上噪声) + scan_indices = np.random.choice(len(map_points), 100) + scan_points = map_points[scan_indices] + np.random.randn(100, 3) * 0.1 + + # 模拟运动 + motion = np.eye(4) + motion[:3, 3] = np.array([0.5, 0.2, 0]) # 平移 + if i > 0: + locator.current_pose = motion @ locator.current_pose + + # 进行定位 + pose, confidence = locator.localize_with_scan_matching( + scan_points, + locator.current_pose + ) + + print(f"Step {i}: Confidence = {confidence:.3f}") + + time.sleep(0.1) # 模拟实时处理 + + # 保存结果 + locator.save_visualization() + + # 保持窗口打开 + print("按任意键关闭窗口...") + plt.show(block=True) + + +if __name__ == "__main__": + main() \ No newline at end of file