Function RotationToAngleAxis

deps/glomap/glomap/math/rigid3d.cc:47–51

}

Eigen::Vector3d RotationToAngleAxis(const Eigen::Matrix3d& rot) {
  Eigen::AngleAxis<double> aa(rot);
  Eigen::Vector3d aa_vec = aa.angle() * aa.axis();
  return aa_vec;
}

Eigen::Matrix3d AngleAxisToRotation(const Eigen::Vector3d& aa_vec) {
  double aa_norm = aa_vec.norm();