Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-08-10 03:40:20 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Bug fix for Vive puck calibration of hands and feet

Browse source 
Before this PR, there were small errors that can occur if you calibrate while not facing down the z-axis in vive sensor space.
As indicated by the small arrow in the steam vr grid.

The hands could end up flipped 180 degrees and the feet could end up with an incorrect offset resulting in a wider or narrower stance then expected.
This PR is intended to fix these issues.
This commit is contained in:
Anthony Thibault 2018-09-24 16:56:22 -07:00
parent ea7bc26ad8
commit fa27bb7e11
1 changed files with 15 additions and 18 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

33
plugins/openvr/src/ViveControllerManager.cpp
View file

@ -568,6 +568,7 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr
return; return;
} }
// Compute the defaultToRefrenceMat, this will take inputCalibration default poses into the reference frame. (sensor space)
glm::mat4 defaultToReferenceMat = glm::mat4(); glm::mat4 defaultToReferenceMat = glm::mat4();
if (_headConfig == HeadConfig::HMD) { if (_headConfig == HeadConfig::HMD) {
defaultToReferenceMat = calculateDefaultToReferenceForHmd(inputCalibration); defaultToReferenceMat = calculateDefaultToReferenceForHmd(inputCalibration);
@ -1025,17 +1026,15 @@ void ViveControllerManager::InputDevice::hapticsHelper(float deltaTime, bool lef
void ViveControllerManager::InputDevice::calibrateLeftHand(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair) { void ViveControllerManager::InputDevice::calibrateLeftHand(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair) {
controller::Pose& handPose = handPair.second; controller::Pose& handPose = handPair.second;
glm::mat4 handPoseAvatarMat = createMatFromQuatAndPos(handPose.getRotation(), handPose.getTranslation()); glm::vec3 handPoseZAxis = handPose.getRotation() * glm::vec3(0.0f, 0.0f, 1.0f);
glm::vec3 handPoseTranslation = extractTranslation(handPoseAvatarMat); glm::vec3 defaultHandYAxis = transformVectorFast(defaultToReferenceMat * inputCalibration.defaultLeftHand, glm::vec3(0.0f, 1.0f, 0.0f));
glm::vec3 handPoseZAxis = glmExtractRotation(handPoseAvatarMat) * glm::vec3(0.0f, 0.0f, 1.0f);
glm::vec3 avatarHandYAxis = transformVectorFast(inputCalibration.defaultLeftHand, glm::vec3(0.0f, 1.0f, 0.0f));
const float EPSILON = 1.0e-4f; const float EPSILON = 1.0e-4f;
if (fabsf(fabsf(glm::dot(glm::normalize(avatarHandYAxis), glm::normalize(handPoseZAxis))) - 1.0f) < EPSILON) { if (fabsf(fabsf(glm::dot(glm::normalize(defaultHandYAxis), glm::normalize(handPoseZAxis))) - 1.0f) < EPSILON) {
handPoseZAxis = glm::vec3(0.0f, 0.0f, 1.0f); handPoseZAxis = glm::vec3(0.0f, 0.0f, 1.0f);
} }
glm::vec3 zPrime = handPoseZAxis; glm::vec3 zPrime = handPoseZAxis;
glm::vec3 xPrime = glm::normalize(glm::cross(avatarHandYAxis, handPoseZAxis)); glm::vec3 xPrime = glm::normalize(glm::cross(defaultHandYAxis, handPoseZAxis));
glm::vec3 yPrime = glm::normalize(glm::cross(zPrime, xPrime)); glm::vec3 yPrime = glm::normalize(glm::cross(zPrime, xPrime));
glm::mat4 newHandMat = glm::mat4(glm::vec4(xPrime, 0.0f), glm::vec4(yPrime, 0.0f), glm::mat4 newHandMat = glm::mat4(glm::vec4(xPrime, 0.0f), glm::vec4(yPrime, 0.0f),
@ -1043,7 +1042,7 @@ void ViveControllerManager::InputDevice::calibrateLeftHand(const glm::mat4& defa
glm::vec3 translationOffset = glm::vec3(0.0f, _handPuckYOffset, _handPuckZOffset); glm::vec3 translationOffset = glm::vec3(0.0f, _handPuckYOffset, _handPuckZOffset);
glm::quat initialRotation = glmExtractRotation(handPoseAvatarMat); glm::quat initialRotation = handPose.getRotation();
glm::quat finalRotation = glmExtractRotation(newHandMat); glm::quat finalRotation = glmExtractRotation(newHandMat);
glm::quat rotationOffset = glm::inverse(initialRotation) * finalRotation; glm::quat rotationOffset = glm::inverse(initialRotation) * finalRotation;
@ -1056,17 +1055,15 @@ void ViveControllerManager::InputDevice::calibrateLeftHand(const glm::mat4& defa
void ViveControllerManager::InputDevice::calibrateRightHand(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair) { void ViveControllerManager::InputDevice::calibrateRightHand(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair) {
controller::Pose& handPose = handPair.second; controller::Pose& handPose = handPair.second;
glm::mat4 handPoseAvatarMat = createMatFromQuatAndPos(handPose.getRotation(), handPose.getTranslation()); glm::vec3 handPoseZAxis = handPose.getRotation() * glm::vec3(0.0f, 0.0f, 1.0f);
glm::vec3 handPoseTranslation = extractTranslation(handPoseAvatarMat); glm::vec3 defaultHandYAxis = transformVectorFast(defaultToReferenceMat * inputCalibration.defaultRightHand, glm::vec3(0.0f, 1.0f, 0.0f));
glm::vec3 handPoseZAxis = glmExtractRotation(handPoseAvatarMat) * glm::vec3(0.0f, 0.0f, 1.0f);
glm::vec3 avatarHandYAxis = transformVectorFast(inputCalibration.defaultRightHand, glm::vec3(0.0f, 1.0f, 0.0f));
const float EPSILON = 1.0e-4f; const float EPSILON = 1.0e-4f;
if (fabsf(fabsf(glm::dot(glm::normalize(avatarHandYAxis), glm::normalize(handPoseZAxis))) - 1.0f) < EPSILON) { if (fabsf(fabsf(glm::dot(glm::normalize(defaultHandYAxis), glm::normalize(handPoseZAxis))) - 1.0f) < EPSILON) {
handPoseZAxis = glm::vec3(0.0f, 0.0f, 1.0f); handPoseZAxis = glm::vec3(0.0f, 0.0f, 1.0f);
} }
glm::vec3 zPrime = handPoseZAxis; glm::vec3 zPrime = handPoseZAxis;
glm::vec3 xPrime = glm::normalize(glm::cross(avatarHandYAxis, handPoseZAxis)); glm::vec3 xPrime = glm::normalize(glm::cross(defaultHandYAxis, handPoseZAxis));
glm::vec3 yPrime = glm::normalize(glm::cross(zPrime, xPrime)); glm::vec3 yPrime = glm::normalize(glm::cross(zPrime, xPrime));
glm::mat4 newHandMat = glm::mat4(glm::vec4(xPrime, 0.0f), glm::vec4(yPrime, 0.0f), glm::mat4 newHandMat = glm::mat4(glm::vec4(xPrime, 0.0f), glm::vec4(yPrime, 0.0f),
glm::vec4(zPrime, 0.0f), glm::vec4(0.0f, 0.0f, 0.0f, 1.0f)); glm::vec4(zPrime, 0.0f), glm::vec4(0.0f, 0.0f, 0.0f, 1.0f));
@ -1074,7 +1071,7 @@ void ViveControllerManager::InputDevice::calibrateRightHand(const glm::mat4& def
glm::vec3 translationOffset = glm::vec3(0.0f, _handPuckYOffset, _handPuckZOffset); glm::vec3 translationOffset = glm::vec3(0.0f, _handPuckYOffset, _handPuckZOffset);
glm::quat initialRotation = glmExtractRotation(handPoseAvatarMat); glm::quat initialRotation = handPose.getRotation();
glm::quat finalRotation = glmExtractRotation(newHandMat); glm::quat finalRotation = glmExtractRotation(newHandMat);
glm::quat rotationOffset = glm::inverse(initialRotation) * finalRotation; glm::quat rotationOffset = glm::inverse(initialRotation) * finalRotation;
@ -1105,15 +1102,15 @@ void ViveControllerManager::InputDevice::calibrateFeet(const glm::mat4& defaultT
void ViveControllerManager::InputDevice::calibrateFoot(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& footPair, bool isLeftFoot){ void ViveControllerManager::InputDevice::calibrateFoot(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& footPair, bool isLeftFoot){
controller::Pose footPose = footPair.second; controller::Pose footPose = footPair.second;
glm::mat4 puckPoseAvatarMat = createMatFromQuatAndPos(footPose.getRotation(), footPose.getTranslation()); glm::mat4 puckPoseMat = createMatFromQuatAndPos(footPose.getRotation(), footPose.getTranslation());
glm::mat4 defaultFoot = isLeftFoot ? inputCalibration.defaultLeftFoot : inputCalibration.defaultRightFoot; glm::mat4 defaultFoot = isLeftFoot ? inputCalibration.defaultLeftFoot : inputCalibration.defaultRightFoot;
glm::mat4 footOffset = computeOffset(defaultToReferenceMat, defaultFoot, footPose); glm::mat4 footOffset = computeOffset(defaultToReferenceMat, defaultFoot, footPose);
glm::quat rotationOffset = glmExtractRotation(footOffset); glm::quat rotationOffset = glmExtractRotation(footOffset);
glm::vec3 translationOffset = extractTranslation(footOffset); glm::vec3 translationOffset = extractTranslation(footOffset);
glm::vec3 avatarXAxisInPuckFrame = glm::normalize(transformVectorFast(glm::inverse(puckPoseAvatarMat), glm::vec3(-1.0f, 0.0f, 0.0f))); glm::vec3 localXAxisInPuckFrame = glm::normalize(transformVectorFast(glm::inverse(puckPoseMat) * defaultToReferenceMat, glm::vec3(-1.0f, 0.0f, 0.0f)));
float distance = glm::dot(translationOffset, avatarXAxisInPuckFrame); float distance = glm::dot(translationOffset, localXAxisInPuckFrame);
glm::vec3 finalTranslation = translationOffset - (distance * avatarXAxisInPuckFrame); glm::vec3 finalTranslation = translationOffset - (distance * localXAxisInPuckFrame);
glm::mat4 finalOffset = createMatFromQuatAndPos(rotationOffset, finalTranslation); glm::mat4 finalOffset = createMatFromQuatAndPos(rotationOffset, finalTranslation);
if (isLeftFoot) { if (isLeftFoot) {