Ask Your Question

Convert /tf to nav_msgs/Odometry

asked 2014-07-08 08:04:59 -0500

TSC gravatar image

updated 2014-07-08 09:11:18 -0500


I want to use robot_pose_ekf to receive data from a IMU and a visual odometry source. This last one only publishes a /tf between to frames and I want to get the geometry_msgs/PoseWithCovariance from it.

Is there a proper way of converting a /tf to a nav_msgs/Odometry message?

Thanks in advance!

EDIT: Thanks all for the answers! I'm going to take a comment in a single answer. I'm using a Asus Xtion Pro Live with rgbdslam_v2 package. This last one publishes /tf between a fixed_frame and a choosen frame from us at a 10hz rate. I need this to be an odom type of message to insert it in robot_pose_ekf.

I don't really know if I'm using the right package for the purpose, but I lost a serious amount of time porting it to an ARM platform (an Odroid) and I didn't want to abandoned this one just because it doesn't give me odometry out-of-the-box. Besides, changing to, for example, to ccny-rgbd-tools will probably mean a great amount of work again to get it ported to ARM.

So can anyone point me out how can I get a proper convertion from tf to Odometry. I'm not really a hardcoder so point out some parts of code in the ccny_rgbd_tools visual_odometry.cpp would be good.

Thanks again!

edit retag flag offensive close merge delete


The answer which was written by me gives the sample code from ccny_rgbd_tools's visual_odometry.cpp

sai gravatar image sai  ( 2014-07-08 09:36:30 -0500 )edit

In this case, follow the sample code. You probably want to set the covariance to something hard coded, when you put it into the EKF.

dornhege gravatar image dornhege  ( 2014-07-08 10:56:50 -0500 )edit

@dornhege can you please point me some code sample to hard code a good covariance matrix? I'm following the dirty fix in: But it doesn't seem to be good in my case (since I will have 6DOF motion). Tkx

TSC gravatar image TSC  ( 2014-07-09 06:12:30 -0500 )edit

I'm not sure how you want to do more than a simple hack if you don't have the data.

dornhege gravatar image dornhege  ( 2014-07-09 06:40:41 -0500 )edit

Maybe a normal distribution for each matrix value?

TSC gravatar image TSC  ( 2014-07-09 06:58:33 -0500 )edit

That's what this hack gives you.

dornhege gravatar image dornhege  ( 2014-07-09 07:05:35 -0500 )edit

0.2 , 0.2 , 999999 , 999999 , 999999 , 0.2 doesn't seem Gaussian but ok. Anyway, giving that I need a 6DOF motion, give a 0.2 instead of 999999 to other values seams reasonable right?

TSC gravatar image TSC  ( 2014-07-09 07:25:21 -0500 )edit

Those are variances under the assumption that a diagonal matrix is correct.

dornhege gravatar image dornhege  ( 2014-07-09 07:28:10 -0500 )edit

3 Answers

Sort by ยป oldest newest most voted

answered 2014-07-08 08:23:55 -0500

sai gravatar image

Have a look at poseTFToMsg

Sample code block from here

This code below converts a TF msg to a odometry msg, you can find example for pose msg also in the link above

             void VisualOdometry::publishOdom(const std_msgs::Header& header)
                OdomMsg odom;
                odom.header.stamp = header.stamp;
                odom.header.frame_id = fixed_frame_;
                 tf::poseTFToMsg(f2b_, odom.pose.pose);
edit flag offensive delete link more


I will check this. Probably have to adapt graph_manager.cpp in rgbdslam src folder to get this kind of message. Thanks!

TSC gravatar image TSC  ( 2014-07-08 14:05:56 -0500 )edit

I do not think there is need to look at graph_manager.cpp. In the above visual_odometry.cpp, you can find another function called 'publishTf' that actually converts the estimated transformation to a TF and also to odom msg, pose msg etc. I think completely going through the visual_odometry.cpp and its header file where `f2b_` is defined should help you a lot.

sai gravatar image sai  ( 2014-07-08 20:56:29 -0500 )edit

answered 2014-07-08 08:32:15 -0500

2ROS0 gravatar image

updated 2014-07-08 10:22:50 -0500

nav_msgs/Odometry contains translation, orientation as well as twist (linear and angular velocities)

tf on the other hand only contains translation and orientation

So unless you can find a way to derive the twist...

edit flag offensive delete link more

answered 2014-07-08 08:28:06 -0500

dornhege gravatar image

Proper way: No

Why: Odometry contains Twist and Covariances. Both are not present in /tf. You could derivate tf information to get Twists and maybe somehow give covariances, but I wouldn't consider that proper. Ideally ones goes the other way and produces /tf from Odometry.

Depending on what you want to do with the data, Just putting pose information into the odometry might be sufficient for the application that needs an Odometry message.

edit flag offensive delete link more

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: 2014-07-08 08:04:59 -0500

Seen: 4,909 times

Last updated: Jul 08 '14