| 27 | // } |
| 28 | |
| 29 | void Eval::compute_adj_rpe(Eigen::Matrix4d& gt, |
| 30 | Eigen::Matrix4d& lo, |
| 31 | double& t_e, |
| 32 | double& r_e) { |
| 33 | Eigen::Matrix4d delta_T = lo.inverse() * gt; |
| 34 | |
| 35 | t_e = delta_T.topRightCorner(3, 1).norm(); |
| 36 | |
| 37 | r_e = std::abs(std::acos( |
| 38 | fmin(fmax((delta_T.block<3, 3>(0, 0).trace() - 1) / 2, -1.0), |
| 39 | 1.0))) / |
| 40 | M_PI * 180; |
| 41 | } |
| 42 | |
| 43 | #endif |