Elevation mapping crashes when StereoSensorProcessor is used

[ercanburak]

Hi, and thanks for your work.

I'm trying to use elevation mapping node with point cloud generated by a ZED2 camera, so I adjust the input sources like below in my yaml file:

input_sources:
    zed:
        type: pointcloud
        topic: /zed2/zed_node/point_cloud/cloud_registered
        queue_size: 1
        publish_on_update: true
        sensor_processor:
            type: stereo
            ignore_points_above:       1.4
            apply_voxelgrid_filter:    true
            voxelgrid_filter_size:     0.1
            p_1:   0.03287  # from aslam
            p_2:  -0.0001276 # from aslam
            p_3:   0.4850 # from aslam
            p_4: 399.1046 # from aslam
            p_5:   0.000006735 # from aslam
            lateral_factor: 0.001376915 # from aslam
            depth_to_disparity_factor: 47.3 # from aslam
            cutoff_min_depth:          0.3
            cutoff_max_depth:          20

When the type of sensor_processor is stereo, the elevation mapping node crashes when computeVariances method of StereoSensorProcessor is called. This is probably occuring since computeVariances call getI and getJ methods, where an unitialized vector named indices_ is trying to be accessed.

[maximilianwulf]

Thank you for the submission, is it possible to recreate the issue or that you submit a possible solution?

[ercanburak]

Yes, I can recreate the issue. Actually, it can be recreated easily, all that needs to be done is to set sensor_processor type as stereo in the parameters YAML file, and input a point cloud to the system.

I took a quick look at the history of the code, and it seems to me that this issue has been there for quite some time. In order to solve this, one needs to consider all the processing/filtering done to the original point cloud and populate the indices_ vector accordingly before computeVariances method has called.

I won't be able to put the time required to investigate this in more detail and attempt a solution for now, at least not in the upcoming few weeks. Maybe later I can have another look.

By the way, do you have some collaboration/coding style guidelines available if I were to do the fix?

[maximilianwulf]

True, I can recreate it with the turtlebot3 demo.

[maximilianwulf]

I took a quick look at the history of the code, and it seems to me that this issue has been there for quite some time. In order to solve this, one needs to consider all the processing/filtering done to the original point cloud and populate the indices_ vector accordingly before computeVariances method has called.

Agreed, indices_ is definitely never initialized. That should definitely be fixed.

By the way, do you have some collaboration/coding style guidelines available if I were to do the fix?

Yes, https://github.com/ANYbotics/styleguide

[nbertschmann]

Hi @ercanburak ,

Were you able to find a solution to this issue? I am looking to use a ZED camera to generate my point cloud.

[lucasw]

I set it to structured_light and it worked ok with a zed point cloud, but haven't tested extensively, not sure if that changes anything important about the elevation map (if any options use volumetric ray casting of the full depth pixel width and height that would be nice).

[maximilianwulf]

not sure if that changes anything important about the elevation map

The difference is in the sensor noise model. In my opinion if you don't care about absolute precision, that should be fine enough for most use cases.

[JohnerzZ]

@lucasw Did you actually get it to work with the ZED2 camera? If so, would you mind sharing your .launch and .yaml files? Because changing the type to structured light did not seem to solve the issue for me, even though my rqt_graph and tf_tree seem to be fine.

[Musab-Imadeldin]

@maximilianwulf Will there be an issue if I use an Intel Realsense camera D435i but used a different sensor noise model like ZR300. And what @lucasw mentioned about inputting the pointcloud into the YAML; Is this something that we have to add manually or can the pointcloud work without this modification?

See, I'm using the realsense_demo.launch but the model won't appear as well as the elevation mapping visualization.

Another question is, and I know this is a basic Rviz thing, but when I try to add (display type) in Rviz Left panel, there's only a Grid Map option. But when I launch the other simple demo launch files, it will have the Elevation Mapping function ready. But the camera model I have isn't launched.

I read a few of the issues about linking the pointcloud but I'm not sure if this means I need to modify the input sources/ change sensor noise model yaml file or editing the <--Rviz launch with demo configuration--> at the bottom of the realsense_demo.launch at the end with something that can make the elevation map (visualize).

Could you kindly provide me insight on this as soon as you can?

Thanks in advance.