| 86 | |
| 87 | template <typename T> |
| 88 | void CheckIsometry(const Eigen::Transform<T, 3, Eigen::Isometry>& X) |
| 89 | { |
| 90 | CheckRotMat<T>(X.linear()); |
| 91 | Eigen::Matrix<T, 1, 4> bottom_expected; |
| 92 | bottom_expected << 0, 0, 0, 1; |
| 93 | const T bottom_error |
| 94 | = (X.matrix().bottomRows(1) - bottom_expected).array().abs().maxCoeff(); |
| 95 | |
| 96 | DART_UNUSED(bottom_error); |
| 97 | DART_ASSERT( |
| 98 | bottom_error < kCheckTolerance |
| 99 | && "Homogeneous matrix is improperly scaled."); |
| 100 | } |
| 101 | |
| 102 | template <typename T> |
| 103 | void CheckQuaternion(const Eigen::Quaternion<T>& q) |