small_gicp_relocalization（先验点云地图重定位）

small_gicp_relocalization（先验点云地图重定位）

实车先看：地图、起点和云台零位

这个节点在已有地图导航模式 slam:=False 下，把当前配准点云与先验PCD地图对齐，输出 map → odom 的位置、朝向修正。它不直接控制车轮或云台，也不是重新创建地形障碍的节点；slam:=True 的整套启动不运行它。

当前没有实物，也没有本节点的有效重定位测试。下文分别说明“代码如何解释这些值”和“实车是否适用”，不把源码流程当作已经验证的定位效果。

参数

ros__parameters（ROS节点参数）

参数表

前12项对应当前实物 YAML；prior_pcd_file 虽被注释，却由 launch另行传入；input_cloud_topic 没写在这份 YAML中，仍使用源码默认值。这两项也列出，便于完整理解输入来源。

small_gicp_relocalization:
  ros__parameters:
    use_sim_time: False
    num_threads: 4
    num_neighbors: 20
    global_leaf_size: 0.1
    registered_leaf_size: 0.05
    max_dist_sq: 3.0
    map_frame: "map"
    odom_frame: "odom"
    base_frame: "base_footprint"
    robot_base_frame: "gimbal_yaw"
    lidar_frame: "front_mid360"
    init_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]   # [x, y, z, roll, pitch, yaw]
    # The prior_pcd_file does not need to be specified since it going to be set by defaults in launch.
    # prior_pcd_file: ""

本块没有当前完全不使用的自定义参数：robot_base_frame 只在收到手动初始位姿时参与，init_pose 只在启动时设初值，它们不是持续调节项。话题、坐标系和路径不能按数值大小调参。

参数详解

输入与输出：匹配的是哪两份点云

实时点云已经由Point-LIO与接口节点配准到 odom，本节点直接累加坐标，不再逐帧转雷达系，也不额外按云台yaw旋转这些点。云台旋转时，静态环境应仍在共同的odom坐标下对齐；这个结论依赖上游位姿、安装外参和时间接口正确，目前没有独立yaw验证。

回调只保存最近输入的frame_id与时间戳，没有检查多帧是否同系，也没有在frame_id变化时清空累计点。PCD没有ROS消息那样的frame_id，本节点按自己的来源假设处理；文件名和坐标系字符串不能证明地图已经对应。

每500 ms真实时间尝试匹配一次，有点才处理；每轮降采样、估计局部形状、搜索地图对应点，最多做10次优化迭代。匹配报告收敛才更新结果，未收敛则沿用旧结果，但本轮累计点仍会清空。收敛表示算法停止条件满足，不证明匹配到了场地中正确的位置；源码没有再按匹配误差、有效对应比例等检查可信度。

prior_pcd_file：先确认用的是自己的场地地图

整套启动的默认路径在安装后的 pb2025_nav_bringup 共享目录：

相应实物默认二维地图 map/reality/rmul_2024.yaml 也不存在；仿真默认二维地图存在。这只是当前默认文件检查，没有读取运行节点参数，也不排除使用者另传world、PCD或二维地图路径。上实物前需要准备当前场地的对应地图，不能把仿真PCD存在当成实物地图已准备。

localization_launch.py 在参数文件之后，额外传入 {"prior_pcd_file": prior_pcd_file}，因此注释掉这一行不表示节点不使用先验PCD。它与Point-LIO是否启用 prior_pcd_en 是不同功能：Point-LIO可不加载先验，重定位节点仍需要先验PCD。

文件加载失败只记录错误并返回，构造函数仍继续准备匹配目标，没有明确阻止空目标进入后续匹配；不能把“节点起来了”视为地图加载成功。源码不会在运行时重新读取PCD，也不会替你核对它与Nav2二维地图的位置、朝向、米制尺度或场地版本是否一致。

base_frame 与 lidar_frame：启动时会转动整份地图

PCD读入后，源码查询 base_footprint ← front_mid360，再把这个矩阵左乘到整份地图的每个点。它按节点当前时间查询，失败后每秒重试，成功一次就继续；没有查询次数上限，也没有在后续云台转动时更新目标地图。

这对应源码的一个来源假设：PCD基准是初始 lidar_odom，用初始底盘与雷达关系把地图转换到所需参考中。但当前Point-LIO实际保存的是 camera_init 下的配准世界点，且其位姿原点涉及内部IMU；不是每一帧的雷达自身坐标。把这类PCD当初始雷达系直接套机械变换，是否与实际建图基准一致，必须复核，不能仅照README的命名认定转换正确。

雷达在云台上，这一步特别依赖零位。 同一份PCD若建图时与重新启动时云台相对yaw不同，当前 base←lidar 会含不同角度；按源码计算，目标地图可能被不同矩阵转动，而独立加载的二维地图不会自动跟着调整。是否实际造成错位，需要结合PCD生成时的起点、云台零位、底盘方向与地图原点判断，目前没有测试结论。

该查询与 loam_interface 的一次性基准转换意图相关，但两节点分别查询，不保证取到同一时刻的云台角。这里的机械关系始终保留：MID360挂在 gimbal_yaw，只有实际yaw会带动它，实际pitch位于后续层；两个odom关节固定。重力参考、内部IMU→雷达关系或地图来源错误，不能通过增大匹配距离掩盖。

包内README还写过“未设置时使用空变换”，当前实现没有对应的单位变换旁路：若PCD能读取但frame为空或TF缺失，会进入重试。应以当前源码为准，本轮不修改节点或地图转换。

init_pose 与 robot_base_frame：两种初始位姿不是同一个对象

YAML的 init_pose 直接写入匹配矩阵。 在当前输入为odom、目标为map的约定下，它是 map←odom 初值，即“odom原点在map中的位置和方向”。

[0,0,0,0,0,0] 表示先猜两系完全重合；这只有在起始参考关系吻合时才合适，不表示算法已知道机器人在地图中的真实位置。位置单位m，角度rad；偏航90°应为约1.5708，而不是填90。旋转按yaw、pitch、roll组成 Rz×Ry×Rx。

例如：若odom起点恰好与所描述的机器人原点重合且朝向一致，那么机器人起始地图位姿可能就是map←odom；若odom原点与云台存在高度或安装偏移，或机器人已在odom内移动，就不能直接照抄机器人位置。当前底盘、云台、雷达原点不同，需按实际参考关系计算。

话题 initialpose 则被当作 robot_base_frame=gimbal_yaw 在地图中的位姿。 回调再查询最新 gimbal_yaw←输入扫描系，当前扫描系应为odom，两者相乘得到map←odom：

map←odom = 人工给定的 map←gimbal_yaw × TF中的 gimbal_yaw←odom

所以RViz箭头若被你理解成底盘车头，而当前云台相对底盘左转90°，就可能少算一个云台角。位置也对应云台原点：若人工消息给z=0，源码把它当作云台原点位于map的z=0，而不是“底盘地面高度为0”；高度应按这份地图的基准核对。只改 robot_base_frame 的名字也需要重新核对整条关系，不能把“机器人”三个字自动等同于底盘。本参数仅该回调用，不在每次点云匹配时扣机械角。

回调没有按消息header.frame_id转换位姿、没有使用其协方差，也没有按消息时间查TF，而是取最新变换；收到首帧扫描前，扫描frame_id还为空，查询可能失败。手动设初值只改当前和下一轮匹配猜测，不会重转PCD，也不清空已累计点。云台快速转动时，人工位姿对应的朝向与最新TF是否同一时刻也要核对。

max_dist_sq：1.73 m是配对门槛，不是定位误差指标

当前构建使用的small_gicp代码直接判断“对应点距离平方是否大于 max_dist_sq”；超出就拒绝。源点先按当前猜测变换到地图，再找最近地图点，才检查这项，因此在米制点云中单位为m²：

max_dist_sq = 3.0 m² → 最大对应点距离约 sqrt(3) = 1.73 m
希望门槛为0.5 m → 平方值为0.25 m²

第二行仅解释单位换算，不是建议修改当前值。它不限制机器人总移动距离，不表示允许定位偏差1.73 m，更不是扫描量程。错误初值下，源点仍可能碰巧靠近另一面墙并通过门槛。

实车中，放宽可留下更多点对，但重复柱子、相似走廊和动态物体也可能参与误配；收紧会减少远距离错配，却可能让真实初值偏差稍大时无足够点对。先确认地图、初值和参考系，再结合正确匹配／误匹配情况调整，不能单独把“更小”理解成“更准”。

该单位按当前构建缓存库源码核实，库提交为 fa0cfc983417c044b6036f71dfa448038bb9c820。CMake引用master，重新获取依赖后应复核版本，不把当前缓存行为当成所有版本的固定承诺。

两个leaf_size、num_neighbors 与 num_threads（点云精简、邻点数量与计算线程）

global_leaf_size=0.1 m 处理先验地图；registered_leaf_size=0.05 m 处理每轮累计实时点云。两者不要求相同，也不等于地形分析格子、Nav2地图分辨率或定位精度。地图处理不会改原PCD，实时处理不会改变Point-LIO本身的输出。

调小通常保留更多细节，代价是计算量增加；原始数据没有的细节不会因调小而出现。远处点稀疏、墙角或小柱子能否稳定描述，需看实际数据；人员、敌车等动态点在本节点没有专门筛除，实时点更多也可能带来更多干扰。

num_neighbors=20 指每点用于描述局部形状的邻点数，地图与实时点都用它。它不是20个匹配候选或20帧累计。范围太大可能混入墙角两面、坡面与障碍等不同结构，太小又可能受噪声影响；当前库实际找到的邻点不足5时，会设为无效局部特征的默认值，因此稀疏区域与降采样尺度要一起看。

num_threads=4 控制邻域估计、建树及匹配的并行计算。降采样调用没有传这个参数，当前库该函数默认4线程；把参数改大，不等于所有步骤都跟着增加线程。它不是节点频率，也不保证线程翻倍就快一倍，先沿用当前值。

map_frame、odom_frame、use_sim_time 与输出边界（地图、里程计参考系与时间基准）

地图匹配结果是“目标地图←输入累计点云”的变换；输出直接命名为 map→odom。要使这个名字正确，必须先保证目标PCD和输入实际坐标符合约定；本节点没有另行变换到这两个名称指定的系。

输出约20 Hz真实时间重复当前结果，点云匹配则约2 Hz真实时间尝试一次；20 Hz不表示每次都得到新定位。TF时间取最后收到的扫描时间加0.1秒，固定在源码中，不是本块参数；也没有“已成功匹配过”的输出保护，启动初值同样可被发布。这里只记录输出接口，仍不展开此前搁置的导航走停或TF时效故障排查。

use_sim_time 应与输入、机械TF和启动模式一致。True影响节点当前时间与TF缓存，不会让两个wall timer随仿真倍速自动调整，也不会修复此前Point-LIO点时间单位或内部位姿转换问题。

启动来源与当前验证范围

13项自定义参数均在构造时声明和读取，没有动态参数更新回调；init_pose 用于启动，人工初值走 initialpose 消息。13项之外的ROS标准 use_sim_time 由节点时钟处理，不能混为算法参数动态更新。

• 源码能确认： 参数读取、地图一次性转换、累积配准点云、平方距离门槛、启动初值与人工初值的区别、匹配收敛后的更新规则和输出接口。
• 仿真已观察： 2026-10-08仅验证独立Point-LIO静止和约20 cm往返；未验证本节点、先验地图匹配、独立云台yaw、人工初值或全局定位。
• 必须实物测量／文件来源核实： PCD与二维地图的共同基准、建图与启动云台零位、雷达／内部IMU／底盘外参、地图米制尺度及实际起点、可见结构与动态干扰、正确位置下的匹配稳定性。当前重力方向和点时间问题继续保留待核实。

开发问题

关掉small_gicp

主要调整是关闭 small_gicp、增加静态 map → odom；导航节点继续复用原来的 navigation_launch.py

日志某些报错

GICP did not converge.

点云对不上，要自己跑一边slam建出来PCD，然后再使用这个PCD进行匹配。

No accumulated points to process.

这条提示表示 small_gicp 的实时点云缓存为空，发生在匹配之前。现在先查数据有没有送到，暂时不用调匹配参数或初始位姿。

它默认订阅的是 /registered_scan，数据链是：

MID360 → Point-LIO → /cloud_registered
                         ↓ loam_interface
                   /registered_scan → small_gicp