Tech ENABLE_NEW_EULER_ANGLES set as default

This commit is contained in:
Enrico Turri 2019-05-21 10:16:44 +02:00
parent ac368e2c9e
commit 91b97337b0
2 changed files with 1 additions and 44 deletions

View File

@ -1180,7 +1180,6 @@ Transform3d assemble_transform(const Vec3d& translation, const Vec3d& rotation,
Vec3d extract_euler_angles(const Eigen::Matrix<double, 3, 3, Eigen::DontAlign>& rotation_matrix)
{
#if ENABLE_NEW_EULER_ANGLES
// reference: http://www.gregslabaugh.net/publications/euler.pdf
Vec3d angles1 = Vec3d::Zero();
Vec3d angles2 = Vec3d::Zero();
@ -1219,40 +1218,7 @@ Vec3d extract_euler_angles(const Eigen::Matrix<double, 3, 3, Eigen::DontAlign>&
double min_2 = angles2.cwiseAbs().minCoeff();
bool use_1 = (min_1 < min_2) || (is_approx(min_1, min_2) && (angles1.norm() <= angles2.norm()));
Vec3d angles = use_1 ? angles1 : angles2;
#else
auto y_only = [](const Eigen::Matrix<double, 3, 3, Eigen::DontAlign>& matrix) -> bool {
return (matrix(0, 1) == 0.0) && (matrix(1, 0) == 0.0) && (matrix(1, 1) == 1.0) && (matrix(1, 2) == 0.0) && (matrix(2, 1) == 0.0);
};
// see: https://www.learnopencv.com/rotation-matrix-to-euler-angles/
double cy_abs = ::sqrt(sqr(rotation_matrix(0, 0)) + sqr(rotation_matrix(1, 0)));
Vec3d angles = Vec3d::Zero();
if (cy_abs >= 1e-6)
{
angles(0) = ::atan2(rotation_matrix(2, 1), rotation_matrix(2, 2));
angles(1) = ::atan2(-rotation_matrix(2, 0), cy_abs);
angles(2) = ::atan2(rotation_matrix(1, 0), rotation_matrix(0, 0));
// this is an hack to try to avoid this function to return "strange" values due to gimbal lock
if (y_only(rotation_matrix) && (angles(0) == (double)PI) && (angles(2) == (double)PI))
{
angles(0) = 0.0;
angles(1) = ::atan2(rotation_matrix(2, 0), cy_abs) - (double)PI;
angles(2) = 0.0;
}
}
else
{
angles(0) = 0.0;
angles(1) = ::atan2(-rotation_matrix(2, 0), cy_abs);
angles(2) = (angles(1) >= 0.0) ? ::atan2(rotation_matrix(1, 2), rotation_matrix(1, 1)) : ::atan2(-rotation_matrix(1, 2), rotation_matrix(1, 1));
}
#endif // ENABLE_NEW_EULER_ANGLES
return angles;
return use_1 ? angles1 : angles2;
}
Vec3d extract_euler_angles(const Transform3d& transform)

View File

@ -28,15 +28,6 @@
#define ENABLE_NONCUSTOM_DATA_VIEW_RENDERING (0 && ENABLE_1_42_0_ALPHA1)
//====================
// 1.42.0.alpha4 techs
//====================
#define ENABLE_1_42_0_ALPHA4 1
// Changed algorithm to extract euler angles from rotation matrix
#define ENABLE_NEW_EULER_ANGLES (1 && ENABLE_1_42_0_ALPHA4)
//====================
// 1.42.0.alpha7 techs
//====================