Function CalcAngle

deps/glomap/glomap/math/rigid3d.cc:7–15

namespace glomap {

double CalcAngle(const Rigid3d& pose1, const Rigid3d& pose2) {
  double cos_r =
      ((pose1.rotation.inverse() * pose2.rotation).toRotationMatrix().trace() -
       1) /
      2;
  cos_r = std::min(std::max(cos_r, -1.), 1.);

  return std::acos(cos_r) * 180 / EIGEN_PI;
}

double CalcTrans(const Rigid3d& pose1, const Rigid3d& pose2) {
  return (Inverse(pose1).translation - Inverse(pose2).translation).norm();