Ignore points closer than a threshold

[Adam Trizuljak]

Hello, I am trying to use STVL in conjunction with AirSim to simulate a drone with a depth camera. The problem is that the point cloud from the virtual depth camera also contains parts of the drone, which are then added into the STVL map as obstacles:

The obvious way to fix this is to move the camera forward so it doesn't see parts of the drone (this can be adjusted in AirSim). However, is it possible to configure STVL to ignore points that are closer than a threshold? I tried adding the min_z = 1.0 and max_z = 7.0 parameters to my marking source, as I understood that they define the near and far planes of the frustum. However this has no effect, the near points are still added to the map regardless of the min_z value.

I am using ROS Melodic and STVL is built from source. This is my config:

local_costmap:
  global_frame: world_enu
  robot_base_frame: base_link
  update_frequency: 5.0
  publish_frequency: 5.0
  plugins:
    - {name: rgbd_obstacle_layer,     type: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"}

  rgbd_obstacle_layer:
    enabled:                  true
    voxel_decay:              15    # seconds if linear, e^n if exponential
    decay_model:              0     # 0=linear, 1=exponential, -1=persistent
    voxel_size:               0.25  # meters
    track_unknown_space:      true  # default space is known
    max_obstacle_height:      5.0   # meters
    unknown_threshold:        15    # voxel height
    mark_threshold:           0     # voxel height
    update_footprint_enabled: true
    combination_method:       1     # 1=max, 0=override
    obstacle_range:           4.0   # max obstacle distance to insert into map, meters
    origin_z:                 0.0   # meters
    publish_voxel_map:        true # default off
    transform_tolerance:      0.2   # seconds
    mapping_mode:             false # default off, saves map not for navigation
    map_save_duration:        60    # default 60s, how often to autosave
    observation_sources:      rgbd1_mark rgbd1_clear
    rgbd1_mark:
      data_type: PointCloud2
      topic: /rgbd/depth
      marking: true
      clearing: false
      min_obstacle_height: -2.0     # default 0, meters
      max_obstacle_height: 5.0     # default 3, meters
      expected_update_rate: 0.0    # default 0, if not updating at this rate at least, remove from buffer
      observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest
      inf_is_valid: false          # default false, for laser scans
      voxel_filter: true          # default off, apply voxel filter to sensor, recommend on
      voxel_min_points: 5          # default 0, minimum points per voxel for voxel filter
      clear_after_reading: true    # default false, clear the buffer after the layer gets readings from it
      max_z: 7.0                  # default 0, meters
      min_z: 1.0                  # default 10, meters
    rgbd1_clear:
      data_type: PointCloud2
      topic: /rgbd/depth
      marking: false
      clearing: true
      max_z: 7.0                  # default 0, meters
      min_z: 0.1                  # default 10, meters
      vertical_fov_angle: 0.8745  # default 0.7, radians
      horizontal_fov_angle: 1.048 # default 1.04, radians
      decay_acceleration: 5.0     # default 0, 1/s^2. If laser scanner MUST be 0
      model_type: 0                # default 0, model type for frustum. 0=depth camera, 1=3d lidar like VLP16 or similar

[Steve Macenski]

Min_z impacts the clearly frustum logic, not the marking logic. Typically, if you have occlusions in your sensor field of view due to your platform, you’d use a robot self filter or similar as a preprocessing step to the data into the navigation stack. That would go for this layer and any others that might be using that data stream. There are several packages that can do this, but for example in 2D lidars there’s the Filters project that have length and angular range filters to remove such points. I’d suggest looking into those.

[Adam Trizuljak]

Thanks for the clarification about min_z and max_z. Since there already is a maximum obstacle_range parameter, I was expecting that there would also be a parameter for minimum obstacle range. After some digging I found that it could be added to this line in spatio_temporal_voxel_grid.cpp if needed. In my case, using something like the rgbd_self_filter package is overkill, if the same thing can be accomplished just by changing an if condition. For now I just moved the camera forward in AirSim.