| 48 | |
| 49 | template <typename Derived> |
| 50 | static Eigen::Matrix<typename Derived::Scalar, 4, 4> Qleft(const Eigen::QuaternionBase<Derived>& q) { |
| 51 | Eigen::Quaternion<typename Derived::Scalar> qq = positify(q); |
| 52 | Eigen::Matrix<typename Derived::Scalar, 4, 4> ans; |
| 53 | ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose(); |
| 54 | ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = |
| 55 | qq.w() * Eigen::Matrix<typename Derived::Scalar, 3, 3>::Identity() + |
| 56 | skewSymmetric(qq.vec()); |
| 57 | return ans; |
| 58 | } |
| 59 | |
| 60 | template <typename Derived> |
| 61 | static Eigen::Matrix<typename Derived::Scalar, 4, 4> Qright(const Eigen::QuaternionBase<Derived>& p) { |
nothing calls this directly
no outgoing calls
no test coverage detected