From c84e10feeb8a10a8c7fab839d63fccf9c6d67048 Mon Sep 17 00:00:00 2001 From: Daniil Date: Wed, 16 Oct 2024 12:06:21 -0400 Subject: [PATCH] Tweak to fix rot2vec bug --- src/so3/Operations.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/so3/Operations.cpp b/src/so3/Operations.cpp index bcb8cfa..f96aedd 100644 --- a/src/so3/Operations.cpp +++ b/src/so3/Operations.cpp @@ -82,14 +82,14 @@ Eigen::Vector3d rot2vec(const Eigen::Matrix3d& C_ab) { const double phi_ba = acos(std::clamp(0.5 * (C_ab.trace() - 1.0), -1.0, 1.0)); const double sinphi_ba = sin(phi_ba); - if (fabs(sinphi_ba) > 1e-9) { + if (fabs(sinphi_ba) > 1e-6) { // General case, angle is NOT near 0, pi, or 2*pi Eigen::Vector3d axis; axis << C_ab(2, 1) - C_ab(1, 2), C_ab(0, 2) - C_ab(2, 0), C_ab(1, 0) - C_ab(0, 1); return (0.5 * phi_ba / sinphi_ba) * axis; - } else if (fabs(phi_ba) > 1e-9) { + } else if (fabs(phi_ba) > 1e-6) { // Angle is near pi or 2*pi // ** Note with this method we do not know the sign of 'phi', however since // we know phi is