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

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

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

This node operates inexisting-map navigation mode slam:=False , aligning the current registered point cloud with a prior PCD map and outputting position and orientation corrections for map → odom . It does not directly control wheels or the gimbal, nor does it recreate terrain obstacles. The complete slam:=True launch does not run it.

There is currently no physical unit, nor any valid relocalization test for this node. The following separately explains "how the code interprets these values" and "whether they apply to a real vehicle," and does not treat the source code flow as an already-verified localization result.

1.2.参数

1.2.1.ros__parameters (ROS node parameters)

1.2.1.1.Parameter table

The first 12 items correspond to the current physical YAML;prior_pcd_file although they are commented out, they are passed in separately by launch;input_cloud_topic they are not written in this YAML and still use the source code default values. These two items are also listed to facilitate a complete understanding of the input sources.

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: ""

This block has no custom parameters that are currently completely unused:robot_base_frameparticipates only when a manual initial pose is received,init_poseinitial values are set only at startup; they are not continuously adjusted items. Topics, coordinate frames, and paths cannot be tuned by numerical values.

1.2.1.2.Parameter Details

1.2.1.2.1.Input and Output: Which two point clouds are being matched

The real-time point cloud has already been registered by Point-LIO and the interface node to odom. This node directly accumulates coordinates, no longer converting to the radar frame frame by frame, nor additionally rotating these points by the gimbal yaw. When the gimbal rotates, the static environment should still be aligned under the common odom coordinates; this conclusion depends on the correctness of the upstream pose, installation extrinsics, and time interface, and there is currently no independent yaw verification.

The callback only saves the most recent input frame_id and timestamp, does not check whether multiple frames belong to the same system, and does not clear the accumulated points when frame_id changes. PCD does not have a frame_id like ROS messages, and this node processes based on its own source assumptions; the filename and coordinate frame string cannot prove that the map already corresponds.

Every 500 ms of real time, one matching attempt is made, and processing is performed only if there are points; each round involves downsampling, estimating the local shape, and searching for corresponding points in the map, with at most 10 optimization iterations. The matching report updates the result only upon convergence; if it does not converge, the old result is retained, but the points accumulated in this round are still cleared. Convergence means the algorithm's stopping condition is satisfied, and does not prove that the correct location in the field was matched; the source code no longer checks credibility based on matching error, valid correspondence ratio, etc.

1.2.1.2.2.prior_pcd_file: First, confirm that you are using your own venue map

The default path for the entire startup is in the shared directory after installation: pb2025_nav_bringup shared directory:

The corresponding default hardware two-dimensional map map/reality/rmul_2024.yaml also does not exist, while the default simulation map does. This only checks current default files; running-node parameters were not read, and users may supply other world, PCD or two-dimensional map paths. Prepare corresponding maps for the actual site before running on hardware. An existing simulation PCD does not establish hardware-map readiness.

localization_launch.py After the parameter file, additionally pass in {"prior_pcd_file": prior_pcd_file}. Therefore, commenting out this line does not mean the node does not use the prior PCD. It is a different function from whether Point-LIO is enabled prior_pcd_en: Point-LIO may not load the prior, but the relocalization node still requires the prior PCD.

If file loading fails, only the error is logged and the function returns; the constructor still continues to prepare the matching target, and there is no explicit prevention of an empty target from entering subsequent matching; the fact that "the node started up" cannot be regarded as successful map loading. The source code will not re-read the PCD at runtime, nor will it verify for you whether it is consistent with the Nav2 2D map in position, orientation, metric scale, or site version.

1.2.1.2.3.base_frame and lidar_frame: at startup, the entire map is rotated

After the PCD is read in, the source code queries base_footprint ← front_mid360, then left-multiplies this matrix onto every point of the entire map. It queries based on the node's current time, and after a failure retries every second, continuing once it succeeds; there is no upper limit on the number of queries, nor does it update the target map during subsequent gimbal rotation.

This corresponds to a source assumption in the code: the PCD baseline is the initial lidar_odom, and the map is transformed into the desired reference using the initial chassis-to-lidar relationship. However, the current Point-LIO actually stores registered world points under camera_init and its pose origin involves the internal IMU; it is not the lidar's own coordinate frame for each frame. Whether treating such PCDs as the initial lidar frame and directly applying the mechanical transform is consistent with the actual mapping baseline must be re-verified; one must not assume the conversion is correct solely based on the README naming.

With the LiDAR on the gimbal, this step particularly depends on the zero position. If relative gimbal yaw differs between creating the PCD and restarting with it, the current base←lidar contains a different angle. According to the code, a different matrix may rotate the target map, while the independently loaded two-dimensional map does not adjust automatically. Whether this actually causes misalignment depends on the PCD generation start pose, gimbal zero position, chassis heading and map origin; no test conclusion is available yet.

This query is related to the one-time benchmark conversion intent of loam_interface, but the two nodes query separately, so it is not guaranteed that the gimbal angles are obtained at the same instant. The mechanical relationship here is always preserved: the MID360 is mounted on gimbal_yaw; only the actual yaw drives it, while the actual pitch is located in a subsequent layer; the two odom joints are fixed. Errors in the gravity reference, the internal IMU-to-lidar relationship, or the map source cannot be masked by increasing the matching distance.

The README in the package also stated that "an empty transform is used when unset," but the current implementation has no corresponding identity-transform bypass: if the PCD can be read but frame is empty or the TF is missing, it enters retry. The current source code should be taken as authoritative, and no changes will be made to the node or map transform in this round.

1.2.1.2.4.init_pose and robot_base_frame: The two initial poses are not the same object

YAML's init_pose is directly written into the matching matrix. Under the convention that the current input is odom and the target is map, it is the map←odom initial value, that is, "the position and orientation of the odom origin in the map".

[0,0,0,0,0,0] indicates an initial guess that the two frames are fully coincident; this is appropriate only when the initial reference relationship matches, and it does not mean the algorithm already knows the robot's true position in the map. Position units are m, angle units are rad; a yaw of 90° should be approximately 1.5708, not 90. Rotation is composed in the order yaw, pitch, roll Rz×Ry×Rx.

For example: if the odom origin happens to coincide with the described robot origin and has the same orientation, then the robot's initial map pose might be map←odom; if there is a height or mounting offset between the odom origin and the gimbal, or if the robot has already moved within odom, you cannot directly copy the robot position. Currently the chassis, gimbal, and lidar origins are different, so the calculation must be based on the actual reference relationships.

topic initialpose is treated as robot_base_frame=gimbal_yaw the pose in the map. callback then queries the latest gimbal_yaw←输入扫描系. The current scan frame should be odom, and multiplying the two yields map←odom:

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

Therefore, if you interpret the RViz arrow as the chassis front, while the current gimbal is turned 90° left relative to the chassis, you may miss a gimbal angle. The position also corresponds to the gimbal origin: if the manual message gives z=0, the source code treats it as the gimbal origin being at z=0 in the map, not as "the chassis ground height being 0"; the height should be checked against this map's datum. Changing only the name of robot_base_frame also requires rechecking the entire relationship; the three characters "robot" cannot automatically be equated with the chassis. This parameter is only used in this callback and is not deducted as a mechanical angle during each point cloud matching.

The callback does not transform the pose according to the message header.frame_id, does not use its covariance, and does not look up TF by message time; instead, it takes the latest transform. Before the first scan is received, the scan frame_id is still empty, so the query may fail. Manually setting the initial value only changes the current and next matching guesses; it does not re-transform the PCD, nor does it clear the accumulated points. When the gimbal rotates rapidly, it is also necessary to verify whether the orientation corresponding to the manual pose and the latest TF are at the same time.

1.2.1.2.5.max_dist_sq: 1.73 m is a pairing threshold, not a positioning error metric

The small_gicp code used by the current build directly checks whether squared correspondence distance exceeds max_dist_sq and rejects the pair if it does. Source points are first transformed into the map using the current estimate, then paired with their nearest map points before this check. With metric point clouds, the units are therefore m²:

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

The second line only explains unit conversion and is not a recommendation to modify the current value. It does not limit the robot's total travel distance, does not mean that a positioning deviation of 1.73 m is allowed, and is even less a scanning range. Under an incorrect initial value, the source point may still happen to be close to another wall and pass the threshold.

On hardware, relaxing this threshold retains more point pairs, but repetitive pillars, similar corridors and dynamic objects may also create incorrect matches. Tightening it reduces distant mismatches but may leave too few pairs when the true initial error is somewhat larger. First verify the map, initial estimate and reference frames, then tune using evidence of correct and incorrect matches. Smaller alone does not mean more accurate.

This unit was verified against the currently built cached library source code; the library commit is fa0cfc983417c044b6036f71dfa448038bb9c820. CMake references master; after re-fetching dependencies, the version should be re-verified, and the current cached behavior should not be treated as a fixed commitment for all versions.

1.2.1.2.6.Two leaf_size, num_neighbors, and num_threads (point cloud simplification, number of neighbors, and computation threads)

global_leaf_size=0.1 m Process the prior map;registered_leaf_size=0.05 m Process the accumulated real-time point cloud for each round. The two are not required to be the same, nor are they equal to the terrain analysis grid, Nav2 map resolution, or localization accuracy. Map processing will not modify the original PCD, and real-time processing will not change the output of Point-LIO itself.

Reducing the setting usually retains more details, at the cost of increased computation; details absent from the raw data will not appear merely because the setting is reduced. Whether distant sparse points, wall corners, or small pillars can be described stably depends on the actual data; dynamic points such as personnel and enemy vehicles are not specifically filtered out at this node, and more real-time points may also introduce more interference.

num_neighbors=20It refers to the number of neighboring points used at each point to describe the local shape, and it is used for both the map and real-time points. It is not 20 matching candidates or an accumulation of 20 frames. If the range is too large, it may mix in different structures such as two sides of a wall corner, a slope surface, and an obstacle; if it is too small, it may be affected by noise. When the current library actually finds fewer than 5 neighboring points, it is set to the default value for an invalid local feature, so sparse regions and the downsampling scale should be considered together.

num_threads=4Control parallel computation for neighborhood estimation, tree building, and matching. The downsampling call does not pass this parameter, and in the current library this function defaults to 4 threads; increasing the parameter does not mean that all steps will increase their thread count accordingly. It is not the node frequency, nor does it guarantee that doubling the threads will double the speed; for now, continue using the current value.

1.2.1.2.7.map_frame, odom_frame, use_sim_time and output boundaries (map, odometry reference frame, and time base)

The map matching result is the transformation from "target map ← input accumulated point cloud"; the output is directly named map→odom. To make this name correct, the target PCD and the input actual coordinates must first conform to the convention; this node does not separately transform into the frames specified by these two names.

Output repeats the current result at approximately 20 Hz real time; point cloud matching attempts once at approximately 2 Hz real time; 20 Hz does not mean a new localization is obtained every time. The TF time takes the last received scan time plus 0.1 seconds, fixed in the source code, not a parameter of this block; there is also no output protection for "has already been successfully matched," and the startup initial value can likewise be published. Here only the output interface is recorded; the previously shelved troubleshooting of navigation start-stop or TF timeliness faults is still not expanded.

use_sim_time should be consistent with the input, mechanical TF, and startup mode. True affects the node's current time and TF cache; it will not cause two wall timers to automatically adjust with the simulation speed multiplier, nor will it fix the previous Point-LIO point time unit or internal pose transformation issues.

1.2.1.2.8.Startup source and current verification scope

All 13 custom parameters are declared and read at construction time; there is no dynamic parameter update callback;init_pose is used for startup, and manual initial values go through the initialpose message. ROS standard use_sim_time beyond the 13 items are handled by the node clock and cannot be conflated with dynamic updates of algorithm parameters.

• The source code can confirm: Parameter reading, one-time map conversion, cumulative registration point cloud, squared distance threshold, the difference between the startup initial value and the manual initial value, the update rule after matching convergence, and the output interface.
• The simulation has observed: 2026-10-08 only verified independent Point-LIO stationary and approximately 20 cm round trip; did not verify this node, prior map matching, independent gimbal yaw, manual initial value, or global localization.
• Must be verified by physical measurement / file sources: The common reference of the PCD and the 2D map, the gimbal zero position for mapping and startup, the lidar / internal IMU / chassis extrinsics, the map metric scale and actual starting point, visible structures and dynamic interference, and matching stability at the correct position. The current gravity direction and point timing issues remain pending verification.

1.3.开发问题

1.3.1.关掉small_gicp

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

1.3.2.日志某些报错

1.3.2.1.GICP did not converge.

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

1.3.2.2.No accumulated points to process.

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

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

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