stereolabs / zed-ros-wrapper

ROS wrapper for the ZED SDK
https://www.stereolabs.com/docs/ros/
MIT License
447 stars 391 forks source link

Trouble Fusing ZED2 odometry in Kalman Filter #549

Closed JordySehn closed 4 years ago

JordySehn commented 4 years ago

No issue, just a question if I could call on your expertise.

I am trying to fuse the visual odometry as well as the imu data produced from the ZED2 with my robots other sensors in a Kalman filter with robot_localization. I noticed the odom messages still do not contain twist information yet as noted in issue #361.

I could proceed to try to go through the process described in issue #361, but I thought considering the zed2 imu I could use that for the velocity measurement instead.

As such I was trying to fuse just the x, y, yaw pose information from the odometry message with the x, y, and yaw velocity from the imu/data_raw and while it does sort of work, I can not get nearly as good performance from the odom to base_link transform produced by the sensor fusion package as I can by just enabling the zed_wrapper odom to baselink transform. I am fairly confident in my robot_localization configurations assuming the covariance matrices produced by the imu and odometry are accurate.

Is this the right approach I should be taking? Any advice? I would really like to be able to fuse in my other sensors as well rather than just relying solely on the ZED2.

Myzhar commented 4 years ago

Hi @JordySehn, if you are performing a pure 2D localization you can consider to enable the "2D mode" setting the "two_d_mode" to true. https://github.com/stereolabs/zed-ros-wrapper/blob/master/zed_wrapper/params/common.yaml#L58

We achieved good results in localization using the robot_localization package, but it all depends on which data you decide to fuse and how...