Code cleanup

This commit is contained in:
Enrico Turri 2019-01-14 08:56:01 +01:00
parent 1d81469d97
commit 4affd6ace7
2 changed files with 19 additions and 25 deletions

View File

@ -1181,7 +1181,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 #if ENABLE_NEW_EULER_ANGLES
bool x_only = (rotation_matrix(0, 0) == 1.0) && (rotation_matrix(0, 1) == 0.0) && (rotation_matrix(0, 2) == 0.0) && (rotation_matrix(1, 0) == 0.0) && (rotation_matrix(2, 0) == 0.0); bool x_only = (rotation_matrix(0, 0) == 1.0) && (rotation_matrix(0, 1) == 0.0) && (rotation_matrix(0, 2) == 0.0) && (rotation_matrix(1, 0) == 0.0) && (rotation_matrix(2, 0) == 0.0);
bool y_only = (rotation_matrix(0, 1) == 0.0) && (rotation_matrix(1, 0) == 0.0) && (rotation_matrix(1, 1) == 1.0) && (rotation_matrix(1, 2) == 0.0) && (rotation_matrix(2, 1) == 0.0); bool y_only = (rotation_matrix(0, 1) == 0.0) && (rotation_matrix(1, 0) == 0.0) && (rotation_matrix(1, 1) == 1.0) && (rotation_matrix(1, 2) == 0.0) && (rotation_matrix(2, 1) == 0.0);
@ -1218,24 +1217,23 @@ Vec3d extract_euler_angles(const Eigen::Matrix<double, 3, 3, Eigen::DontAlign>&
} }
} }
// debug check // // debug check
Geometry::Transformation t; // Geometry::Transformation t;
t.set_rotation(angles); // t.set_rotation(angles);
if (!t.get_matrix().matrix().block(0, 0, 3, 3).isApprox(rotation_matrix)) // if (!t.get_matrix().matrix().block(0, 0, 3, 3).isApprox(rotation_matrix))
{ // {
std::cout << "something went wrong in extracting euler angles from matrix" << std::endl; // std::cout << "something went wrong in extracting euler angles from matrix" << std::endl;
//
// Eigen::Matrix<double, 3, 3, Eigen::DontAlign> m = t.get_matrix().matrix().block(0, 0, 3, 3); //// Eigen::Matrix<double, 3, 3, Eigen::DontAlign> m = t.get_matrix().matrix().block(0, 0, 3, 3);
// for (int r = 0; r < 3; ++r) //// for (int r = 0; r < 3; ++r)
// { //// {
// for (int c = 0; c < 3; ++c) //// for (int c = 0; c < 3; ++c)
// { //// {
// std::cout << r << ", " << c << ": " << m(r, c) << " - " << rotation_matrix(r, c) << std::endl; //// std::cout << r << ", " << c << ": " << m(r, c) << " - " << rotation_matrix(r, c) << std::endl;
// } //// }
// } //// }
} // }
#else #else
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
auto y_only = [](const Eigen::Matrix<double, 3, 3, Eigen::DontAlign>& matrix) -> bool { 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); 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);
}; };
@ -1265,9 +1263,7 @@ Vec3d extract_euler_angles(const Eigen::Matrix<double, 3, 3, Eigen::DontAlign>&
angles(1) = ::atan2(-rotation_matrix(2, 0), cy_abs); 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)); 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 #endif // ENABLE_NEW_EULER_ANGLES
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
return angles; return angles;
} }

View File

@ -49,12 +49,10 @@
#endif // _technologies_h_ #endif // _technologies_h_
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
//==================== //====================
// 1.42.0.alpha3 techs // 1.42.0.alpha4 techs
//==================== //====================
#define ENABLE_1_42_0_ALPHA3 1 #define ENABLE_1_42_0_ALPHA4 1
// Changed algorithm to extract euler angles from rotation matrix // Changed algorithm to extract euler angles from rotation matrix
#define ENABLE_NEW_EULER_ANGLES (1 && ENABLE_1_42_0_ALPHA3) #define ENABLE_NEW_EULER_ANGLES (1 && ENABLE_1_42_0_ALPHA4)
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@