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();
}
}