Fixed euler angles calculation for the singular case

This commit is contained in:
Enrico Turri 2019-01-04 14:53:01 +01:00
parent 2ce308693f
commit 94becf120b

View File

@ -1194,9 +1194,9 @@ Vec3d extract_euler_angles(const Eigen::Matrix<double, 3, 3, Eigen::DontAlign>&
} }
else else
{ {
angles(0) = ::atan2(-rotation_matrix(1, 2), rotation_matrix(1, 1)); angles(1) = 0.0;
angles(1) = ::atan2(-rotation_matrix(2, 0), sy); angles(1) = ::atan2(-rotation_matrix(2, 0), sy);
angles(2) = 0.0; angles(2) = (angles(1) >-0.0) ? ::atan2(rotation_matrix(1, 2), rotation_matrix(1, 1)) : ::atan2(-rotation_matrix(1, 2), rotation_matrix(1, 1));
} }
return angles; return angles;