rotationMatrix() and angles give different results

[libdoron]

I have a Sophus::SE3f object. I need to export it's rotation angles and use them elsewhere to rebuild the rotation matrix.

In my code, I export the rotation matrix in 3 ways:

1. using angleX(), angleY(), angleZ()
2. using quaternion, as mentioned in #309
3. using rotationMatrix()

void saveSE3(Sophus::SE3f se3)
{
    double yaw(se3.angleX());
    double pitch(se3.angleY());
    double roll(se3.angleZ());

    ofstream("save_file.txt") << std::fixed << std::setprecision(18) 
                         << yaw << ", "
                         << pitch << ", "
                         << roll << std::endl << std::endl;

    auto q1 = se3.unit_quaternion();
    auto euler = q1.toRotationMatrix().eulerAngles(0, 1, 2);
    ofstream("save_file.txt", std::ofstream::app) << std::fixed << std::setprecision(18) 
                         << euler << std::endl << std::endl;

    ofstream("save_file.txt", std::ofstream::app) << std::fixed << std::setprecision(18) 
                         << se3_.rotationMatrix() << std::endl;

}

In my example, I get the following values:

0.080531626939773560, -0.184074640274047852, -0.106850937008857727

0.071102283895015717, -0.187578573822975159, -0.100159876048564911

0.977534770965576172  0.098238475620746613 -0.186480477452278137
-0.112921454012393951  0.991149425506591797 -0.069796212017536163
 0.177973344922065735  0.089285872876644135  0.979976296424865723

The first thing I notice is that the first set of angles (from angleX, angleY, angleZ functions) is slightly(?) different than then second one (extracted from the quaternion).

I later copy those values to Python3 and try to rebuild the rotation matrix:

from scipy.spatial.transform import Rotation as R
import numpy as np

Rorigin = np.array([0.977534770965576172,  0.098238475620746613, -0.186480477452278137,
-0.112921454012393951,  0.991149425506591797, -0.069796212017536163,
 0.177973344922065735,  0.089285872876644135,  0.979976296424865723]).reshape([3,3])

yaw_rad, pitch_rad, roll_rad = 0.080531626939773560, -0.184074640274047852, -0.106850937008857727
Rangles = R.from_euler('xyz', [yaw_rad, pitch_rad, roll_rad], degrees=False).as_matrix()

yaw_rad, pitch_rad, roll_rad = 0.071102283895015717, -0.187578573822975159, -0.100159876048564911
Rquat = R.from_euler('xyz', [yaw_rad, pitch_rad, roll_rad], degrees=False).as_matrix()

print(Rangles)
[[ 0.97749926,  0.09166174, -0.18998241],
 [-0.10484603,  0.99264475, -0.06052862],
 [ 0.18303689,  0.07908558,  0.97991988]]

print(Rquat)
[[ 0.97753477,  0.08655822, -0.19218077],
 [-0.09823849,  0.99379885, -0.0520868 ],
 [ 0.18648049,  0.06979621,  0.97997628]]

print(Rorigin)
[[ 0.97753477  0.09823848 -0.18648048]
 [-0.11292145  0.99114943 -0.06979621]
 [ 0.17797334  0.08928587  0.9799763 ]]

Neither Rangles nor Rquat have the same values as Rorigin.

When I use these matrices to rotate a 3d point and then project it on a camera I get a 5 pixels difference, so this is not a minor precision issue.

Should I use another method to export those angles? Another way to reconstruct the rotation matrix from those angles?

[DmitriyKorchemkin]

angle?() methods return rotation angle around corresponding axis that acts as similar as possible (in l2 sense) on the points from corresponding plane. It is not expected to match with Euler angles in all cases except some degenerate ones (rotations purely around one of the axes).

scipy.spatial.transform.Rotation.from_euler supports both intrinsic and extrinsic Euler angles denoted by different case of letters (parameter seq described in https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.from_euler.html ). If you try scipy.spatial.transform.Rotation.from_euler('XYZ', ...) -- you'll get Rorigin (up to precision of specified values)

[libdoron]

Thanks @DmitriyKorchemkin Using scipy.spatial.transform.Rotation.from_euler('XYZ', ...) with the angles I get from the 'angle?()' functions, I still don't get the original rotation matrix. Taking the example from my original post:

yaw_rad, pitch_rad, roll_rad = 0.080531626939773560, -0.184074640274047852, -0.106850937008857727
Rangles = R.from_euler('XYZ', [yaw_rad, pitch_rad, roll_rad], degrees=False).as_matrix()

I now get:

array([[ 0.97749926,  0.10484603, -0.18303689],
       [-0.12094245,  0.98950412, -0.07908558],
       [ 0.17282394,  0.09944303,  0.97991988]])

Which is still not so similar to Rorigin. I looked over multiple examples and the results I get are still very different. Still, do you think it is correct to assume that if I need the Euler angles I should stick to the rotationMatrix() function and extract the euler angles from there?

[DmitriyKorchemkin]

I've meant that you need to use scipy.spatial.transform.Rotation.from_euler('XYZ', ...) with the angles you get from Eigen::MatrixBase<...>::eulerAngles(0, 1, 2);.

Seem to match perfectly:

>>> import scipy.spatial.transform as sst
>>> sst.Rotation.from_euler('XYZ', [0.071102283895015717, -0.187578573822975159, -0.100159876048564911]).as_matrix()
array([[ 0.97753477,  0.09823849, -0.18648049],
       [-0.11292146,  0.99114945, -0.06979621],
       [ 0.17797336,  0.08928587,  0.97997628]])

Angles that you get from Sophus::SO3<...>::angle?() will never match Euler angles except some corner-cases, because they're different and somewhat unrelated quantities.

[libdoron]

I checked and you are right - they match. Thanks!