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 1/5] =?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 737c4bdf0d281f4a3f383d205052db61377de670 Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Thu, 18 Dec 2025 20:18:52 +0800 Subject: [PATCH 2/5] =?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 3/5] =?UTF-8?q?=E4=BF=AE=E6=94=B9readme=E6=96=87=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 4/5] =?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 a92adb9cc41362edd4888f86fe0fc7b03cc506a1 Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Fri, 19 Dec 2025 15:19:08 +0800 Subject: [PATCH 5/5] =?UTF-8?q?=E6=9B=B4=E6=96=B0readme=E6=96=87=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数据 + - 输出高频位姿估计(前端里程计)