手持扫描仪支架设计、3D打印与真实场景下激光雷达深度相机融合建图SLAM实验（Jetson Orin NX、MID-360s、深度相机；ROS2、Fast-LIO2、RTAB-Map）

Ciallo～(∠・ω< )⌒★若为初次学习此类内容或需要了解相关仿真内容，可参考下列前置科技：

近期给组里项目帮忙的时候整的一些东西，也是在上无人机前做点手持的东西测试一下（是的给无人机提供雷达里程计的事这么久了还没弄好）。简单来说这次做的事情是：1、设计和打印手持扫描仪支架并完成组装；2、确定深度相机和激光雷达之间的坐标转换参数；3、激光雷达里程计负责定位，深度相机实现色彩建图。之前的实验发现纯用深度相机的视觉里程计非常容易中途丢失定位而无法继续建图，配合激光雷达后效果好了很多。

之前找别人定制一个支架，如下左图，用来把相机和激光雷达固定到无人机上，图方便所以在其基础上进一步设计（喜）。后来不小心（最好是）买了一台拓竹P2S，以后东西都可以自己打了耶。支架上要搭载的东西包括图左的支架和两个传感器、jetson orin nx、格氏4S 5300mah电池（以前组的无人机的电池，主要是规格能带上飞机）、开关模块，和各种线。总之最终设计如下中图，实际打印并组装后如下右图。

相机和雷达在ros2中的都遵循右手坐标系（x朝前，y朝左，z朝上），雷达的原点在半球体中心底部，相机的原点每个厂家可能不同，我这边的虽然是第三方厂家，但遵循orbbec的底层，原点在左红外相机处（指相机正视你的前方时的左边，或者你看着相机正面时右边的那个IR摄像头）。

至于它们的的坐标系转换，更详细的内容发布在了另一篇文章中（简直水贴大师），总之是算出了所需要的雷达相对于相机的转换参数：

其实应该直接做外参标定的，但是太烂加上时间紧任务重（迫真）所以用了点简单的方法凑合。

部署在jetson上的扫描仪launch文件如下，需要提前部署好livox_ros_driver2、fast_lio和自己相机的ros2功能包等，会自动保存db到本地，也有rosbag录制（占空间太快给我注释掉了）：

import datetime
import os
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, ExecuteProcess
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_xml.launch_description_sources import XMLLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node

def generate_launch_description():
    livox_ros2_dir = get_package_share_directory('livox_ros_driver2')
    fast_lio_dir = get_package_share_directory('fast_lio')
    astra_dir = get_package_share_directory('astra_camera')
    rtabmap_launch_dir = get_package_share_directory('rtabmap_launch')

    # ========== 定义超参数 ==========
    timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S")
    bag_path_arg = DeclareLaunchArgument(
        'bag_path',
        default_value=os.path.expanduser(f'~/WushanPilot/src/slam/rosbags/rosbags_{timestamp}'),
        description='Path and prefix for the output ros2 bag'
    )

    # ========== 1. 启动传感器与 TF ==========

    # Livox MID-360 启动
    livox_driver_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(livox_ros2_dir, 'launch_ROS2', 'msg_MID360s_launch.py') 
        )
    )

    # Astra (Gemini) 双目相机启动
    astra_camera_launch = IncludeLaunchDescription(
        XMLLaunchDescriptionSource(
            os.path.join(astra_dir, 'launch', 'gemini.launch.xml')
        )
    )

    # TF: body -> base_link (适配 FAST-LIO 默认输出)
    static_tf_livox = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='body_to_base_link',
        arguments=['--x', '0', '--y', '0', '--z', '0',
                   '--roll', '0', '--pitch', '0', '--yaw', '0',
                   '--frame-id', 'body',
                   '--child-frame-id', 'base_link']
    )

    # TF: base_link -> camera_link
    static_tf_camera = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='base_to_camera_tf',
        arguments=['--x', '-0.0192047', '--y', '0.057002122', '--z', '-0.033594209',
                   '--roll', '0', '--pitch', '-0.87266463', '--yaw', '1.57079633',    # RPY or Euler
                   '--frame-id', 'base_link',
                   '--child-frame-id', 'camera_link']
    )

    # ========== 2. FAST-LIO (仅提供里程计) ==========
    
    fast_lio_config_path = os.path.join(fast_lio_dir, 'config', 'mid360.yaml')
    fast_lio_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(fast_lio_dir, 'launch', 'mapping.launch.py')
        ),
        launch_arguments={'config_file': fast_lio_config_path}.items()
    )

    # ========== 3. RTAB-Map (接收 Lidar 里程计 + RGBD 建图) ==========

    database_path = os.path.expanduser(f'~/WushanPilot/src/slam/output/rtabmap_fusion_{timestamp}.db')
    
    rtabmap_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(rtabmap_launch_dir, 'launch', 'rtabmap.launch.py')
        ),
        launch_arguments={
            'rtabmap_args': '--delete_db_on_start',
            
            # 传感器话题输入
            'rgb_topic': '/camera/color/image_raw',
            'depth_topic': '/camera/depth/image_raw',
            'camera_info_topic': '/camera/color/camera_info',
            'odom_topic': '/Odometry',  # 关键：接收 FAST-LIO 发布的里程计
            
            # 坐标系设置
            'frame_id': 'base_link',
            'odom_frame_id': 'camera_init', # FAST-LIO 的世界坐标系通常是 camera_init
            
            # 核心策略配置
            'visual_odometry': 'false', # 关键：关闭自带 VIO，节省算力
            'subscribe_scan_cloud': 'false', # 关键：不使用激光雷达点云建图（纯 RGBD 建图）
            'approx_sync': 'true',

            # --- 新增：画质提升与点云优化参数 ---
            # 1. 减少深度图降采样 (默认通常是 4，改为 1 或 2，数字越小越密，但越吃算力)
            'Grid/DepthDecimation': '1', 
            
            # 2. 减小点云地图的分辨率 (默认 0.05m，即 5cm，改成 1cm 或 2cm)
            'Grid/CellSize': '0.015', 
            
            # 3. 限制深度相机的有效建图距离
            # 结构光/ToF 相机超过 3 米后噪点极大，会把远处的点云糊在近处
            # 针对贴近清理病害植被的任务，只保留近处高质量数据
            'Grid/MaxDepth': '2.5',   
            'Grid/MinDepth': '0.25',
            
            # 4. 开启点云统计滤波去噪 (去除悬浮的噪点)
            'Grid/NoiseFilteringRadius': '0.05',
            'Grid/NoiseFilteringMinNeighbors': '2',
            
            # 5. 更新频率 (如果飞行速度不快，可以稍微降低节点生成频率，减少轨迹上的重影)
            'RGBD/LinearUpdate': '0.1',  # 移动多少米记录一帧
            'RGBD/AngularUpdate': '0.1', # 旋转多少弧度记录一帧
            # ---------------------------------
            
            # QoS 配置 (适配相机硬件)
            'qos_camera_info': '1',
            'qos_image': '1',
            'qos_depth': '1',
            
            # 其他设置
            'database_path': database_path,
            'rtabmap_viz': 'false',
            'rviz': 'false'
        }.items()
    )

    # ========== 4. 自动录制 rosbag ==========
    
    record_bag_process = ExecuteProcess(
        cmd=['ros2', 'bag', 'record', '-o', LaunchConfiguration('bag_path'), 
             '/livox/lidar', '/livox/imu', 
             '/camera/color/image_raw', '/camera/depth/image_raw', '/camera/color/camera_info'],
        output='screen'
    )

    return LaunchDescription([
        bag_path_arg,
        livox_driver_launch,
        astra_camera_launch,
        static_tf_livox,
        static_tf_camera,
        fast_lio_launch,
        rtabmap_launch,
        # record_bag_process
    ])

实际使用时可以开个tmux分离控制台，不然要顺便有人在旁边扛着电脑提供热点……

在一些场景中走了走的实验效果如下（景区/室内/田里）：

效果肯定是比纯深度相机和视觉里程计要好，但还是差点意思，可能相机太便宜了内外参数还有待进一步标定，以及需要更好的建图和回环算法之类的。不过好像别的成熟产品的思路其实是激光雷达建图，深度相机起辅助作用给它的点云上色，虽然这样的点云密度令人担忧（AI最好没骗我）。

既然有3D打印机，是时候接点定制三维打印的单了是吗，姆哈哈哈哈哈……