From 4894fdfcb0ba7feeca04edaf9ebc25aa9c64b624 Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Fri, 7 Dec 2018 11:17:28 +0100
Subject: [PATCH] Use own implementation for log_R
---
src/rotations.h | 26 ++++++++++++++++++++++++--
1 file changed, 24 insertions(+), 2 deletions(-)
diff --git a/src/rotations.h b/src/rotations.h
index 9657ebeaf..fda907c17 100644
--- a/src/rotations.h
+++ b/src/rotations.h
@@ -205,8 +205,30 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 1> log_R(const Eigen::MatrixBa
typedef typename Derived::Scalar T;
- Eigen::AngleAxis<T> aa = Eigen::AngleAxis<T>(_R);
- return aa.axis() * aa.angle();
+// Eigen::AngleAxis<T> aa(_R);
+// return aa.axis() * aa.angle();
+
+ /*
+ *
+ * phi = ( trace ( R) -1 ) / 2;
+ *
+ * u = vee( R - R')/(2 sin phi)
+ */
+
+ T phi = acos(_R.trace() - (T)1.0 ) / (T)2.0 ;
+
+ if (phi > (T)1e-8)
+ {
+ Eigen::Matrix<T,3,1> u = vee(_R - _R.transpose()) / ((T)2 * sin(phi));
+ return phi*u;
+ }
+ else
+ {
+ // Try a better 1-order approximation instead of this 0-order
+ // Anyway Eigen AngleAxis was using (1,0,0) too
+ Eigen::Matrix<T,3,1> u( (T)1.0, (T)0.0, (T)0.0 );
+ return phi*u;
+ }
}
/** \brief Rotation vector to quaternion conversion
--
GitLab