LiDAR intrinsics are inconsistent with the received pointcloud. Failing integration.

[hofbauerc]

Hello Everyone,

I am trying to use isaac_ros_nvblox using a pre-recorded ros2 bag. For this matter I wanted to use an ouster OS1 Lidar sensor (Gen 1) with following parameters: lidar_width: 1024 lidar_height: 64 lidar_vertical_fov_rad: 0.5215044 min_angle_below_zero_elevation_rad: -0.2607522 max_angle_above_zero_elevation_rad: 0.2607522

However, during execution of the node I always get following error in the beginning: [ERROR] [1718020447.830592432] [nvblox_node]: LiDAR intrinsics are inconsistent with the received pointcloud. Failing integration.

Due to this I get no outputs of the node itself. All the resulting pointclouds (e.g. ESDF) are published but with 0 points in them.

Here is my current configuration for the node:

nvblox_node:
  ros__parameters:
    # miscellaneous
    voxel_size: 0.05
    num_cameras: 0
    use_tf_transforms: true
    use_topic_transforms: false
    # multi mapper
    mapping_type: "static_tsdf"  # ["static_tsdf", "static_occupancy"]
    connected_mask_component_size_threshold: 2000

    global_frame: "base_link"

    # Parameters governing frequency of different processing steps in the reconstruction pipeline.
    # Processing happens every n:th tick_period. <=0 means that no processing take place
    tick_period_ms: 10
    integrate_depth_rate_hz: 40.0
    integrate_color_rate_hz: 5.0
    integrate_lidar_rate_hz: 40.0
    update_mesh_rate_hz: 5.0
    update_esdf_rate_hz: 10.0
    publish_layer_pointcloud_rate_hz: 5.0
    decay_tsdf_rate_hz: 5.0
    decay_dynamic_occupancy_rate_hz: 10.0
    clear_map_outside_radius_rate_hz: 1.0

    # printing statistics on console
    print_rates_to_console: true
    print_timings_to_console: true
    print_delays_to_console: true
    print_statistics_on_console_period_ms: 10000

    # esdf settings
    esdf_mode: 0 # 0: 3d, 1: 2d
    publish_esdf_distance_slice: true
    # color settings
    use_color: false
    # depth settings
    use_depth: false
    # lidar settings
    use_lidar: true
    lidar_width: 1024
    lidar_height: 64
    lidar_vertical_fov_rad: 0.5215044 
    min_angle_below_zero_elevation_rad: -0.2607522
    max_angle_above_zero_elevation_rad: 0.2607522
    use_non_equal_vertical_fov_lidar_params: false
    # Input queues
    maximum_sensor_message_queue_length: 100
    # Map clearing settings
    map_clearing_radius_m: 15.0 # no map clearing if < 0.0
    map_clearing_frame_id: "ouster_1_sensor"
    # QoS settings
    input_qos: "SYSTEM_DEFAULT"
    # Rviz visualization
    slice_visualization_attachment_frame_id: "ouster_1_sensor"
    slice_visualization_side_length: 30.0

    layer_visualization_min_tsdf_weight: 0.1
    layer_visualization_max_tsdf_distance: 0.1
    layer_visualization_exclusion_height_m: 10.0
    layer_visualization_exclusion_radius_m: 7.0
    layer_visualization_undo_gamma_correction: false
    max_back_projection_distance: 7.0
    back_projection_subsampling: 1 # no subsampling if == 1

    static_mapper:
      lidar_projective_integrator_max_integration_distance_m: 30.0
      # mapper
      maintain_mesh_block_stream_queue: true
      do_depth_preprocessing: false
      depth_preprocessing_num_dilations: 3
      # projective integrator (tsdf/color/occupancy)
      projective_integrator_max_integration_distance_m: 5.0
      projective_integrator_truncation_distance_vox: 4.0
      projective_integrator_weighting_mode: "inverse_square_tsdf_distance_penalty"
      projective_integrator_max_weight: 5.0
      # occupancy integrator
      free_region_occupancy_probability: 0.45
      occupied_region_occupancy_probability: 0.55
      unobserved_region_occupancy_probability: 0.5
      occupied_region_half_width_m: 0.1
      # esdf integrator
      esdf_integrator_min_weight: 0.1
      esdf_integrator_max_site_distance_vox: 5.0
      esdf_integrator_max_distance_m: 2.0
      esdf_slice_max_height: 5.0
      esdf_slice_min_height: -5.0
      # mesh integrator
      mesh_integrator_min_weight: 0.1
      mesh_integrator_weld_vertices: true
      # tsdf decay integrator
      tsdf_decay_factor: 0.95
      exclude_last_view_from_decay: true
      # mesh streamer
      mesh_streamer_exclusion_height_m: 2.0
      mesh_streamer_exclusion_radius_m: 7.0
      mesh_bandwidth_limit_mbps: 999999925.0 # in mega bits per second

And an example output of the cloud using ros2 topic echo

header:
  stamp:
    sec: 1708533834
    nanosec: 832583424
  frame_id: ouster_1_sensor
height: 64
width: 1024
fields:
- name: x
  offset: 0
  datatype: 7
  count: 1
- name: y
  offset: 4
  datatype: 7
  count: 1
- name: z
  offset: 8
  datatype: 7
  count: 1
- name: intensity
  offset: 16
  datatype: 7
  count: 1
- name: t
  offset: 20
  datatype: 6
  count: 1
- name: reflectivity
  offset: 24
  datatype: 4
  count: 1
- name: ring
  offset: 26
  datatype: 4
  count: 1
- name: ambient
  offset: 28
  datatype: 4
  count: 1
- name: range
  offset: 32
  datatype: 6
  count: 1
is_bigendian: false
point_step: 48
row_step: 49152
data:
- 0
- 0
- 0
- 0
- 0
- 0
- ...

Am I missing some specific parameter in order for this to work? I already tried multiple variations of the lidar intrinsics, but the result remains the same.

Thanks in advance for the help.

[alexmillane]

Do you have a short rosbag recording of the LiDAR data?

[hofbauerc]

Hi,

Thanks for the reply. Here is a bag (mcap) containing the ouster cloud and the tf from the odometry. In this case base_link is the actual fixed frame, ouster_1_sensor is the frame of the moving sensor. I hope the link works:

https://drive.google.com/file/d/1j8pl06Jko8w4smOG7oAq58NMzTrWdpRU/view?usp=sharing

The sensor itself is an Ouster OS1 Gen1. The defined LIDAR intrinsics should be correct according to the datasheet.

[LukasBergs]

I am having the same issues with an Ouster OS1-128 LiDAR. @alexmillane, if you need more information, please let me know.

[pglira]

I'm also having the same issue with an ouster lidar. Any suggestions or workarounds?

[hofbauerc]

For anyone still having this issue I managed to get it working using data from an Ouster OS1 Gen1.

The check fails because of 2 reasons:

• In my case there were points with x, y, z values being 0 wich leads to wrong calculations in the following code: nvblox
• Additionally I created a ros2 node which performs the same check as in nvblox. This removes some points which don't fall in the exact FoV defined in the datasheet (due to uncertainties of the sensor during point acquisition). Additionally I remove NaN and 0 points in this node.

With those points removed the LIDAR Intrinsics Check works without issues. The problem is, that if one point fails during the LIDAR Intrinsics Check, the node basically stops working. Therefore removing them before that solves the issue.

Maybe adding a feature that removes those points could be added to node? For example using a flag to remove points outside the defined FoV?

[alexmillane]

Thank you for your detailed explanation. I'm glad that you got this working. Internally, we're focusing on stereo/depth cameras at the moment so we're less likely to see shortcomings in the LiDAR pipeline.

If you format the changes above as a pull-request I'm happy to review it and get it in. I would suggest that we take the removal of the bag points into the nvblox_node (rather than a separate node), and put it behind a flag, as you suggest.

[hofbauerc]

@alexmillane Thanks for the reply, if I have the time I will consider adding the functionality.

However, one last thing that I can't quite understand is the dynamic mapping mode using the LiDAR as input. Is this functionality supported? It works with the realsense example without issues but when I try to use only LiDAR data as input for nvblox and set the mapper to dynamic (using mostly the standard config values) I can't get output in terms of dynamic ESDF. The static one is working without issues. The dynamic one gets published but has no data in it. Is there a specific parameter that I am missing for dynamic mapping to work with the LiDAR as input?

In the realsense launch files the LiDAR as input get deactivated as soon as the mode is set to dynamic which raises my concern that this functionality isn't supported. Can you give me a quick answer if this should work or not?

Here are my current params, but as I said those are mostly the standard values:

nvblox_node:
  ros__parameters:
    # miscellaneous
    voxel_size: 0.2
    num_cameras: 0
    use_tf_transforms: true
    use_topic_transforms: false
    # multi mapper
    mapping_type: "dynamic"  # ["static_tsdf", "static_occupancy"]
    connected_mask_component_size_threshold: 2000

    global_frame: "world"

    # Parameters governing frequency of different processing steps in the reconstruction pipeline.
    # Processing happens every n:th tick_period. <=0 means that no processing take place
    tick_period_ms: 10
    integrate_depth_rate_hz: 40.0
    integrate_color_rate_hz: 5.0
    integrate_lidar_rate_hz: 40.0
    update_mesh_rate_hz: 5.0
    update_esdf_rate_hz: 10.0
    publish_layer_pointcloud_rate_hz: 5.0
    decay_tsdf_rate_hz: 5.0
    decay_dynamic_occupancy_rate_hz: 0.0
    clear_map_outside_radius_rate_hz: 1.0

    # printing statistics on console
    print_rates_to_console: true
    print_timings_to_console: true
    print_delays_to_console: true
    print_statistics_on_console_period_ms: 10000

    # esdf settings
    esdf_mode: 0 # 0: 3d, 1: 2d
    publish_esdf_distance_slice: true
    # color settings
    use_color: false
    # depth settings
    use_depth: false
    # lidar settings
    use_lidar: true
    lidar_width: 512
    lidar_height: 128
    lidar_vertical_fov_rad: 0.7853981633
    min_angle_below_zero_elevation_rad: -0.4328417
    max_angle_above_zero_elevation_rad: 0.4677482
    use_non_equal_vertical_fov_lidar_params: false
    # Input queues
    maximum_sensor_message_queue_length: 100
    # Map clearing settings
    map_clearing_radius_m: 30.0 # no map clearing if < 0.0
    map_clearing_frame_id: "ouster_base"
    # QoS settings
    input_qos: "SYSTEM_DEFAULT"
    # Rviz visualization
    slice_visualization_attachment_frame_id: "ouster_base"
    slice_visualization_side_length: 30.0

    layer_visualization_min_tsdf_weight: 0.1
    layer_visualization_max_tsdf_distance: 0.1
    layer_visualization_exclusion_height_m: 10.0
    layer_visualization_exclusion_radius_m: 7.0
    layer_visualization_undo_gamma_correction: false
    max_back_projection_distance: 7.0
    back_projection_subsampling: 1 # no subsampling if == 1

    static_mapper:
      check_neighborhood: true
      lidar_projective_integrator_max_integration_distance_m: 30.0
      # mapper
      maintain_mesh_block_stream_queue: true
      do_depth_preprocessing: false
      depth_preprocessing_num_dilations: 3
      # projective integrator (tsdf/color/occupancy)
      projective_integrator_max_integration_distance_m: 30.0
      projective_integrator_truncation_distance_vox: 4.0
      projective_integrator_weighting_mode: "inverse_square_tsdf_distance_penalty"
      projective_integrator_max_weight: 5.0
      # occupancy integrator
      free_region_occupancy_probability: 0.45
      occupied_region_occupancy_probability: 0.55
      unobserved_region_occupancy_probability: 0.5
      occupied_region_half_width_m: 0.1
      # esdf integrator
      esdf_integrator_min_weight: 0.1
      esdf_integrator_max_site_distance_vox: 5.0
      esdf_integrator_max_distance_m: 2.0
      esdf_slice_max_height: 5.0
      esdf_slice_min_height: -5.0
      # mesh integrator
      mesh_integrator_min_weight: 0.1
      mesh_integrator_weld_vertices: true
      # tsdf decay integrator
      tsdf_decay_factor: 0.95
      exclude_last_view_from_decay: true
      # mesh streamer
      mesh_streamer_exclusion_height_m: 2.0
      mesh_streamer_exclusion_radius_m: 7.0
      mesh_bandwidth_limit_mbps: 999999925.0 # in mega bits per second

    dynamic_mapper:
      check_neighborhood: true
      lidar_projective_integrator_max_integration_distance_m: 30.0
      # mapper
      maintain_mesh_block_stream_queue: true
      do_depth_preprocessing: false
      depth_preprocessing_num_dilations: 3
      # projective integrator (tsdf/color/occupancy)
      projective_integrator_max_integration_distance_m: 30.0
      projective_integrator_truncation_distance_vox: 4.0
      projective_integrator_weighting_mode: "inverse_square_tsdf_distance_penalty"
      projective_integrator_max_weight: 5.0
      # occupancy integrator
      free_region_occupancy_probability: 0.45
      occupied_region_occupancy_probability: 0.55
      unobserved_region_occupancy_probability: 0.5
      occupied_region_half_width_m: 0.1
      # esdf integrator
      esdf_integrator_min_weight: 0.1
      esdf_integrator_max_site_distance_vox: 5.0
      esdf_integrator_max_distance_m: 2.0
      esdf_slice_max_height: 5.0
      esdf_slice_min_height: -5.0
      # mesh integrator
      mesh_integrator_min_weight: 0.1
      mesh_integrator_weld_vertices: true
      # tsdf decay integrator
      tsdf_decay_factor: 0.95
      exclude_last_view_from_decay: true
      # mesh streamer
      mesh_streamer_exclusion_height_m: 2.0
      mesh_streamer_exclusion_radius_m: 7.0
      mesh_bandwidth_limit_mbps: 999999925.0 # in mega bits per second

My test currently is a simulation of LiDAR where I move the object to see if the ESDF changes at the previous position, but as I said not it seems like the dynamic mapper isn't working or doing anything except publishing empty messages.

Here some screnshots where I moved an object inside the simulation