| 304 | } |
| 305 | |
| 306 | void |
| 307 | wRo_to_euler(const Eigen::Matrix3f& wRo, double& yaw, double& pitch, double& roll) |
| 308 | { |
| 309 | yaw = standardRad(std::atan2(wRo(1, 0), wRo(0, 0))); |
| 310 | double c = std::cos(yaw); |
| 311 | double s = sin(yaw); |
| 312 | pitch = standardRad( |
| 313 | std::atan2(static_cast<double>(-wRo(2, 0)), wRo(0, 0) * c + wRo(1, 0) * s)); |
| 314 | roll = standardRad( |
| 315 | std::atan2(wRo(0, 2) * s - wRo(1, 2) * c, -wRo(0, 1) * s + wRo(1, 1) * c)); |
| 316 | } |
| 317 | |
| 318 | void |
| 319 | print_Isometry3d(Eigen::Isometry3d pose, std::stringstream& ss) |
no test coverage detected