terrain_analysis（近场地面与障碍高差分析）

1.terrain_analysis（近场地面与障碍高差分析）

1.1.地形分析节点分工

Both nodes subscribe to the loam_interface of lidar_odometry and registered_scan; ext also subscribes to terrain_map and is currently centered on the position from the input odometry, using near-field results within 4 m, computing extended results beyond 4 m, and then merging the outputs. This block only organizes terrain_analysis; the parameters of terrain_analysis_ext are left to the next block.

This node estimates the ground height within a horizontal grid of odom. For output points, intensity is changed to the height difference relative to the ground, in meters, and is no longer radar reflection intensity. The current local costmap's IntensityVoxelLayer filters obstacles by this field, with an allowed range of 0.1 to 2.0 m, and additionally checks conditions such as point coordinate height and distance; it does not add positive and negative heights together as a traversal cost.

1.2.上坡下坡先看哪些参数

First, grasp two questions:whether a normal slope would be mistaken for an obstacle, and whether ground points on the slope have been filtered out. This passage discusses how the navigation system views slopes; there is currently no slope test, so these values cannot be used to determine the maximum gradient the actual vehicle can climb.

The "high" and "low" here are calculated according to the height direction currently used by the code; the actual coordinate direction still needs to be verified. A normal downhill slope is a continuously descending ground surface; a pit, cliff, or downward step involves a sudden drop in the ground, and they cannot all be classified as the same "downhill switch."

For example, when all other conditions are met: a point on the same slope, if calculated as 6 cm higher than this small patch of ground, will be below the current 10 cm obstacle threshold; if the ground height is underestimated and it is calculated as 15 cm higher, it may be drawn as an obstacle. This islocal ground height difference, not the total height of the entire slope. Adjusting the ground estimate or raising the obstacle threshold may reduce slope misjudgments, but may also cause real low obstacles to be ignored; you cannot judge only by whether the slope can be passed.

vehicleHeight=0.5What is limited is the height difference of the normal output point relative to the estimated ground. It does not mean that "the vehicle can climb a 0.5 m high step", nor does it specify the permissible slope.

Therefore, ramp analysis should first follow this order:Whether the ground elevation is selected correctly → whether ramp points have been deleted → whether downstream processing treats normal elevation differences as obstacles. Dynamic object cleanup, old point retention time, and update count mainly address other issues; when ramp surface points are missing, then check their indirect effects.

1.3.参数

1.3.1.ros__parameters (ROS node parameters)

1.3.1.1.Parameter table

terrain_analysis:
  ros__parameters:
    scanVoxelSize: 0.05
    decayTime: 0.5
    noDecayDis: 0.0
    clearingDis: 0.0
    useSorting: True
    quantileZ: 0.2
    considerDrop: False
    limitGroundLift: False
    maxGroundLift: 0.3
    clearDyObs: True
    minDyObsDis: 0.3
    minDyObsAngle: 0.0
    minDyObsRelZ: -0.3
    absDyObsRelZThre: 0.2
    minDyObsVFOV: -28.0
    maxDyObsVFOV: 33.0
    minDyObsPointNum: 1
    noDataObstacle: False
    noDataBlockSkipNum: 0
    minBlockPointNum: 10
    vehicleHeight: 0.5
    voxelPointUpdateThre: 100
    voxelTimeUpdateThre: 1.0
    minRelZ: -1.5
    maxRelZ: 0.5
    disRatioZ: 0.2

1.3.1.2.Parameter Details

1.3.1.2.1.scanVoxelSize (point cloud downsampling size)

The current value of 0.05 m represents the 3D voxel edge length during cumulative point cloud resampling. Points within the same small voxel are merged into a representative point, which can compress repeated scans; increasing this value may reduce narrow obstacles, edges, and valid ground samples, while decreasing it increases the number of points and processing load.

The source code additionally fixes a historical cache chunk of 1.0 m and a planar ground grid of 0.2 m.scanVoxelSizeNot changing the sizes of these two grids does not equal the costmap resolution. Ground samples come from the 3×3 neighborhood of the planar grid,minBlockPointNumand what is judged is the number of height samples collected in this neighborhood.

1.3.1.2.2.decayTime, noDecayDis, and clearingDis (retention of old points, decay exemption, and event-based image clearing)

When a new point enters the cache,intensitytemporarily store "the frame timestamp minus the timestamp of the first point cloud" for history management; when outputting, change it to height difference. The voxel filter of the container PCL averages each field by default, so the age determination targets the resampled representative point and cannot be understood as accurately preserving time for each original point.

The cache cell redoes downsampling and filtering only when any of the following conditions is met: the cumulative number of new input points reaches 100, 1.0 second has elapsed since the last update, or a map-clearing event is received. During the check, points are retained according to the following relationship:

高度在缓存允许窗口内
且（代表点年龄 < decayTime，或水平距离 < noDecayDis）
且不属于本次触发清图的半径内

Therefore, decayTime=0.5 is the age threshold checked during updates, not something that disappears immediately when it reaches 0.5 seconds. A sparse cache cell may wait until the time update threshold before being rechecked; when there is no new point cloud, this processing will not execute. Increasing the decay time helps retain ground samples, but may also prolong the residual presence of old objects; if decreased, it is necessary to observe whether holes appear in sparse cells.

Current noDecayDis=0, there is no close-range attenuation-free zone. This value also participates in the initialization of the no-data function: only after moving at least this distance from the recorded position does it enter the no-data processing state; when 0, the distance condition is satisfied on the first odometry callback; current noDataObstacle=False; the latter does not generate obstacles.

clearingDis Only used after a clear-map event is triggered; what gets cleared are the points of d < clearingDis. Currently 0 has no positive-radius clearing area; the 6th button of joy can trigger the clear map, and the Float32 message of map_clearing will override the clear-map radius and trigger processing. The original comment "points beyond this distance will be cleared" has the opposite direction and cannot be understood that way. Clear map only affects the cache of this node, not deleting the Point-LIO map; no clear-map message was published in this round.

1.3.1.2.3.useSorting, quantileZ, and considerDrop (ground elevation selection and handling of elevation difference at low positions)

For each 0.2 m planar cell, first collect the neighborhood heights.useSorting=True sort them and take the height at index floor(quantileZ×N); currently quantileZ=0.2. For example, with 10 samples, index 2 is taken, i.e., the 3rd sample from low to high. It can reduce the influence of extremely low outliers on the "lowest ground," but an excessively high quantile ratio may also treat the bottom of obstacles or higher parts of slopes as ground.

The ground height is recorded as g, and the original height difference is h=z−g:

• Current considerDrop=False: output only non-negative height differences; points below the estimated ground are filtered out.
• True: first take abs(z−g); concave points may also be output as height differences; they still need to pass other height and sample count conditions.

The source code does not have a step to cancel out the positive and negative slope heights by summing them, nor does it prohibit these two switches from being enabled at the same time. They can be executed simultaneously, but the lower part of the slope may be treated as an obstacle after taking its absolute value.useSortingis a ground estimation method. Whether the vehicle can climb a slope also depends on the coordinate axes, slope angle, height window, downstream obstacle threshold, and vehicle capability. It cannot be guaranteed that "enabling it will allow slope climbing."

The current lower bound of the costmap intensity is 0.1 m. An elevated ground estimate reduces the height difference of protrusions, potentially causing low obstacles to fall below this threshold; an underestimated value may instead label normal ground as an obstacle. Subsequent physical tests should simultaneously check the ground, low obstacles, and slopes, rather than only checking whether the point cloud is continuous.

1.3.1.2.4.limitGroundLift and maxGroundLift (ground estimation lift limits)

With clamping enabled and sorting in use, the ground estimate will not be higher than "the lowest height in the neighborhood + maxGroundLift". It limits the elevation of the percentile ground relative to the lowest sample, not gimbal elevation, chassis ground clearance, or the per-frame ground change rate.

Currently limitGroundLift=False，therefore maxGroundLift=0.3 does not affect the result, so no adjustment is made for now. If the minimum sample itself is a low outlier, enabling the limit may also push the ground lower.

1.3.1.2.5.clearDyObs and its geometric threshold (dynamic residual point filtering)

This switch uses old points and the latest echo to perform grid-level candidate filtering. It does not estimate object velocity, nor does it create areas that are permanently prohibited once a dynamic object has passed through. The candidate count is rebuilt each round, and only grids still marked in the current round are filtered out of the output; the removal of cached old points is handled separately by rules such as decay.

The current process is:

1. In the accumulated point cloud, points whose horizontal distance is not greater than minDyObsDis=0.3 m directly add minDyObsPointNum to the grid; near points are not ignored.
2. Farther points first satisfy angle1 > minDyObsAngle and are then transformed into the LiDAR's own frame corresponding to the input pose; if their own elevation angle is within the field-of-view window, or abs(z_L)<absDyObsRelZThre, the grid is counted.
3. In the cropped points of the latest frame, as long as the grid has a point that satisfies angle1 > minDyObsAngle, the count is reset to zero; this round no longer requires the field-of-view window or the 0.3 m distance threshold.
4. Grids whose final count reaches minDyObsPointNum=1 do not enter the normal terrain output.

The first elevation angle here is calculated according to odom the position relative to the input pose:

$$\theta_1=\operatorname{atan2}(\Delta z-\text{minDyObsRelZ},d)\frac{180}{\pi}$$

At nonzero horizontal distance, the current 0° angle threshold and -0.3 m height offset are equivalent to requiring Δz>-0.3 m。minDyObsAngle and minDyObsRelZ affect both historical candidates and resetting by the latest returns. The net clearing effect after adjustment cannot be inferred from only one direction of change.

The second elevation angle is calculated in the radar's own coordinate system, also in degrees. The current window is (-28°, 33°), and additionally abs(z_L)<0.2 m can bypass this window. The MID360's official specified vertical field of view is -7° to 52°; this differs from the current software window, but the window also serves for candidate screening, so it cannot be directly concluded that the two values must be replaced based on this.Livox official specifications

Before running on the real hardware, it is necessary to verify whether the input pose represents the IMU or the LiDAR, the axes after inverse rotation, and the filtering purpose of this window. The LiDAR's own frame has already accounted for the mounting orientation; the overall tilt angle should not be arbitrarily added to the upper and lower bounds of the field of view. Also, thresholds in degrees here must not be mixed with radian-based JointState. The mechanical convention that the MID360 rotates with the actual yaw but not with the actual pitch is retained.

Occlusion, sparse echoes, or pose errors may also cause static obstacles to temporarily lack "in-frame confirmation," leading to incorrect filtering. The current minDyObsPointNum=1 is relatively sensitive; one should observe the residuals after obstacles leave and the results when static objects are occluded before deciding whether adjustment is needed, rather than directly disabling or modifying it based on the original comment.

1.3.1.2.6.noDataObstacle, noDataBlockSkipNum, and minBlockPointNum (no-data region and minimum ground samples)

noDataObstacle=True and when initialization is complete, cells with insufficient ground samples will generate 4 obstacle marker points,z set as the input pose height,intensity set as vehicleHeight. This indicates actively blocking under-data regions; False only means not generating these points, and how unknown regions are traversed is still determined by downstream.

noDataBlockSkipNumThe number of skip iterations that controls the under-data grid-edge boundary cannot be understood in terms of meters or skipping a certain number of frames. The no-data function is currently disabled, so this item does not affect the results; a brief description of its purpose is sufficient.

minBlockPointNum=10Still in effect: normal terrain point output requires at least 10 neighboring ground samples. Increasing it reduces sparse cell output; decreasing it may retain more small obstacles, but also relies more on a small number of unstable ground samples. It is also affected by downsampling scale, decay time, and actual point density.

1.3.1.2.7.vehicleHeight (terrain output height difference upper limit)

Normal terrain point requirement 0≤h<vehicleHeight; currently this is 0 to 0.5 m, excluding the upper bound of 0.5 m.h relative to the estimated ground,minRelZ/maxRelZ then relative to the z of the input radar pose,odom the two use different reference frames.

This parameter does not directly control the model dimensions or the collision envelope. If an obstacle has only valid echoes above 0.5 m, these points will be filtered out; decreasing it may miss obstacles, while increasing it may introduce more suspended structures into the output. It must be verified against the real vehicle envelope, the obstacle parts visible to the radar, and downstream filtering; it cannot be set to the full vehicle height based on the name alone.

The current normal output height difference also interacts with the downstream 0.1 m obstacle lower limit; therefore, for normal points entering the local obstacle marking, the height difference usually needs to fall between 0.1 and 0.5 m and pass other filters. Whether the current values are suitable for a real vehicle still lacks measurement-based conclusions.

1.3.1.2.8.voxelPointUpdateThre and voxelTimeUpdateThre (cached point cloud update thresholds)

For each 1.0 m cache block, when the new entry point count reaches 100 or 1.0 second has elapsed since the block was last updated, re-downsample and filter out expired points; a map clear event also forces an update. The count is reset to zero after the update, and timing uses the input point cloud time.

Increasing the threshold generally reduces the update frequency at this stage, while decreasing it increases the number of processing runs. These are not publishing frequency parameters, nor are they ground sample count thresholds; publishing is driven by new point cloud processing, and the actual frequency is also affected by load. Continue using the current values for now; if historical residue or computational burden becomes abnormal, then check together with decayTime.

1.3.1.2.9.minRelZ, maxRelZ, and disRatioZ (height window relative to the input position)

Let Δz=z_point−z_input_pose , d be the horizontal distance in odom . When entering the cache and updating the cache, use:

$$\text{minRelZ}-\text{disRatioZ}\,d <\Delta z< \text{maxRelZ}+\text{disRatioZ}\,d$$

Currently at a horizontal distance of 5 m, the cached candidate window is (-2.5, 1.5) m. However, when collecting ground samples and outputting valid points, the source code still requires a fixed -1.5<Δz<0.5and no longer uses distance expansion.

Therefore, disRatioZ can relax the cache candidate range, but it cannot guarantee that points on distant slope surfaces beyond the fixed window are ultimately retained. The original annotation only explained the previous stage; based on that alone, one cannot conclude that the entire chain expands the height range with distance.

These heights follow the z axis of odom and are relative to the input lidar_odometry position, rather than the chassis origin or gimbal pitch layer. First confirm LiDAR-to-ground height, the direction of odom , and the IMU-to-LiDAR pose transformation. Simply widening the window cannot fix a reference-frame error. This round changes neither gravity direction nor any engineering parameters.

1.3.1.2.10.Startup source and current verification scope

The complete reality and simulation launches default to nav2_params.yaml in their respective directories. They pass the processed parameter file to the node through navigation_launch.py and set its name to terrain_analysis to match the YAML. The package’s standalone XML launch and C++ defaults are separate sources. For example, the standalone XML’s decayTime=1.0、noDecayDis=1.75、vehicleHeight=1.5 cannot be assumed to match this table. Running the executable directly does not automatically load the complete YAML.

These 26 items are all declared and read at startup, with no dynamic parameter update callback.maxGroundLift and noDataBlockSkipNum currently do not affect the result because the switch is off;clearingDis are used by events and can be overridden by messages, and changes to their variables cannot be determined solely from the startup parameter table.

The source code also has interface preconditions: the two inputs are called back separately, using the most recently received odometry, with no time pairing, interpolation, or protection for "valid odometry obtained"; the output reference frame is fixedly written as odom, with no validation or transformation of the input reference frame. The upstream pose and coordinate relationships still need to be verified, which will especially affect the height window and dynamic candidate determination of this block.

• The source code can confirm: parameter reading, units, gating branches, decay and map clearing conditions, quantile ground estimation, dynamic candidate filtering, and height output range; in this round, the installed navigation launch and two YAML files are confirmed in the container to correspond to the current source code.
• Simulation has observed: 2026-10-08: only the stationary and short-distance round-trip output of the standalone Point-LIO was verified; this node's ground estimation, slope surfaces, dynamic clearing, under-data obstacles, or final costmap were not verified.
• Must be verified physically: the height from the LiDAR origin to the ground, installation and internal extrinsics, target reference frame direction, actual point density, visible obstacle height, slope and drop features, and the filtering performance when dynamic objects leave or static objects are occluded. There is currently no physical unit, and this round did not start the node or perform runtime tests.

Source code location: src/pb2025_sentry_nav/terrain_analysis/src/terrainAnalysis.cpp; downstream intensity filtering is located at src/pb2025_sentry_nav/pb_nav2_plugins/src/layers/intensity_voxel_layer.cpp. This block corrects the parameter explanations in the log; project source code, configuration, and parameter values are all retained.