This is a ROS program that implements an unconventional pose filter that listens to two unreliable pose streams (describing the same motion) and fuse them together into one, more reliable pose stream. My implementation assumes that the sensors are located inside a rolling spherical robot. However, the filter is not limited to only spherical robots, but can be applied to any scenario where two pose streams appear. Note that the filter works well even if the frequency of both pose streams is not the same. The filter will publish in the speed of the slower stream, and use interpolated information of the faster stream for pose fusion.
If you plan using this filter with your own systems, some modifications need to be made in order to adjust to your motion model accordingly. Feel free to create an issue if you need help.
This interpolated information from one stream is combined with the so called pose "measurement" on the other stream, as well as an underlying motion model. The name "Delta filter" stems from the fact that the filter consideres the transformation changes (= deltas) of both pose streams. The current filtered pose is constantly updated using the deltas. Both deltas are compared to an estimated model delta from a motion model. The underlying motion model is that of a rolling ball, and the model delta estimation uses information from both the interpolated and measured deltas. To construct the filtered pose, the interpolation-, measurement-, and model-deltas are fused together using an outlier robust weighted geometric mean, which is often used in statistics.
Tested on Ubuntu 20.04 on ROS Noetic. The program has a minimal dependency list:
- geometry_msgs
- roscpp
- tf
cd ~/catkin_ws/src
git clone https://github.com/fallow24/delta_pose_filter.git
cd ..
catkin_make
source ~/catkin_ws/devel/setup.bashroslaunch delta_pose_filter config_params.launch
There are the following parameters for you to set:
- topic_publish: Sets topic name on which to advertise the filtered pose
- topic_pose_imu: Sets topic name for subscribing to the first stream
- topic_pose_cam: Sets topic name for subscribing to the second stream
- frame_id_imu: Coordinate frame id of the first pose stream
- frame_id_cam: Coordinate frame id of the second pose stream
- imu_rate: Rate in Hz for the first pose stream
- cam_rate: Rate in Hz for the second pose stream
- sphere_radius: Radius of the spherical robot
- debug_topics: Wether to publish the debug topic
Furthermore, the filter needs information about the coordinate frame definition of both pose streams.
Thus, the "config_params.launch" file includes two static_transform_publishers which describe a change of basis according to
-
$\bf Q$ using the static_transform_publisher "axis_definition_tf_imu_cam" -
$\bf P$ using the static_transform_publisher "global_static_tf_imu_cam"
- Set the parameter "debug_topics" found in the config file to "true"
- Use rviz to vizualize "/delta/debug". Set global frame to "map".
- Use rviz to also vizualize the SLOWER pose stream
- If those two poses dont have the same axis definitions, adjust Q
- If those two poses dont move in the same principal direction, adjust P
Note that the origins DO NOT HAVE TO BE ALIGNED.
As this filter only looks at the "deltas", i.e. pose changes, it will still work.
A corresponding publication will soon be added. This work is to be presented at the European Conference on Mobile Robotics (ECMR), held in Coimbra, Portugal on 4-7 Sept.
@InProceedings{ecmr2023delta,
author = {Fabian Arzberger and Fabian Wiecha and Jasper Zevering and Julian Rothe and Dorit Borrmann and Sergio Montenegro and Andreas N{u\"}chter},
title = {Delta filter -- robust visual-inertial pose estimation in real-time: A multi-trajectory filter on a spherical mobile mapping system},
year = {2023},
month = {September},
address = {Coimbra, Portugal},
publisher = {Accepted for publication on the European Conference on Mobile Robots (ECMR) 2023}
}