void VideoIMUFusionDevice::handleIMUVelocity( const OSVR_TimeValue ×tamp, const OSVR_AngularVelocityReport &report) { using namespace osvr::util::eigen_interop; Eigen::Quaterniond q = map(report.state.incrementalRotation); Eigen::Vector3d rot; if (q.w() >= 1. || q.vec().isZero(1e-10)) { rot = Eigen::Vector3d::Zero(); } else { #if 0 auto magnitude = q.vec().blueNorm(); rot = (q.vec() / magnitude * (2. * std::atan2(magnitude, q.w()))) / report.state.dt; #else auto angle = std::acos(q.w()); rot = q.vec().normalized() * angle * 2 / report.state.dt; #endif } m_fusion.handleIMUVelocity(timestamp, rot); if (m_fusion.running()) { sendMainPoseReport(); } sendVelocityReport(); }
Eigen::Quaterniond ExponentialMapToQuaternion(const Eigen::Vector3d& exponential_rotation) { double angle = 0.5 * exponential_rotation.norm(); Eigen::Quaterniond quaternion; quaternion.w() = std::cos(angle); quaternion.vec() = 0.5 * boost::math::sinc_pi(angle) * exponential_rotation; return quaternion; }
Eigen::Vector3d QuaternionToExponentialMap(const Eigen::Quaterniond& quaternion) { Eigen::Vector3d vec = quaternion.vec(); if (vec.norm() < ITOMP_EPS) return Eigen::Vector3d::Zero(); double theta = 2.0 * std::acos(quaternion.w()); vec.normalize(); return theta * vec; }
void VideoIMUFusionDevice::handleIMUVelocity( const OSVR_TimeValue ×tamp, const OSVR_AngularVelocityReport &report) { using namespace osvr::util::eigen_interop; Eigen::Quaterniond q = map(report.state.incrementalRotation); Eigen::Vector3d rot; if (q.w() >= 1.) { rot = Eigen::Vector3d::Zero(); } else { auto magnitude = q.vec().blueNorm(); rot = (q.vec() / magnitude * (2. * std::atan2(magnitude, q.w()))) / report.state.dt; /// @todo without transformations being applied to vel quats, this /// is needed. // std::swap(rot[0], rot[1]); rot[1] *= -1.; rot[2] *= -1.; } m_fusion.handleIMUVelocity(timestamp, rot); if (m_fusion.running()) { sendMainPoseReport(); } }