From fa6666e32a58c99357c75d27fdb191df407e9800 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 18 Oct 2017 18:58:55 +0200
Subject: [PATCH] Add q2R for vector q
---
src/rotations.h | 13 +++++++++++++
1 file changed, 13 insertions(+)
diff --git a/src/rotations.h b/src/rotations.h
index 1e2426985..a5fc57e56 100644
--- a/src/rotations.h
+++ b/src/rotations.h
@@ -245,6 +245,19 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 3> q2R(const Eigen::Quaternion
return _q.matrix();
}
+/** \brief quaternion to rotation matrix conversion
+ *
+ * @param _q a right-handed unit quaternion
+ * @return the equivalent rotation matrix
+ */
+template<typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3> q2R(const Eigen::MatrixBase<Derived>& _q)
+{
+ MatrixSizeCheck<4,1>::check(_q);
+ Eigen::Quaternion<typename Derived::Scalar> q(_q(3),_q(0),_q(1),_q(2));
+ return q2R( q );
+}
+
/** \brief rotation matrix to quaternion conversion
*
* @param _R a rotation matrix
--
GitLab