Evaluating rosify_difodo: Using /tf topic
As previously stated, In order to generate a evaluation that can be compare with TUM dataset I have to use the same coordinate system that they are using. This means that I have to include somehow the transformation between the world and the kinect camera from TUM dataset into rosify difodo. For this Ill allow difodo to suscribe to the /tf topic so it can “listen” to the transformation between the world and the camera.
Making rosify difodo listen to /tf topic
How to create tf and how do they work
Before making difodo listen to /tf lets explain why is this neccesary and how they work. See here to see how to publish static transformation and more uiseful commands with tf. NOTE: static_transform_publisher publish “x y z yaw pitch roll” (roll and yaw are in the inverse order as one would expect) or “x y z qx qy qz qw”. An example on how to publish:
rosrun tf static_transform_publisher 1.0 0 0 0 0 0 /world /odometry 30
With this you can publish a /tf of type static. This means that this transform wont change over time, so using a high frequency will be a waste since the message can be heard once and never use again. Transforms uses (Tx.Ty.Tz) and (roll, pitch, yaw). In the following example the odometry system will have its coordinate reference system 1 meter in Tx from the world so the odometry that we get from, for example, DIFODO in this case will have to add 1m to be referenced against the world
Also this can be specify in a launch file as:
<launch>
<node pkg="tf" type="static_transform_publisher" name="tf_odometry_world" args="1 0 0 0 0 0 /world /odometry 50"/>
</launch>
Understading more of transformations in ROS
First, one should read REP 105 to get to know the common naming convections of ROS frames. A frame is a coordinate system in ROS. The most common frames used are:
- base_link: This frame is a mobile frame. This means its position against the father frame can change over time due to the movement of the robot. It usually represents the center of gravity of the robot, or center of the robot but it could be any place in our robot. Is useful to be the father frame of the different parts or sensors of the robot (an arm, a camera…). base_link father frame is usually “odom”
- odom: Odom is a fixed frame. The father of odom is the “map” frame. As I see it (since this is not entirely true for all cases) this frame represents the starting position of the robot at the beggining of the scene . For example we could have the map located 1meter away from the robot and then start moving. “Odom” frame position would be 1 meter away from the “map”. “base_link” is referenced against “odom” so as the robot moves, the tf between odom and base_link changes.
- map: Fixed frame, usually consider the absolute world frame. All robots can be located from this general frame. “odom” would tell “map” where the robot started moving and then by using the tf between “base_link” and “odom”, the absolute position on the robot could be found.
- earth: The earth represents an absolute point in the world, is a fixed frame and is only used when we want to join several fixed frames, like several “map” frames. Only used on complex architectures.
I specially like this answer that I think explains this really well: https://answers.ros.org/question/237295/confused-about-coordinate-frames-can-someone-please-explain/?answer=237330#post-id-237330
Making difodo listen to /tf
Right now rosify difodo is not using or listening to any /tf but in order to use TUM dataset and locate against their “map” which they call “world” we will have to make some tweaks. Another option is to just publish a static_tf between the “map” or “world” and “odom” and let rosify difodo use it. Since we are going to be using TUM dataset to evaluate this algorithm and the new algorithms develop I think is worth to create a special case to be ready to use with TUM, and avoid having to set up manually the static transformation between the world and the odom for each scene in the dataset.
Currently what rosify difodo provides in the /difodo/odometry topic is the position or pose of the base link referenced to the odom frame. This information is also publish in the /tf topic as a transform available between those frames.
What we are missing right now it the transform from the absolute world (“map” or “world” from TUM dataset) to the “odom” frame. On the next version of the software this will be release.
What has being made in the branch “development” os rosify_difodo is the possibility to listen and get the /tf desired (input world_frame_name AND odom_frame_name, and also a control variable called log_pose_relative_to_world that will use or not the specified /tf). Then is log_pose_relative_to_world is true this /tf will be applied to the odometry that we get on each iteration.
Creating a option that uses the transformation from world to kinect for TUM dataset
Since we can specify the world_frame_name and odom_frame_name all that is needed to append this transformation to the odometry that we get from difodo is set those. For TUM dataset: -world_frame_name = “world” -odom_frame_name = “openni_camera”
This transformation allows us to know the position of the lens of the kinect camera regarding to the world. If we use this transform all the time we would be getting the TRUE position of the camera all the time instead of using our odometry. So in order to use our odometry we have to get the last known position (or transform) before we start to get depth frames and we are able to compute odometry. Once we get the first odometry available we will freeze (stop reading) the /tf values and use only the last known position to transform the odometry in to the world.
An example of the results when doing this with rosify_difodo against the groundtruth can be seen next for the freiburg1_desk sequence:
What to do next
- Since I want to test SD-SLAM against rosify_difodo I would also have to do SD-SLAM apply these /tf to the odometry and also generate an output file with the format required. Since this would be exactly the same Ive made with rosify_difodo I think that it would make more sense to extract this logic out of rosify_difodo and create a new ros node or package that do this. This package will recieve the two frames desired to get the /tf that will be later be applied to the odometry (that also will have to be inserted as an input) and will create the evaluation file that can be used by TUM and slam-testbed.