ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A
Ask Your Question

EKF with ZED

asked 2022-08-08 07:10:41 -0600

amit_z gravatar image

I want to fuse the ZED pose with an external GPS. For a start, I am just trying to pass the ZED signal through an EKF (robot_localization) node (I expect to see the same behavior as without the EKF). Therefore, I am disabling "publish_map_tf" (map -> odom), and run an EKF node that subscribes to the pose topic. Although I expect to see no change in behavior, eventually my base_link drifts, as if it has velocity even when I stay in the same place.

my ZED2 parameters are:

# params/common.yaml
# Common parameters to Stereolabs ZED and ZED mini cameras

# Dynamic parameters cannot have a namespace
brightness:                 4                                   # Dynamic
contrast:                   4                                   # Dynamic
hue:                        0                                   # Dynamic
saturation:                 4                                   # Dynamic
sharpness:                  4                                   # Dynamic
gamma:                      8                                   # Dynamic - Requires SDK >=v3.1
auto_exposure_gain:         true                                # Dynamic
gain:                       100                                 # Dynamic - works only if `auto_exposure_gain` is false
exposure:                   100                                 # Dynamic - works only if `auto_exposure_gain` is false
auto_whitebalance:          true                                # Dynamic
whitebalance_temperature:   42                                  # Dynamic - works only if `auto_whitebalance` is false
depth_confidence:           50                                  # Dynamic
depth_texture_conf:         100                                 # Dynamic
pub_frame_rate:             15.0                                # Dynamic - frequency of publishing of video and depth data
point_cloud_freq:           30.0 #15                               # Dynamic - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value)

    camera_name:                zed                             # A name for the camera (can be different from camera model and node name and can be overwritten by the launch file)
    zed_id:                     0
    serial_number:              0
    resolution:                 3                               # '0': HD2K, '1': HD1080, '2': HD720, '3': VGA
    grab_frame_rate:            30                              # Frequency of frame grabbing for internal SDK operations
    gpu_id:                     -1
    base_frame:                 'base_link'                     # must be equal to the frame_id used in the URDF file
    verbose:                    false                           # Enable info message by the ZED SDK
    svo_compression:            2                               # `0`: LOSSLESS, `1`: AVCHD, `2`: HEVC
    self_calib:                 true                            # enable/disable self calibration at starting
    camera_flip:                false

    img_downsample_factor:      1.0                             # Resample factor for images [0.01,1.0] The SDK works with native image sizes, but publishes rescaled image.
    extrinsic_in_camera_frame:  true                            # if `false` extrinsic parameter in `camera_info` will use ROS native frame (X FORWARD, Z UP) instead of the camera frame (Z FORWARD, Y DOWN) [`true` use old behavior as for version < v3.1]

    quality:                    1                               # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA
    sensing_mode:               0                               # '0': STANDARD, '1': FILL (not use FILL for robotic applications)
    depth_stabilization:        0                               # `0`: disabled, `1`: enabled
    openni_depth_mode:          false                           # 'false': 32bit float meters, 'true': 16bit uchar millimeters
    depth_downsample_factor:    1.0                             # Resample factor for depth data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices (depth map, point cloud, ...)

    pos_tracking_enabled:       true                            # True to enable positional tracking from start
    publish_tf:                 true                            # publish `odom -> base_link` TF
    publish_map_tf:             false                            # publish `map -> odom` TF
    map_frame:                  'map'
    odometry_frame:             'odom'
    area_memory_db_path:        ''
    area_memory:                true                            # Enable to detect loop closure
    floor_alignment:            false                           # Enable to automatically calculate camera/floor offset
    initial_base_pose:          [0.0,0.0,0.0, 0.0,0.0,0.0]      # Initial position of the `base_frame` -> [X, Y, Z, R, P, Y]
    init_odom_with_first_valid_pose: true                       # Enable to initialize the odometry with the first valid pose
    path_pub_rate:              2.0                             # Camera trajectory ...
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted

answered 2022-08-09 02:58:21 -0600

Per Edwardsson gravatar image

A few clarifications first off: The EKF will produce the motion of the robot according to the robot, which is expressed in the odom->base_link transform. This will typically drift, even in well balanced systems. To combat this drift, an additional transform is introduced, map->odom, which compensates this drift* through techniques like mapping and loop closing. The ZED is capable of calculating both map->odom and odom->base_link transforms, although I would be careful about having one sensor do both. Currently, your ZED publishes odom->base_link (publish_tf: true). Turn that off to isolate any error to the EKF alone.

Now, if you still have drift, turn off all inputs to the EKF and restart. Make sure you have no drift here. Turn on one variable at a time (for instance, listen to just x position) and make sure that works and is stable - in this state, the position of the robot in the odom . Then, work up to the full suite of variables you intend to listen on. I would recommend to keep the number of inputs to the EKF down to a minimum.

  • The robot always starts in the odom origin, so if the robot has a map localization, the map->odom can also place the robot in a map.
edit flag offensive delete link more


Thank you for your response. As explained in the robot_localization doc, usually it is needed to run 2 EKF nodes, one that publishes map -> odom and one for odom->base_link. Since ZED already fuses IMU and visual odometry to produce odom->base_link, and I only want the GPS fusion, I have used only one EKF node (map -> odom) and did not disable publish_tf. Don't you think it is a good idea?

amit_z gravatar image amit_z  ( 2022-08-09 07:22:38 -0600 )edit

The main problem, I believe, is that /odometry/gps, navsat_transfrom, odometry msg, is drifting in all directions, x y and z. This is happening even though the GPS signal seems OK (from ublox/fix) This is the /odometry/gps msg:

  seq: 326
    secs: 1660049500
    nsecs: 499717350
  frame_id: "map"
child_frame_id: ''
      x: -4.9935318673960865
      y: -8.162464517634362
      z: 9.2710430592455
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
amit_z gravatar image amit_z  ( 2022-08-09 07:56:39 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower


Asked: 2022-08-08 07:10:41 -0600

Seen: 83 times

Last updated: Aug 09 '22