Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-04-11 12:20:26 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions 2

Fixes for Vive puck calibration

Browse source
This commit is contained in:
Anthony J. Thibault 2017-09-20 15:41:49 -07:00
parent a8a5138c18
commit 09c61deda8
5 changed files with 70 additions and 49 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

40
interface/src/avatar/MyAvatar.cpp
View file

@ -3035,9 +3035,9 @@ glm::mat4 MyAvatar::getCenterEyeCalibrationMat() const {
if (rightEyeIndex >= 0 && leftEyeIndex >= 0) {
auto centerEyePos = (getAbsoluteDefaultJointTranslationInObjectFrame(rightEyeIndex) + getAbsoluteDefaultJointTranslationInObjectFrame(leftEyeIndex)) * 0.5f;
auto centerEyeRot = Quaternions::Y_180;
return createMatFromQuatAndPos(centerEyeRot, centerEyePos);
return createMatFromQuatAndPos(centerEyeRot, centerEyePos / getSensorToWorldScale());
} else {
return createMatFromQuatAndPos(DEFAULT_AVATAR_MIDDLE_EYE_ROT, DEFAULT_AVATAR_MIDDLE_EYE_POS);
return createMatFromQuatAndPos(DEFAULT_AVATAR_MIDDLE_EYE_ROT, DEFAULT_AVATAR_MIDDLE_EYE_POS / getSensorToWorldScale());
}
}
@ -3047,9 +3047,9 @@ glm::mat4 MyAvatar::getHeadCalibrationMat() const {
if (headIndex >= 0) {
auto headPos = getAbsoluteDefaultJointTranslationInObjectFrame(headIndex);
auto headRot = getAbsoluteDefaultJointRotationInObjectFrame(headIndex);
return createMatFromQuatAndPos(headRot, headPos);
return createMatFromQuatAndPos(headRot, headPos / getSensorToWorldScale());
} else {
return createMatFromQuatAndPos(DEFAULT_AVATAR_HEAD_ROT, DEFAULT_AVATAR_HEAD_POS);
return createMatFromQuatAndPos(DEFAULT_AVATAR_HEAD_ROT, DEFAULT_AVATAR_HEAD_POS / getSensorToWorldScale());
}
}
@ -3059,9 +3059,9 @@ glm::mat4 MyAvatar::getSpine2CalibrationMat() const {
if (spine2Index >= 0) {
auto spine2Pos = getAbsoluteDefaultJointTranslationInObjectFrame(spine2Index);
auto spine2Rot = getAbsoluteDefaultJointRotationInObjectFrame(spine2Index);
return createMatFromQuatAndPos(spine2Rot, spine2Pos);
return createMatFromQuatAndPos(spine2Rot, spine2Pos / getSensorToWorldScale());
} else {
return createMatFromQuatAndPos(DEFAULT_AVATAR_SPINE2_ROT, DEFAULT_AVATAR_SPINE2_POS);
return createMatFromQuatAndPos(DEFAULT_AVATAR_SPINE2_ROT, DEFAULT_AVATAR_SPINE2_POS / getSensorToWorldScale());
}
}
@ -3071,9 +3071,9 @@ glm::mat4 MyAvatar::getHipsCalibrationMat() const {
if (hipsIndex >= 0) {
auto hipsPos = getAbsoluteDefaultJointTranslationInObjectFrame(hipsIndex);
auto hipsRot = getAbsoluteDefaultJointRotationInObjectFrame(hipsIndex);
return createMatFromQuatAndPos(hipsRot, hipsPos);
return createMatFromQuatAndPos(hipsRot, hipsPos / getSensorToWorldScale());
} else {
return createMatFromQuatAndPos(DEFAULT_AVATAR_HIPS_ROT, DEFAULT_AVATAR_HIPS_POS);
return createMatFromQuatAndPos(DEFAULT_AVATAR_HIPS_ROT, DEFAULT_AVATAR_HIPS_POS / getSensorToWorldScale());
}
}
@ -3083,9 +3083,9 @@ glm::mat4 MyAvatar::getLeftFootCalibrationMat() const {
if (leftFootIndex >= 0) {
auto leftFootPos = getAbsoluteDefaultJointTranslationInObjectFrame(leftFootIndex);
auto leftFootRot = getAbsoluteDefaultJointRotationInObjectFrame(leftFootIndex);
return createMatFromQuatAndPos(leftFootRot, leftFootPos);
return createMatFromQuatAndPos(leftFootRot, leftFootPos / getSensorToWorldScale());
} else {
return createMatFromQuatAndPos(DEFAULT_AVATAR_LEFTFOOT_ROT, DEFAULT_AVATAR_LEFTFOOT_POS);
return createMatFromQuatAndPos(DEFAULT_AVATAR_LEFTFOOT_ROT, DEFAULT_AVATAR_LEFTFOOT_POS / getSensorToWorldScale());
}
}
@ -3095,9 +3095,9 @@ glm::mat4 MyAvatar::getRightFootCalibrationMat() const {
if (rightFootIndex >= 0) {
auto rightFootPos = getAbsoluteDefaultJointTranslationInObjectFrame(rightFootIndex);
auto rightFootRot = getAbsoluteDefaultJointRotationInObjectFrame(rightFootIndex);
return createMatFromQuatAndPos(rightFootRot, rightFootPos);
return createMatFromQuatAndPos(rightFootRot, rightFootPos / getSensorToWorldScale());
} else {
return createMatFromQuatAndPos(DEFAULT_AVATAR_RIGHTFOOT_ROT, DEFAULT_AVATAR_RIGHTFOOT_POS);
return createMatFromQuatAndPos(DEFAULT_AVATAR_RIGHTFOOT_ROT, DEFAULT_AVATAR_RIGHTFOOT_POS / getSensorToWorldScale());
}
}
@ -3107,9 +3107,9 @@ glm::mat4 MyAvatar::getRightArmCalibrationMat() const {
if (rightArmIndex >= 0) {
auto rightArmPos = getAbsoluteDefaultJointTranslationInObjectFrame(rightArmIndex);
auto rightArmRot = getAbsoluteDefaultJointRotationInObjectFrame(rightArmIndex);
return createMatFromQuatAndPos(rightArmRot, rightArmPos);
return createMatFromQuatAndPos(rightArmRot, rightArmPos / getSensorToWorldScale());
} else {
return createMatFromQuatAndPos(DEFAULT_AVATAR_RIGHTARM_ROT, DEFAULT_AVATAR_RIGHTARM_POS);
return createMatFromQuatAndPos(DEFAULT_AVATAR_RIGHTARM_ROT, DEFAULT_AVATAR_RIGHTARM_POS / getSensorToWorldScale());
}
}
@ -3118,9 +3118,9 @@ glm::mat4 MyAvatar::getLeftArmCalibrationMat() const {
if (leftArmIndex >= 0) {
auto leftArmPos = getAbsoluteDefaultJointTranslationInObjectFrame(leftArmIndex);
auto leftArmRot = getAbsoluteDefaultJointRotationInObjectFrame(leftArmIndex);
return createMatFromQuatAndPos(leftArmRot, leftArmPos);
return createMatFromQuatAndPos(leftArmRot, leftArmPos / getSensorToWorldScale());
} else {
return createMatFromQuatAndPos(DEFAULT_AVATAR_LEFTARM_ROT, DEFAULT_AVATAR_LEFTARM_POS);
return createMatFromQuatAndPos(DEFAULT_AVATAR_LEFTARM_ROT, DEFAULT_AVATAR_LEFTARM_POS / getSensorToWorldScale());
}
}
@ -3129,9 +3129,9 @@ glm::mat4 MyAvatar::getRightHandCalibrationMat() const {
if (rightHandIndex >= 0) {
auto rightHandPos = getAbsoluteDefaultJointTranslationInObjectFrame(rightHandIndex);
auto rightHandRot = getAbsoluteDefaultJointRotationInObjectFrame(rightHandIndex);
return createMatFromQuatAndPos(rightHandRot, rightHandPos);
return createMatFromQuatAndPos(rightHandRot, rightHandPos / getSensorToWorldScale());
} else {
return createMatFromQuatAndPos(DEFAULT_AVATAR_RIGHTHAND_ROT, DEFAULT_AVATAR_RIGHTHAND_POS);
return createMatFromQuatAndPos(DEFAULT_AVATAR_RIGHTHAND_ROT, DEFAULT_AVATAR_RIGHTHAND_POS / getSensorToWorldScale());
}
}
@ -3140,9 +3140,9 @@ glm::mat4 MyAvatar::getLeftHandCalibrationMat() const {
if (leftHandIndex >= 0) {
auto leftHandPos = getAbsoluteDefaultJointTranslationInObjectFrame(leftHandIndex);
auto leftHandRot = getAbsoluteDefaultJointRotationInObjectFrame(leftHandIndex);
return createMatFromQuatAndPos(leftHandRot, leftHandPos);
return createMatFromQuatAndPos(leftHandRot, leftHandPos / getSensorToWorldScale());
} else {
return createMatFromQuatAndPos(DEFAULT_AVATAR_LEFTHAND_ROT, DEFAULT_AVATAR_LEFTHAND_POS);
return createMatFromQuatAndPos(DEFAULT_AVATAR_LEFTHAND_ROT, DEFAULT_AVATAR_LEFTHAND_POS / getSensorToWorldScale());
}
}

8
libraries/shared/src/GLMHelpers.cpp
View file

@ -473,6 +473,14 @@ glm::mat4 createMatFromScaleQuatAndPos(const glm::vec3& scale, const glm::quat&
glm::vec4(zAxis, 0.0f), glm::vec4(trans, 1.0f));
}
glm::mat4 createMatFromScale(const glm::vec3& scale) {
glm::vec3 xAxis = glm::vec3(scale.x, 0.0f, 0.0f);
glm::vec3 yAxis = glm::vec3(0.0f, scale.y, 0.0f);
glm::vec3 zAxis = glm::vec3(0.0f, 0.0f, scale.z);
return glm::mat4(glm::vec4(xAxis, 0.0f), glm::vec4(yAxis, 0.0f),
glm::vec4(zAxis, 0.0f), glm::vec4(Vectors::ZERO, 1.0f));
}
// cancel out roll
glm::quat cancelOutRoll(const glm::quat& q) {
glm::vec3 forward = q * Vectors::FRONT;

1
libraries/shared/src/GLMHelpers.h
View file

@ -231,6 +231,7 @@ glm::tvec4<T, P> lerp(const glm::tvec4<T, P>& x, const glm::tvec4<T, P>& y, T a)
glm::mat4 createMatFromQuatAndPos(const glm::quat& q, const glm::vec3& p);
glm::mat4 createMatFromScaleQuatAndPos(const glm::vec3& scale, const glm::quat& rot, const glm::vec3& trans);
glm::mat4 createMatFromScale(const glm::vec3& scale);
glm::quat cancelOutRoll(const glm::quat& q);
glm::quat cancelOutRollAndPitch(const glm::quat& q);
glm::mat4 cancelOutRollAndPitch(const glm::mat4& m);

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

@ -375,7 +375,7 @@ void ViveControllerManager::InputDevice::update(float deltaTime, const controlle
calibrateFromHandController(inputCalibrationData);
calibrateFromUI(inputCalibrationData);
updateCalibratedLimbs();
updateCalibratedLimbs(inputCalibrationData);
_lastSimPoseData = _nextSimPoseData;
}
@ -676,40 +676,55 @@ void ViveControllerManager::InputDevice::uncalibrate() {
_overrideHands = false;
}
void ViveControllerManager::InputDevice::updateCalibratedLimbs() {
_poseStateMap[controller::LEFT_FOOT] = addOffsetToPuckPose(controller::LEFT_FOOT);
_poseStateMap[controller::RIGHT_FOOT] = addOffsetToPuckPose(controller::RIGHT_FOOT);
_poseStateMap[controller::HIPS] = addOffsetToPuckPose(controller::HIPS);
_poseStateMap[controller::SPINE2] = addOffsetToPuckPose(controller::SPINE2);
_poseStateMap[controller::RIGHT_ARM] = addOffsetToPuckPose(controller::RIGHT_ARM);
_poseStateMap[controller::LEFT_ARM] = addOffsetToPuckPose(controller::LEFT_ARM);
void ViveControllerManager::InputDevice::updateCalibratedLimbs(const controller::InputCalibrationData& inputCalibration) {
_poseStateMap[controller::LEFT_FOOT] = addOffsetToPuckPose(inputCalibration, controller::LEFT_FOOT);
_poseStateMap[controller::RIGHT_FOOT] = addOffsetToPuckPose(inputCalibration, controller::RIGHT_FOOT);
_poseStateMap[controller::HIPS] = addOffsetToPuckPose(inputCalibration, controller::HIPS);
_poseStateMap[controller::SPINE2] = addOffsetToPuckPose(inputCalibration, controller::SPINE2);
_poseStateMap[controller::RIGHT_ARM] = addOffsetToPuckPose(inputCalibration, controller::RIGHT_ARM);
_poseStateMap[controller::LEFT_ARM] = addOffsetToPuckPose(inputCalibration, controller::LEFT_ARM);
if (_overrideHead) {
_poseStateMap[controller::HEAD] = addOffsetToPuckPose(controller::HEAD);
_poseStateMap[controller::HEAD] = addOffsetToPuckPose(inputCalibration, controller::HEAD);
}
if (_overrideHands) {
_poseStateMap[controller::LEFT_HAND] = addOffsetToPuckPose(controller::LEFT_HAND);
_poseStateMap[controller::RIGHT_HAND] = addOffsetToPuckPose(controller::RIGHT_HAND);
_poseStateMap[controller::LEFT_HAND] = addOffsetToPuckPose(inputCalibration, controller::LEFT_HAND);
_poseStateMap[controller::RIGHT_HAND] = addOffsetToPuckPose(inputCalibration, controller::RIGHT_HAND);
}
}
controller::Pose ViveControllerManager::InputDevice::addOffsetToPuckPose(int joint) const {
controller::Pose ViveControllerManager::InputDevice::addOffsetToPuckPose(const controller::InputCalibrationData& inputCalibration, int joint) const {
auto puck = _jointToPuckMap.find(joint);
if (puck != _jointToPuckMap.end()) {
uint32_t puckIndex = puck->second;
auto puckPose = _poseStateMap.find(puckIndex);
auto puckPostOffset = _pucksPostOffset.find(puckIndex);
auto puckPreOffset = _pucksPreOffset.find(puckIndex);
if (puckPose != _poseStateMap.end()) {
if (puckPreOffset != _pucksPreOffset.end() && puckPostOffset != _pucksPostOffset.end()) {
return puckPose->second.postTransform(puckPostOffset->second).transform(puckPreOffset->second);
} else if (puckPostOffset != _pucksPostOffset.end()) {
return puckPose->second.postTransform(puckPostOffset->second);
} else if (puckPreOffset != _pucksPreOffset.end()) {
return puckPose->second.transform(puckPreOffset->second);
// use sensor space pose.
auto puckPoseIter = _validTrackedObjects.begin();
while (puckPoseIter != _validTrackedObjects.end()) {
if (puckPoseIter->first == puckIndex) {
break;
}
puckPoseIter++;
}
//auto puckPoseIter = _poseStateMap.find(puckIndex);
if (puckPoseIter != _validTrackedObjects.end()) {
glm::mat4 postMat; // identity
auto postIter = _pucksPostOffset.find(puckIndex);
if (postIter != _pucksPostOffset.end()) {
postMat = postIter->second;
}
glm::mat4 preMat = glm::inverse(inputCalibration.avatarMat) * inputCalibration.sensorToWorldMat;
auto preIter = _pucksPreOffset.find(puckIndex);
if (preIter != _pucksPreOffset.end()) {
preMat = preMat * preIter->second;
}
return puckPoseIter->second.postTransform(postMat).transform(preMat);
}
}
return controller::Pose();
@ -924,15 +939,12 @@ void ViveControllerManager::InputDevice::handleButtonEvent(float deltaTime, uint
void ViveControllerManager::InputDevice::handleHeadPoseEvent(const controller::InputCalibrationData& inputCalibrationData, const mat4& mat,
const vec3& linearVelocity, const vec3& angularVelocity) {
//perform a 180 flip to make the HMD face the +z instead of -z, beacuse the head faces +z
glm::mat4 matYFlip = mat * Matrices::Y_180;
controller::Pose pose(extractTranslation(matYFlip), glmExtractRotation(matYFlip), linearVelocity, angularVelocity);
glm::mat4 sensorToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat;
glm::mat4 defaultHeadOffset = glm::inverse(inputCalibrationData.defaultCenterEyeMat) * inputCalibrationData.defaultHeadMat;
controller::Pose hmdHeadPose = pose.transform(sensorToAvatar);
_poseStateMap[controller::HEAD] = hmdHeadPose.postTransform(defaultHeadOffset);
glm::mat4 sensorToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat;
_poseStateMap[controller::HEAD] = pose.postTransform(defaultHeadOffset).transform(sensorToAvatar);
}
void ViveControllerManager::InputDevice::handlePoseEvent(float deltaTime, const controller::InputCalibrationData& inputCalibrationData,

4
plugins/openvr/src/ViveControllerManager.h
View file

@ -79,10 +79,10 @@ private:
void sendUserActivityData(QString activity);
void configureCalibrationSettings(const QJsonObject configurationSettings);
QJsonObject configurationSettings();
controller::Pose addOffsetToPuckPose(int joint) const;
controller::Pose addOffsetToPuckPose(const controller::InputCalibrationData& inputCalibration, int joint) const;
glm::mat4 calculateDefaultToReferenceForHeadPuck(const controller::InputCalibrationData& inputCalibration);
glm::mat4 calculateDefaultToReferenceForHmd(const controller::InputCalibrationData& inputCalibration);
void updateCalibratedLimbs();
void updateCalibratedLimbs(const controller::InputCalibrationData& inputCalibration);
bool checkForCalibrationEvent();
void handleHandController(float deltaTime, uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData, bool isLeftHand);
void handleHmd(uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData);