When scanning column-shaped obstacles, some voxels are not getting marked as occupied.

[astumpf]

Describe the bug Voxels are not getting marked as occupied (never saturate log odds properly?).

To Reproduce In our setup, we us a velodyne capped to 5m range and min cell size of 0.1. Just get a scan of any small column-shaped object. In our case the robot was steady, but wavemap did never set those voxels to occupied. This issue does not occur with wide obstacles such as walls.

Expected behavior All endpoints hitting the pillar should be represented in wavemap as occupied voxels.

Observed outcome See image below

Screenshots

System information (please complete if relevant):

• not relevant

Runtime information (please complete if relevant):

• Custom setup:
  • depth sensor: VLP-16
  • steady robot

Additional context Add any other context about the problem here.

[victorreijgwart]

Hey! My first guess would be that the projection model is not tuned well for a VLP16. Could you share the the config file you used for wavemap?

[astumpf]

I copied the Ouster model and adapted it to the Velodyne specifications. I had some problems with the generic spherical model, but I can't remember what the problem was.

map:
  general:
    world_frame: "odom"
    body_frame: "base_link"
    thresholding_period: { seconds: 2.0 }
    pruning_period: { seconds: 2.0 }
    publication_period: { seconds: 1.0 }
  data_structure:
    type: "hashed_chunked_wavelet_octree" # "wavelet_octree"
    min_cell_width: { meters: 0.1 } #max_height: 14
    remove_blocks_beyond_distance: { meters: 5.0 }  # New: Distance beyond which to drop blocks

inputs:
  - type: "pointcloud"
    general:
      topic_name: "/velodyne_points_self_filtered"
      topic_type: "PointCloud2"
      undistort_motion: false
      topic_queue_length: 10
      max_wait_for_pose: { seconds: 1.0 }
      reprojected_pointcloud_topic_name: "/wavemap/reprojected_pointcloud"
      # projected_range_image_topic_name: "/wavemap/projected_range_image"
    integrators:
      - integration_method:
          type: "hashed_chunked_wavelet_integrator" # "wavelet_integrator"
          min_range: { meters: 0.5 }
          max_range: { meters: 5.0 } # { meters: 15.0 }
          termination_update_error: 0.1
        projection_model:
          type: "ouster_projector" #spherical_projector
          lidar_origin_to_beam_origin: { millimeters: 0.0 }
          lidar_origin_to_sensor_origin_z_offset: { millimeters: 0.0 } # 37.7 from bottom plate
          elevation:
            num_cells: 16
            min_angle: { degrees: -15.0 }
            max_angle: { degrees:  15.0 }
          azimuth:
            num_cells: 1800
            min_angle: { degrees: -180.0 }
            max_angle: { degrees:  180.0 }
        measurement_model:
          type: "continuous_beam" # "continuous_ray"
          angle_sigma: { degrees: 0.035 }
          # NOTE: For best results, we recommend setting range_sigma to
          #       max(min_cell_width / 2, ouster_noise) where ouster_noise = 0.05
          range_sigma: { meters: 0.05 }
          scaling_free: 0.2
          scaling_occupied: 0.4

[victorreijgwart]

Are the points in the Rviz screenshot you shared from the "/wavemap/reprojected_pointcloud" topic? Could you also uncomment "/wavemap/projected_range_image" in the config and show a screenshot of what it looks like?

Regarding the projection model, the spherical_projector would be the right fit for the VLP16. Other than that, the settings are very close to what we used the last time we worked with a VLP16. We used azimuth: num_cells: 1740 back then, which seemed to give slightly nicer range images - but there's no right or wrong number here since Velodynes don't trigger digitally and the number of columns varies from scan to scan.

Is it easy for you to share a representative rosbag? If so, I could also take a look.

[astumpf]

Thanks for taking a look into that! I'll update you with the requested data asap. In meantime, I noticed that Wavemap crashes on startup when activating undistort_motion using this config.

[victorreijgwart]

Yes motion undistortion is not yet implemented for Velodynes, as I didn't have a good rosbag to test it with. If you could share a rosbag with Velodyne pointclouds, some aggressive motion and reasonably good odometry - I'd be happy to add it. When you say it crashes, what does it do exactly? I hope it prints an error message and exits.

[victorreijgwart]

Just turning at a reasonable rate would be enough motion to test the undistortion btw. It doesn't need to be super aggressive or include fast translation.

[astumpf]

It just crashes ungracefully during launch. No errors printed.

I have to admit that I was wrong about the problem, it is not object specific. We are missing a complete layer (the red thing is the vlp-16 laser, scan is the reprojected cloud):

I will check what I can provide as a bag file. Aggressive movements are rather difficult with slow ground-based robots ;-).

[victorreijgwart]

Great. A bag file would be useful, both to add undistortion and look into why this horizontal slice goes missing.

[astumpf]

The other way around: Any idea why it is not clearing?

[victorreijgwart]

It's a bit hard to see what the problem is from the screenshots. Is the object in the midright of the first image dynamic and not getting erased?

Have you tried setting a higher value for angle_sigma, e.g. angle_sigma: { degrees: 0.05 } or even angle_sigma: { degrees: 0.08 }? Since the 16-beam Velodynes/Robosenses are very sparse compared to the 128-beam Ouster, it might help to make the beams a bit softer so they cover more volume.

Did you get to retry the spherical projection model? You mentioned it didn't work when you first tried it, but you were no longer sure what the problem was. If there was a specific issue it'd help to know.

Could you also uncomment "/wavemap/projected_range_image" in the config and show a screenshot of what these images looks like? Wavemap internally uses range images to check whether octants are free, fully unknown, or possibly occupied in the multi-resolution update algorithm. Since the reprojected pointclouds are ok, the images are probably ok as well. But it'd be good to be sure. The "/wavemap/projected_range_image" topic is a standard image topic that you can visualize in Rviz.

[astumpf]

It's a bit hard to see what the problem is from the screenshots. Is the object in the midright of the first image dynamic and not getting erased?

Yes, I highligted it here:

Have you tried setting a higher value for angle_sigma, e.g. angle_sigma: { degrees: 0.05 } or even angle_sigma: { degrees: 0.08 }? Since the 16-beam Velodynes/Robosenses are very sparse compared to the 128-beam Ouster, it might help to make the beams a bit softer so they cover more volume.

See image below (changed to 0.08)

Did you get to retry the spherical projection model? You mentioned it didn't work when you first tried it, but you were no longer sure what the problem was. If there was a specific issue it'd help to know.

Yes, I've already switched over to use only spherical projection model.

Could you also uncomment "/wavemap/projected_range_image" in the config and show a screenshot of what these images looks like? Wavemap internally uses range images to check whether octants are free, fully unknown, or possibly occupied in the multi-resolution update algorithm. Since the reprojected pointclouds are ok, the images are probably ok as well. But it'd be good to be sure. The "/wavemap/projected_range_image" topic is a standard image topic that you can visualize in Rviz.

Tada:

Changing sigma makes only minor difference. The projected range image is hardly to see, as we got only 16 lines.

[astumpf]

Doing double checking: For legacy reasons, we convert the wavemap data into an octomap, which leads to the same results. We can therefore rule out the Wavemap RViz plugin as a possible problem.

[victorreijgwart]

Thanks for the clarifications and for sharing the additional screenshots. It's unfortunate that making the beams softer doesn't help. We mostly used/tested wavemap with LiDARs having 32 or more beams. Maybe we have an assumption somewhere in the code that breaks when the number of beams is very low. I'll debug it interactively with the bag file you shared and get back to you.

[astumpf]

I will send you also the bagfile from which these images are taken.

[astumpf]

Thanks for the clarifications and for sharing the additional screenshots. It's unfortunate that making the beams softer doesn't help. We mostly used/tested wavemap with LiDARs having 32 or more beams. Maybe we have an assumption somewhere in the code that breaks when the number of beams is very low. I'll debug it interactively with the bag file you shared and get back to you.

Thanks a lot! I appreciate your great support!

[victorreijgwart]

Hi Alexander,

Thanks again for sharing the data. I got to dig into it this morning and found the problem. It turned out to be a combination of how your sensor is mounted (perfectly horizontal) and the fact that your odometry is in 2D (pitch=roll=0, constant z). In this case, voxels that are near the same height as the sensor (z ~= 0.559m in your dataset) always project to the same scan line in sensor coordinates (elevation_angle ~= 0°). The VLP16's scan lines closest to 0° elevation are at -1° and +1°. Intuitively, this means that cells at the same height as the sensor are never hit dead-center. When using the continuous_beam measurement model, the measurement update is zero for cells whose center is more than 6 * angle_sigma away from the beam's center. So for an angle_sigma < 1°/6, the 0-plane cells never get updated.

You can solve the issue by either

• Keeping the continuous_beam measurement model but setting angle_sigma: { degrees: 0.17 } or higher.
• Switching to the continuous_ray measurement model, which does not bother about angular uncertainty.
• Mounting your VLP16 s.t. it's not perfectly horizontal, in which case the 0-plane changes when the robot yaws.
• Using 3D odometry, in which case pitch/roll changes (incl. small wobbles) move the 0-plane around.

Assuming the first two options are the most practical for you, you could run some quick comparisons for both and see which maps look the best for your use case. I'd usually recommend the beam model for LiDARs since it better captures thin obstacles when used with a small angle_sigma, especially when the max_range is high. But this doesn't really apply to your use case, so the ray model might be a better fit - with the added benefit of being computationally cheaper.

Let me know if this solves your issue.

[victorreijgwart]

PS, I'll add motion undistortion support for Velodynes/Robosenses sometime in the next week or two.

[astumpf]

Thank you very much for your help! Your suggestions have indeed helped me a lot as they improve the quality of the map for our use case. Although the map cleanup is not a problem anymore, I am still not convinced that this is the solution to the original problem. If you look at the picture in the first post, you can clearly see 3 lidar ray rows hitting the "void" as Wavemap does not recognize any occupancy there.

[yuliangzhong]

Hi @astumpf, my suggestion is to look up the logodds of voxels in the missing area. You can write an interactive marker in Rviz and implement logodd queries (reference) in marker event callback functions. By doing so, we can monitor how logodds vary when robot is approaching and leaving. Probably it would give you more information :)

[victorreijgwart]

When replaying your dataset, I could reproduce the issue you showed in the first image. But it disappears when using the suggestions I shared with you. Are there concrete cases where the issue still appears?

[victorreijgwart]

Rephrasing my earlier message, the updates are sampled exactly at the voxel center points (not over the cubes drawn in Rviz). When using the beam_model, the update will be zero unless the voxel's center is within 6*angle_sigma of the beam's centerline. Assuming the obstacle in the first image is roughly 3m away, the beam at that distance only has a width of ~1cm for angle_sigma: { degrees: 0.035 }, which is much less than the 10cm voxel size.

Modeling thin beams allows wavemap to reconstruct thin objects (e.g. railings, branches, leaves) better. This is because it reduces grazing (beams eroding objects when they pass within one voxel size of their surface without hitting them), which is a problem with lasers since they're very thin. But this tuning only works when the LiDAR covers the observed volume well. For example, for an Ouster OS0 with 128 beams moving in full 3D, we got the best results with angle_sigma: { degrees: 0.035 }. For the Livox Mid 360, which has a lower beam count but uses a non-repeating pattern, we get good results with angle sigmas around 0.08 degrees.

For setups with only 16 beams that don't pitch/roll/z-translate, the only way to get good coverage is with very wide beams. The other solution is to use the ray_model, which uses the closest ray to every voxel without caring about the angular distance.

[victorreijgwart]

@astumpf, I'll close this issue, as it seems solved, and extend the documentation to include what we discussed. Don't hesitate to re-open the issue if you have follow-up questions.

[astumpf]

Sure, thanks for your help so far. I will get back to you if this problem still occurs.