| 19 | } |
| 20 | |
| 21 | double CalcTransAngle(const Rigid3d& pose1, const Rigid3d& pose2) { |
| 22 | double cos_r = (pose1.translation).dot(pose2.translation) / |
| 23 | (pose1.translation.norm() * pose2.translation.norm()); |
| 24 | cos_r = std::min(std::max(cos_r, -1.), 1.); |
| 25 | |
| 26 | return std::acos(cos_r) * 180 / EIGEN_PI; |
| 27 | } |
| 28 | |
| 29 | double CalcAngle(const Eigen::Matrix3d& rotation1, |
| 30 | const Eigen::Matrix3d& rotation2) { |
nothing calls this directly
no outgoing calls
no test coverage detected