Evaluating rosify_difodo: Creating a separate node for evaluation
As I concluded in the previous article, having to apply the /tf from odom to the world and create an evaluation file from rosify_difodo didnt seem right. Also since this logic would also be used by SD-SLAM, I decided to create another ros node that will be launch at the same time so this node generates the evaluation file.
The name for this node or ros package is odometry_evaluation_file_creator.
odometry_evaluation_file_creator
# For rosify_difodo
roslaunch odometry_evaluation_file_creator TUM_dataset_rosify_difodo.launch.launch
# For SD-SLAM
roslaunch odometry_evaluation_file_creator TUM_dataset_sdslam.launch.launch
What it does
This ros package allows to apply a publish /tf to the pose of an odometry being publish. This final transform pose along with the odometry timestamp is publish on an output text file that follows the TUM dataset file format. So it can be compared with TUM sequences.
How it works
The node subscribes to a /tf between two frames and to an odometry topic. When the first odometry arrives, the /tf is retrieved and that /tf is the one that is used for the rest on the execution. The /tf in the TUM dataset between the world frame and the openni_camera express the relation between the world and the camera center. The odometry publish is referenced to the center of the camera, in this case the odom frame. odometry frame and openni_camera frame are the same then.
Configuration parameters
To configure the package we have the following parameters that are within an YAML file:
- odometry_topic: The odometry topic we want to subscribe to. Eg: /sdslam/odometry
- source_frame: The source frame of the tf. The frame from which we want to transform, camera center in this case. Eg: openni_camera
- target_frame: The target frame that we want to transform to. The absolute reference system, the world. Eg world
- output_file_dir: A path to a directory where the output files will be stored.
How to install
To install this node you can follow basically the same instructions required for rosify_difofo. The only differences regarding requisites with rosify_difodo is that this package doesnt require MRPT.
To install it can be done as a ROS package or with CMake. I recommend to do it as a ROS package to use the launch files provided.
The instructions to install are the same as with rosify_difodo: https://roboticslaburjc.github.io/2019-tfm-omar-garrido/install/#install-as-a-ros-package
git clone https://github.com/RoboticsLabURJC/2019-tfm-omar-garrido.git
cp -r odometry_evaluation_file_creator/ ~/Programs/catkin_ws/src/
cd ~/Programs/catkin_ws
## Two options:
# 1. Build all catkin packages
catkin_make
# OR
# 2. Build only the odometry_evaluation_file_creator package
catkin_make --only-pkg-with-deps odometry_evaluation_file_creator
catkin_make -DCATKIN_WHITELIST_PACKAGES=""
How to use
Use the launch files provided within the package. I provided two launch files:
TUM_dataset_rosify_difodo.launch This launch files is intended to be used with rosify_difodo. It loads the configuration YAML from odometry_evaluation_file_creator/launch/config/rosify_difodo_evaluator_config.yaml
TUM_dataset_sdslam.launch This launch files is intended to be used with SD-SLAM. It loads the configuration YAML from odometry_evaluation_file_creator/launch/config/sdslam_evaluator_config.yaml
Call either of them with:
roslaunch odometry_evaluation_file_creator TUM_dataset_rosify_difodo.launch
# OR
roslaunch odometry_evaluation_file_creator TUM_dataset_sdslam.launch
NOTE: In order to make the next step of testing rosify_difodo and SD-SLAM with TUM dataset, new launch files will be created that call the odometry_evaluation_file_creator and the desired package [rosify_difodo, SD-SLAM] all at once within the odometry packages themselves.
TUM evaluation tools
Only the evaluation online works the python scripts seem to use all points from the groundtruth which are more than the ones of dpeth images, getting a higher error that it should.
The RPE only works well if we change the option “relative pose error for pose pairs with a distance of” to 0.1 seconds or less (otherwise it will compare all positions in a 1 seconds intervale which doesnt make sense)
EVO evaluation tool
Evo is a new evaluation tool that is quicly becaming famous around the navigation robotics world. It allows multiple formats from different well known datasets for odometry/slam like TUM dataset.
It can be downloaded from github. Is a python package that can be easily install with pip. check their repo for detailed instructions.
Now we will test evo and see the results and features that is has to offer with a file created with odometry_evaluation_file_creator and rosifu_difodo:
Example of use: APE (Absolute Pose Error)
evo_ape tum ../tum_evaluation_scripts/data/tum_groundtruth/rgbd_dataset_freiburg1_desk-groundtruth_NO_HEADERS.txt ../tum_evaluation_scripts/data/rosify_difodo_output/desk/TIMESTAMPS_DEPTH_IMAGES_27_01_2020-00_40_23_rosify_difodo_odom_output_MOD.txt -va --plot --plot_mode xz --save_results results/difodo.zip
Results
Loaded 1976 stamps and poses from: ../tum_evaluation_scripts/data/tum_groundtruth/rgbd_dataset_freiburg1_desk-groundtruth_NO_HEADERS.txt
Loaded 574 stamps and poses from: ../tum_evaluation_scripts/data/rosify_difodo_output/desk/TIMESTAMPS_DEPTH_IMAGES_27_01_2020-00_40_23_rosify_difodo_odom_output_MOD.txt
Synchronizing trajectories...
Found 573 of max. 1976 possible matching timestamps between...
../tum_evaluation_scripts/data/tum_groundtruth/rgbd_dataset_freiburg1_desk-groundtruth_NO_HEADERS.txt
and: ../tum_evaluation_scripts/data/rosify_difodo_output/desk/TIMESTAMPS_DEPTH_IMAGES_27_01_2020-00_40_23_rosify_difodo_odom_output_MOD.txt
..with max. time diff.: 0.01 (s) and time offset: 0.0 (s).
--------------------------------------------------------------------------------
Aligning using Umeyama's method...
Rotation of alignment:
[[ 0.99529182 -0.07491277 0.06150012]
[ 0.0756443 0.99708816 -0.00965063]
[-0.06059809 0.01425733 0.99806042]]
Translation of alignment:
[-0.02917636 -0.11717359 0.02894151]
Scale correction: 1.0
--------------------------------------------------------------------------------
Compared 573 absolute pose pairs.
Calculating APE for translation part pose relation...
--------------------------------------------------------------------------------
APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)
max 0.168861
mean 0.074616
median 0.073365
min 0.026989
rmse 0.077660
sse 3.455804
std 0.021529
--------------------------------------------------------------------------------
Plotting results...
--------------------------------------------------------------------------------
Saving results to results/difodo.zip...
Plus some interesting plots that allows to see easily the results.
Example of use: RPE (Relative Pose Error)
evo_rpe tum ../tum_evaluation_scripts/data/tum_groundtruth/rgbd_dataset_freiburg1_desk-groundtruth_NO_HEADERS.txt ../tum_evaluation_scripts/data/rosify_difodo_output/desk/TIMESTAMPS_DEPTH_IMAGES_27_01_2020-00_40_23_rosify_difodo_odom_output_MOD.txt -va --plot --plot_mode xz --save_results results/difodo.zip
Results
Loaded 1976 stamps and poses from: ../tum_evaluation_scripts/data/tum_groundtruth/rgbd_dataset_freiburg1_desk-groundtruth_NO_HEADERS.txt
Loaded 574 stamps and poses from: ../tum_evaluation_scripts/data/rosify_difodo_output/desk/TIMESTAMPS_DEPTH_IMAGES_27_01_2020-00_40_23_rosify_difodo_odom_output.txt
Synchronizing trajectories...
Found 573 of max. 1976 possible matching timestamps between...
../tum_evaluation_scripts/data/tum_groundtruth/rgbd_dataset_freiburg1_desk-groundtruth_NO_HEADERS.txt
and: ../tum_evaluation_scripts/data/rosify_difodo_output/desk/TIMESTAMPS_DEPTH_IMAGES_27_01_2020-00_40_23_rosify_difodo_odom_output.txt
..with max. time diff.: 0.01 (s) and time offset: 0.0 (s).
--------------------------------------------------------------------------------
Aligning using Umeyama's method...
Rotation of alignment:
[[ 0.99529182 -0.07491277 0.06150012]
[ 0.0756443 0.99708816 -0.00965063]
[-0.06059809 0.01425733 0.99806042]]
Translation of alignment:
[-0.02917636 -0.11717359 0.02894151]
Scale correction: 1.0
--------------------------------------------------------------------------------
Found 572 pairs with delta 1 (frames) among 573 poses using consecutive pairs.
Compared 572 relative pose pairs, delta = 1 (frames) with consecutive pairs.
Calculating RPE for translation part pose relation...
--------------------------------------------------------------------------------
RPE w.r.t. translation part (m)
for delta = 1 (frames) using consecutive pairs
(with SE(3) Umeyama alignment)
max 0.105270
mean 0.024026
median 0.021812
min 0.000000
rmse 0.029693
sse 0.504321
std 0.017448
--------------------------------------------------------------------------------
Plotting results...
--------------------------------------------------------------------------------
Problem with TUM and EVO
Rigth now there is one problem with odometry_evaluation_file_creator, not a problem with the ROS node itself, but with the way the evaluation of odometry is done. TUM online evaluation tool and EVO both need the two odometry results files to be in the same timestamps. This means that in order to be evaluated one result against the groundtruth the timestamps for the odometry must be at the timestamps the groundtruth data was taken. This of course doesnt makes sense and rosify_difodo uses the current time for the odometry messages it generates. So in order to create valid and useful evaluation files, we need to get the timestamps from one of the topics of TUM rosbags and use them to generate the evaluation file.
One possible solution could be:
- Use the /tf time
- Use the images timestamps (like the first version of this node that was included together within rosify_difodo)
But before doing this in the odometry_evaluation_file_creator first I want to see how SD-SLAM works with TUM dataset in order to create a node that is useful for both rosify_difodo and SD-SLAM.
SD-SLAM
Its been a while since last time I use SD-SLAM. So I have to reinstall it again and use it with TUM dataset. In order to do this the installation and all information on how to use is on the README.md from the SD-SLAM github. https://github.com/JdeRobot/SDslam
Just brieftly instructions:
- First install the lib.
- Then the ROS node (since the ROS node depends on the lib previously compiled)
In order to run it is as simple as this:
rosrun SD-SLAM RGBD TUM1.yaml
The provided .yaml, will need to be properly set, in this case for TUM dataset scenes marked as freidburg1:
%YAML:1.0
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
# Camera calibration and distortion parameters (OpenCV)
Camera.Width: 640
Camera.Height: 480
Camera.fx: 517.306408
Camera.fy: 516.469215
Camera.cx: 318.643040
Camera.cy: 255.313989
Camera.k1: 0.262383
Camera.k2: -0.953104
Camera.p1: -0.005358
Camera.p2: 0.002628
Camera.k3: 1.163314
# Camera frames per second
Camera.fps: 30.0
# IR projector baseline times fx (aprox.)
Camera.bf: 40.0
# Close/Far threshold. Baseline times.
ThDepth: 40.0
# Deptmap values factor
DepthMapFactor: 1.0
# ROS
ROS.CameraTopic: "/camera/rgb/image_color"
ROS.DepthTopic: "/camera/depth/image"
#ROS.IMUTopic: "/imu_data"
For freidburg1, freidburg2 and freidburg3 the camera intrinsic parameters changed so for each scene of the dataset we have to choose among the correct YAML configuration file.
Next steps
After revisiting SD-SLAM I found out that SD-SLAM is not rosify entirely, since it doesnt publish the odometry Pose on a ros topic, it just create a yaml file with the pose and more information.
-
So one of the next thing to do will be modify SD-SLAM so it publish the pose
-
In order to solve the problem with the timestamps, Im going to give a special configuration parameter to both rosify_difodo and SD-SLAM.