| 1165 | } |
| 1166 | |
| 1167 | static Eigen::Matrix3d YPR2Rot(Eigen::Vector3d ypr) |
| 1168 | { |
| 1169 | double y = ypr(0) / 180.0 * M_PI; |
| 1170 | double p = ypr(1) / 180.0 * M_PI; |
| 1171 | double r = ypr(2) / 180.0 * M_PI; |
| 1172 | |
| 1173 | Eigen::Matrix3d Rz; |
| 1174 | Rz << cos(y), -sin(y), 0.0, |
| 1175 | sin(y), cos(y), 0.0, |
| 1176 | 0.0, 0.0, 1; |
| 1177 | |
| 1178 | Eigen::Matrix3d Ry; |
| 1179 | Ry << cos(p), 0.0, sin(p), |
| 1180 | 0.0, 1.0, 0.0, |
| 1181 | -sin(p), 0.0, cos(p); |
| 1182 | |
| 1183 | Eigen::Matrix3d Rx; |
| 1184 | Rx << 1.0, 0.0, 0.0, |
| 1185 | 0.0, cos(r), -sin(r), |
| 1186 | 0.0, sin(r), cos(r); |
| 1187 | |
| 1188 | return Rz * Ry * Rx; |
| 1189 | } |
| 1190 | |
| 1191 | static Eigen::Vector3d Rot2YPR(const Eigen::Matrix3d R) |
| 1192 | { |