Cannot generate elevation map with ZED2 stereo camera

[JohnerzZ]

Hi and thanks a lot for your work.

I'm trying to use the elevation mapping node with a ZED2 camera. I have previously successfully used it with a D435i camera so I followed the same procedure.

My zed2.launch file is:

<launch>

    <arg name="svo_file"             default="" /> <!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->
    <arg name="stream"               default="" /> <!-- <arg name="stream" default="<ip_address>:<port>"> -->

    <arg name="zed_node_name"        default="zed_node" />
    <arg name="camera_model"         default="zed2" />
    <arg name="publish_urdf"         default="true" />

    <arg name="camera_name"          default="zed" />

    <arg name="base_frame"           default="base_link" />


  <!-- ZED Wrapper Node-->
  <include file="$(find zed_wrapper)/launch/include/zed_camera.launch.xml">
      <arg name="camera_name"         value="$(arg camera_name)" />
      <arg name="svo_file"            value="$(arg svo_file)" />
      <arg name="stream"              value="$(arg stream)" />
      <arg name="node_name"           value="$(arg zed_node_name)" />
      <arg name="camera_model"        value="$(arg camera_model)" />
      <arg name="base_frame"          value="$(arg base_frame)" />
      <arg name="publish_urdf"        value="$(arg publish_urdf)" />
  </include>

    <!-- Publish tf 'base_footprint' as pose.-->
  <node pkg="elevation_mapping_demos" type="tf_to_pose_publisher.py" name="tf_to_pose_publisher">
    <param name="from_frame" type="string" value="odom" />
    <param name="to_frame"   type="string" value="zed2_link" />
  </node>

  
  <!-- Elevation mapping node -->
  <node pkg="elevation_mapping" type="elevation_mapping" name="elevation_mapping" output="screen">
    <rosparam command="load" file="$(find elevation_mapping_demos)/config/robots/zed2.yaml" />
    <rosparam command="load" file="$(find elevation_mapping_demos)/config/elevation_maps/simple_demo_map.yaml" />
    <rosparam command="load" file="$(find elevation_mapping)/config/sensor_processors/aslam.yaml" />
    <rosparam command="load" file="$(find elevation_mapping_demos)/config/postprocessing/postprocessor_pipeline.yaml" />
  </node>
  

  <!-- Launch visualizations for the resulting elevation map -->
  <include file="$(find elevation_mapping_demos)/launch/visualization.launch" />

  <!-- Launch RViz with the demo configuration -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find elevation_mapping_demos)/rviz/elevation_map_visualization_pointcloud.rviz" />

</launch>

and my zed2.yaml is:

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

map_frame_id: "odom"
robot_base_frame_id: "base_link"
robot_pose_with_covariance_topic: "/zed_node/pose_with_covariance"
robot_pose_cache_size: 200
track_point_frame_id: "base_link"
track_point_x: 0.0
track_point_y: 0.0
track_point_z: 0.0

Both the zed2 node and the elevation mapping node intialize just fine. However, the grid map is not shown in rviz, as i demonstrate in the following image:

However, the pointcloud and pose with covariance from ZED2 are being published correctly as seen here:

For debugging purposes, I am checking the tf_frames and the rqt_graph but i cannot find anything wrong with this setup.

I have seen older issues and it is recommended to change the sensor type from stereo to structured_light but this does not seem to work either.

[nimiCurtis]

Hi @JohnerzZ did you succeed to handle with that issue?
I've got the same problem with running the pkg on zedm stereo camera.

[JohnerzZ]

Hi @nimiCurtis yes I did solve that issue. My ZED2 now successfuly works with the elevation_mapping package. I had to change my sensor_processor to realsense_d435.yaml in my .launch file and my sensor_processor type to structured_light in my zed2.yaml. The sensor_processor parameters had to change accordingly as well. I also had some errors when naming my topics.

TLDR, the working .launch file and .yaml file:

zed2.launch:

<launch>

    <arg name="svo_file"             default="" /> <!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->
    <arg name="stream"               default="" /> <!-- <arg name="stream" default="<ip_address>:<port>"> -->

    <arg name="zed_node_name"        default="zed_node" />
    <arg name="camera_model"         default="zed2" />
    <arg name="publish_urdf"         default="true" />

    <arg name="camera_name"          default="zed" />

    <arg name="base_frame"           default="base_link" />


  <!-- ZED Wrapper Node-->
  <include file="$(find zed_wrapper)/launch/include/zed_camera.launch.xml">
      <arg name="camera_name"         value="$(arg camera_name)" />
      <arg name="svo_file"            value="$(arg svo_file)" />
      <arg name="stream"              value="$(arg stream)" />
      <arg name="node_name"           value="$(arg zed_node_name)" />
      <arg name="camera_model"        value="$(arg camera_model)" />
      <arg name="base_frame"          value="$(arg base_frame)" />
      <arg name="publish_urdf"        value="$(arg publish_urdf)" />
  </include>

    <!-- Publish tf 'base_footprint' as pose.-->
  <node pkg="elevation_mapping_demos" type="tf_to_pose_publisher.py" name="tf_to_pose_publisher">
    <param name="from_frame" type="string" value="odom" />
    <param name="to_frame"   type="string" value="base_link" />
  </node>

  
  <!-- Elevation mapping node -->
  <node pkg="elevation_mapping" type="elevation_mapping" name="elevation_mapping" output="screen">
    <rosparam command="load" file="$(find elevation_mapping_demos)/config/robots/zed2.yaml" />
    <rosparam command="load" file="$(find elevation_mapping_demos)/config/elevation_maps/long_range.yaml" />
    <rosparam command="load" file="$(find elevation_mapping)/config/sensor_processors/realsense_d435.yaml" />
    <rosparam command="load" file="$(find elevation_mapping_demos)/config/postprocessing/postprocessor_pipeline.yaml" />
  </node>
  

  <!-- Launch visualizations for the resulting elevation map -->
  <include file="$(find elevation_mapping_demos)/launch/visualization.launch" />

  <!-- Launch RViz with the demo configuration -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find elevation_mapping_demos)/rviz/elevation_map_visualization_pointcloud.rviz" />

</launch>

zed2.yaml:

input_sources:
    zed:
        type: pointcloud
        topic: "/zed_node/point_cloud/cloud_registered"
        queue_size: 1
        publish_on_update: true
        sensor_processor:
            ignore_points_above: .inf
            ignore_points_below: -.inf
            cutoff_min_depth: 0.2
            cutoff_max_depth: 3.25
            type: structured_light
            normal_factor_a: 0.000611
            normal_factor_b: 0.003587
            normal_factor_c: 0.3515
            normal_factor_d: 0
            normal_factor_e: 1
            lateral_factor: 0.01576

map_frame_id: "odom"
robot_base_frame_id: "base_link"
robot_pose_with_covariance_topic: "/base_link_pose"
robot_pose_cache_size: 200
track_point_frame_id: "base_link"
track_point_x: 0.0
track_point_y: 0.0
track_point_z: 0.0

[nimiCurtis]

Thanks a lot @JohnerzZ! it is now working for me as well.

[weixiaojian-nc]

Hi @JohnerzZ ，it's greet to meet you! I'm trying to use the elevation mapping node with a D435i camera.But always fail.So, could you share your files to help me with the task?Thanks so much!