Calculation of RPE wrt rotation for TUM data files

[claydergc]

Hello,

I used evo_rpe to plot the relative pose error of a localization solution compared to ground truth using the TUM file format. The problem occurred when plotting the relative pose error w.r.t. rotation, some of the errors were above 175 degrees. One of these errors is pointed out by the arrows in magenta in the following image.

Then, I used evo_traj to plot the rpy errors individually and see what happened. This is the graph where another big error between the ground truth and the localization solution is shown in a magenta ellipse.

The transition from the negative yaw to the positive yaw of the previous graph occurs in these two quaternions (qw, qx, qy, qz):

quat_sol_714_14 = [0.01056662665802486 -0.06599253533823049 -0.14999789477837638 -0.9864248391249876]

quat_sol_714_15 = [0.0040815509665283745 -0.06583596472663102 -0.15311641950263558 -0.9860042235046027]

Those quaternions are similar, but when converting them to RPY these result in [17.2885 -7.6640 -179.9394] and [17.7000 -7.5319 179.3000], respectively.

I understand that the conversion of quaternions to RPY angles is based on the atan2 function (quat2euler) and the atan2 function's domain is [-pi, pi]. So, it would be helpful if the graph of the RPE w.r.t to rotation (the one I showed first) could use some kind of error between quaternions. This is to have a clearer visualization of where high rotation errors occur. Or maybe, is this an issue more related to the way of how RPE w.r.t. rotation is calculated?

Thanks in advance for your response :)

UPDATE:

Since I was not sure if I was doing something wrong, I manually created two files with two poses each one:

In the ground truth file (gt.txt) the first pose is 0,0,0 with 0 rotation and the second pose is 1,0,0 with a rotation of pi radians in yaw.

gt.txt:

0.0 0 0 0 0 0 0 1
0.1 1 0 0 0 0 1 0

In the solution file (test.txt) the first pose is 0,0,0 with 0 rotation and the second pose is 1,0,0 with a rotation of pi+0.01 radians in yaw. I used the eul2quat() function of Matlab to get the quaternion.

test.txt:

0.0 0 0 0 0 0 0 1
0.1 1 0 0 0 0 1 -0.005

When running evo_traj and looking at the RPY angle errors. This is what I obtained:

Again, the pi+0.01 yaw rotation is wrongly represented. As I mentioned earlier, this is not a problem of the evo package, but it could be useful to have a rotation error visualization based on quaternions.

Thanks again for your nice package :)