Odometry drifts from the actual PoseWithCovarianceStamped

classic Classic list List threaded Threaded
2 messages Options
Reply | Threaded
Open this post in threaded view

Odometry drifts from the actual PoseWithCovarianceStamped

I am working with a python project that uses rtabmap with a RealSense D435i to capture a pointcloud(pcd) of the environment
and the position of the camera relative to the pcd and sends those to unity.
It gets the camera position by subscribing to the Odometry message from rtabmap/odom where it accesses odom.pose.pose.
However this position doesnt appear to be what I am looking for. It starts out good but over time it drifts further away from the cameras
actual position relative to the point cloud and if the tracking is lost (ros starts a new pcd) it doesnt reset to its actual position relative to the pcd once
the old pcd is found again but just continues from where it left off.
I managed to get the correct position of the camera by subscribing to the PoseWithCovarianceStamped message from rtabmap/localization_pose instead
which gives good results but is sent on a much lower intervall.
I would be interested to know why the odometry behaves this way and if there is a way to get the best of both worlds (getting to use the frequent odometry messages without drifting away from the pcd)?

How I explain this to myself is that the odomerty strictly works with the movement its calculates from the frames and just updates the position based on that movement without making use of the loop closure that rtabmap has. But this doesnt really make sense to me and isnt in line with what ive read online. Also on rviz the odometry seems fine but plotting the position values on rqt shows me that at least the position field is "off".

I am very new to this so I hope this makes sense and any help would be appreciated :)
Happy Holidays

The launchfile with the parameters can be found here if that helps
Reply | Threaded
Open this post in threaded view

Re: Odometry drifts from the actual PoseWithCovarianceStamped

The odometry pose should be continuous and not jump on localization (see https://www.ros.org/reps/rep-0105.html). If you want the pose in map frame at same frequency than odometry, your node can subscribe to odometry topic and use a TF Listener to get latest transform between "map" frame and the frame_id set in odometry's header (e.g., "odom" frame), then transform the odometry pose with the returned transform. You will then get the odometry pose in map frame (it is similar to visualize the odometry topic in RVIZ with global frame id set to map).

You can also skip transforming the pose msg by simply get TF between "map" and the child_frame_id in the odometry message at the time of the pose msg.