Exemple #1
0
void VideoIMUFusionDevice::handleIMUVelocity(
    const OSVR_TimeValue &timestamp, 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();
}
Exemple #2
0
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;
}
Exemple #3
0
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;
}
Exemple #4
0
void VideoIMUFusionDevice::handleIMUVelocity(
    const OSVR_TimeValue &timestamp, 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();
    }
}