Tech ENABLE_NEW_EULER_ANGLES set as default
This commit is contained in:
parent
ac368e2c9e
commit
91b97337b0
@ -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)
|
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
|
// reference: http://www.gregslabaugh.net/publications/euler.pdf
|
||||||
Vec3d angles1 = Vec3d::Zero();
|
Vec3d angles1 = Vec3d::Zero();
|
||||||
Vec3d angles2 = 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();
|
double min_2 = angles2.cwiseAbs().minCoeff();
|
||||||
bool use_1 = (min_1 < min_2) || (is_approx(min_1, min_2) && (angles1.norm() <= angles2.norm()));
|
bool use_1 = (min_1 < min_2) || (is_approx(min_1, min_2) && (angles1.norm() <= angles2.norm()));
|
||||||
|
|
||||||
Vec3d angles = use_1 ? angles1 : angles2;
|
return 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;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
Vec3d extract_euler_angles(const Transform3d& transform)
|
Vec3d extract_euler_angles(const Transform3d& transform)
|
||||||
|
@ -28,15 +28,6 @@
|
|||||||
#define ENABLE_NONCUSTOM_DATA_VIEW_RENDERING (0 && ENABLE_1_42_0_ALPHA1)
|
#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
|
// 1.42.0.alpha7 techs
|
||||||
//====================
|
//====================
|
||||||
|
Loading…
Reference in New Issue
Block a user