| 15 | } |
| 16 | |
| 17 | double CalcTrans(const Rigid3d& pose1, const Rigid3d& pose2) { |
| 18 | return (Inverse(pose1).translation - Inverse(pose2).translation).norm(); |
| 19 | } |
| 20 | |
| 21 | double CalcTransAngle(const Rigid3d& pose1, const Rigid3d& pose2) { |
| 22 | double cos_r = (pose1.translation).dot(pose2.translation) / |
nothing calls this directly
no outgoing calls
no test coverage detected