Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-06-18 11:40:30 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

more debugging

Browse source
This commit is contained in:
Brad Hefta-Gaub 2016-12-08 23:32:03 -08:00
parent 4959a88581
commit b6838c6ec4
2 changed files with 85 additions and 130 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

6
interface/src/Application.cpp
View file

@ -3977,11 +3977,17 @@ void Application::update(float deltaTime) {
controller::Pose leftHandPose = userInputMapper->getPoseState(controller::Action::LEFT_HAND); controller::Pose leftHandPose = userInputMapper->getPoseState(controller::Action::LEFT_HAND);
controller::Pose rightHandPose = userInputMapper->getPoseState(controller::Action::RIGHT_HAND); controller::Pose rightHandPose = userInputMapper->getPoseState(controller::Action::RIGHT_HAND);
qDebug() << __FUNCTION__ << "...............leftHandPose:" << leftHandPose.translation;
auto myAvatarMatrix = createMatFromQuatAndPos(myAvatar->getOrientation(), myAvatar->getPosition()); auto myAvatarMatrix = createMatFromQuatAndPos(myAvatar->getOrientation(), myAvatar->getPosition());
auto worldToSensorMatrix = glm::inverse(myAvatar->getSensorToWorldMatrix()); auto worldToSensorMatrix = glm::inverse(myAvatar->getSensorToWorldMatrix());
auto avatarToSensorMatrix = worldToSensorMatrix * myAvatarMatrix; auto avatarToSensorMatrix = worldToSensorMatrix * myAvatarMatrix;
myAvatar->setHandControllerPosesInSensorFrame(leftHandPose.transform(avatarToSensorMatrix), rightHandPose.transform(avatarToSensorMatrix)); myAvatar->setHandControllerPosesInSensorFrame(leftHandPose.transform(avatarToSensorMatrix), rightHandPose.transform(avatarToSensorMatrix));
qDebug() << __FUNCTION__ << "POST transform leftHandPose:" << leftHandPose.translation;
updateThreads(deltaTime); // If running non-threaded, then give the threads some time to process... updateThreads(deltaTime); // If running non-threaded, then give the threads some time to process...
updateDialogs(deltaTime); // update various stats dialogs if present updateDialogs(deltaTime); // update various stats dialogs if present

209
plugins/hifiKinect/src/KinectPlugin.cpp
View file

@ -28,7 +28,7 @@ Q_LOGGING_CATEGORY(inputplugins, "hifi.inputplugins")
const char* KinectPlugin::NAME = "Kinect"; const char* KinectPlugin::NAME = "Kinect";
const char* KinectPlugin::KINECT_ID_STRING = "Kinect"; const char* KinectPlugin::KINECT_ID_STRING = "Kinect";
QStringList jointNames = { QStringList kinectJointNames = {
"SpineBase", "SpineBase",
"SpineMid", "SpineMid",
"Neck", "Neck",
@ -198,71 +198,75 @@ static controller::StandardPoseChannel KinectJointIndexToPoseIndex(KinectJointIn
} }
} }
static const char* controllerJointName(controller::StandardPoseChannel i) { QStringList controllerJointNames = {
switch (i) { "Hips",
case controller::HIPS: return "Hips"; "RightUpLeg",
case controller::RIGHT_UP_LEG: return "RightUpLeg"; "RightLeg",
case controller::RIGHT_LEG: return "RightLeg"; "RightFoot",
case controller::RIGHT_FOOT: return "RightFoot"; "LeftUpLeg",
case controller::LEFT_UP_LEG: return "LeftUpLeg"; "LeftLeg",
case controller::LEFT_LEG: return "LeftLeg"; "LeftFoot",
case controller::LEFT_FOOT: return "LeftFoot"; "Spine",
case controller::SPINE: return "Spine"; "Spine1",
case controller::SPINE1: return "Spine1"; "Spine2",
case controller::SPINE2: return "Spine2"; "Spine3",
case controller::SPINE3: return "Spine3"; "Neck",
case controller::NECK: return "Neck"; "Head",
case controller::HEAD: return "Head"; "RightShoulder",
case controller::RIGHT_SHOULDER: return "RightShoulder"; "RightArm",
case controller::RIGHT_ARM: return "RightArm"; "RightForeArm",
case controller::RIGHT_FORE_ARM: return "RightForeArm"; "RightHand",
case controller::RIGHT_HAND: return "RightHand"; "RightHandThumb1",
case controller::RIGHT_HAND_THUMB1: return "RightHandThumb1"; "RightHandThumb2",
case controller::RIGHT_HAND_THUMB2: return "RightHandThumb2"; "RightHandThumb3",
case controller::RIGHT_HAND_THUMB3: return "RightHandThumb3"; "RightHandThumb4",
case controller::RIGHT_HAND_THUMB4: return "RightHandThumb4"; "RightHandIndex1",
case controller::RIGHT_HAND_INDEX1: return "RightHandIndex1"; "RightHandIndex2",
case controller::RIGHT_HAND_INDEX2: return "RightHandIndex2"; "RightHandIndex3",
case controller::RIGHT_HAND_INDEX3: return "RightHandIndex3"; "RightHandIndex4",
case controller::RIGHT_HAND_INDEX4: return "RightHandIndex4"; "RightHandMiddle1",
case controller::RIGHT_HAND_MIDDLE1: return "RightHandMiddle1"; "RightHandMiddle2",
case controller::RIGHT_HAND_MIDDLE2: return "RightHandMiddle2"; "RightHandMiddle3",
case controller::RIGHT_HAND_MIDDLE3: return "RightHandMiddle3"; "RightHandMiddle4",
case controller::RIGHT_HAND_MIDDLE4: return "RightHandMiddle4"; "RightHandRing1",
case controller::RIGHT_HAND_RING1: return "RightHandRing1"; "RightHandRing2",
case controller::RIGHT_HAND_RING2: return "RightHandRing2"; "RightHandRing3",
case controller::RIGHT_HAND_RING3: return "RightHandRing3"; "RightHandRing4",
case controller::RIGHT_HAND_RING4: return "RightHandRing4"; "RightHandPinky1",
case controller::RIGHT_HAND_PINKY1: return "RightHandPinky1"; "RightHandPinky2",
case controller::RIGHT_HAND_PINKY2: return "RightHandPinky2"; "RightHandPinky3",
case controller::RIGHT_HAND_PINKY3: return "RightHandPinky3"; "RightHandPinky4",
case controller::RIGHT_HAND_PINKY4: return "RightHandPinky4"; "LeftShoulder",
case controller::LEFT_SHOULDER: return "LeftShoulder"; "LeftArm",
case controller::LEFT_ARM: return "LeftArm"; "LeftForeArm",
case controller::LEFT_FORE_ARM: return "LeftForeArm"; "LeftHand",
case controller::LEFT_HAND: return "LeftHand"; "LeftHandThumb1",
case controller::LEFT_HAND_THUMB1: return "LeftHandThumb1"; "LeftHandThumb2",
case controller::LEFT_HAND_THUMB2: return "LeftHandThumb2"; "LeftHandThumb3",
case controller::LEFT_HAND_THUMB3: return "LeftHandThumb3"; "LeftHandThumb4",
case controller::LEFT_HAND_THUMB4: return "LeftHandThumb4"; "LeftHandIndex1",
case controller::LEFT_HAND_INDEX1: return "LeftHandIndex1"; "LeftHandIndex2",
case controller::LEFT_HAND_INDEX2: return "LeftHandIndex2"; "LeftHandIndex3",
case controller::LEFT_HAND_INDEX3: return "LeftHandIndex3"; "LeftHandIndex4",
case controller::LEFT_HAND_INDEX4: return "LeftHandIndex4"; "LeftHandMiddle1",
case controller::LEFT_HAND_MIDDLE1: return "LeftHandMiddle1"; "LeftHandMiddle2",
case controller::LEFT_HAND_MIDDLE2: return "LeftHandMiddle2"; "LeftHandMiddle3",
case controller::LEFT_HAND_MIDDLE3: return "LeftHandMiddle3"; "LeftHandMiddle4",
case controller::LEFT_HAND_MIDDLE4: return "LeftHandMiddle4"; "LeftHandRing1",
case controller::LEFT_HAND_RING1: return "LeftHandRing1"; "LeftHandRing2",
case controller::LEFT_HAND_RING2: return "LeftHandRing2"; "LeftHandRing3",
case controller::LEFT_HAND_RING3: return "LeftHandRing3"; "LeftHandRing4",
case controller::LEFT_HAND_RING4: return "LeftHandRing4"; "LeftHandPinky1",
case controller::LEFT_HAND_PINKY1: return "LeftHandPinky1"; "LeftHandPinky2",
case controller::LEFT_HAND_PINKY2: return "LeftHandPinky2"; "LeftHandPinky3",
case controller::LEFT_HAND_PINKY3: return "LeftHandPinky3"; "LeftHandPinky4"
case controller::LEFT_HAND_PINKY4: return "LeftHandPinky4"; };
default: return "???";
static const char* getControllerJointName(controller::StandardPoseChannel i) {
if (i >= 0 && i < controller::NUM_STANDARD_POSES) {
return qPrintable(controllerJointNames[i]);
} }
return "unknown";
} }
// convert between YXZ Kinect euler angles in degrees to quaternion // convert between YXZ Kinect euler angles in degrees to quaternion
@ -356,26 +360,6 @@ bool KinectPlugin::initializeDefaultSensor() {
#endif #endif
} }
/// <summary>
/// Converts a body point to screen space
/// </summary>
/// <param name="bodyPoint">body point to tranform</param>
/// <param name="width">width (in pixels) of output buffer</param>
/// <param name="height">height (in pixels) of output buffer</param>
/// <returns>point in screen-space</returns>
#if 0
D2D1_POINT_2F KinectPlugin::BodyToScreen(const CameraSpacePoint& bodyPoint, int width, int height) {
// Calculate the body's position on the screen
DepthSpacePoint depthPoint = { 0 };
_coordinateMapper->MapCameraPointToDepthSpace(bodyPoint, &depthPoint);
float screenPointX = static_cast<float>(depthPoint.X * width) / cDepthWidth;
float screenPointY = static_cast<float>(depthPoint.Y * height) / cDepthHeight;
return D2D1::Point2F(screenPointX, screenPointY);
}
#endif
void KinectPlugin::updateBody() { void KinectPlugin::updateBody() {
#ifndef HAVE_KINECT #ifndef HAVE_KINECT
return; return;
@ -389,15 +373,9 @@ void KinectPlugin::updateBody() {
HRESULT hr = _bodyFrameReader->AcquireLatestFrame(&pBodyFrame); HRESULT hr = _bodyFrameReader->AcquireLatestFrame(&pBodyFrame);
if (SUCCEEDED(hr)) { if (SUCCEEDED(hr)) {
//qDebug() << __FUNCTION__ << "line:" << __LINE__;
INT64 nTime = 0; INT64 nTime = 0;
hr = pBodyFrame->get_RelativeTime(&nTime); hr = pBodyFrame->get_RelativeTime(&nTime);
IBody* ppBodies[BODY_COUNT] = {0}; IBody* ppBodies[BODY_COUNT] = {0};
if (SUCCEEDED(hr)) { if (SUCCEEDED(hr)) {
hr = pBodyFrame->GetAndRefreshBodyData(_countof(ppBodies), ppBodies); hr = pBodyFrame->GetAndRefreshBodyData(_countof(ppBodies), ppBodies);
} }
@ -441,7 +419,6 @@ void KinectPlugin::ProcessBody(INT64 nTime, int nBodyCount, IBody** ppBodies) {
_joints.resize(JointType_Count, { { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f } }); _joints.resize(JointType_Count, { { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f } });
} }
Joint joints[JointType_Count]; Joint joints[JointType_Count];
JointOrientation jointOrientations[JointType_Count]; JointOrientation jointOrientations[JointType_Count];
HandState leftHandState = HandState_Unknown; HandState leftHandState = HandState_Unknown;
@ -456,38 +433,17 @@ void KinectPlugin::ProcessBody(INT64 nTime, int nBodyCount, IBody** ppBodies) {
if (SUCCEEDED(hr)) { if (SUCCEEDED(hr)) {
auto jointCount = _countof(joints); auto jointCount = _countof(joints);
//qDebug() << __FUNCTION__ << "nBodyCount:" << nBodyCount << "body:" << i << "jointCount:" << jointCount; //qDebug() << __FUNCTION__ << "nBodyCount:" << nBodyCount << "body:" << i << "jointCount:" << jointCount;
QString state;
for (int j = 0; j < jointCount; ++j) { for (int j = 0; j < jointCount; ++j) {
QString jointName = jointNames[joints[j].JointType]; //QString jointName = kinectJointNames[joints[j].JointType];
switch(joints[j].TrackingState) {
case TrackingState_NotTracked:
state = "Not Tracked";
break;
case TrackingState_Inferred:
state = "Inferred";
break;
case TrackingState_Tracked:
state = "Tracked";
break;
}
glm::vec3 jointPosition { joints[j].Position.X, glm::vec3 jointPosition { joints[j].Position.X,
joints[j].Position.Y, joints[j].Position.Y,
joints[j].Position.Z }; joints[j].Position.Z };
// filling in the _joints data...
if (joints[j].TrackingState != TrackingState_NotTracked) { if (joints[j].TrackingState != TrackingState_NotTracked) {
_joints[j].pos = jointPosition; _joints[j].pos = jointPosition;
if (joints[j].JointType == JointType_HandLeft ||
joints[j].JointType == JointType_HandRight) {
//qDebug() << __FUNCTION__ << jointName << ":" << state << "pos:" << jointPosition;
} }
}
// FIXME - do something interesting with the joints...
//jointPoints[j] = BodyToScreen(joints[j].Position, width, height);
} }
} }
} }
@ -516,8 +472,7 @@ void KinectPlugin::deactivate() {
SafeRelease(_coordinateMapper); SafeRelease(_coordinateMapper);
// close the Kinect Sensor // close the Kinect Sensor
if (_kinectSensor) if (_kinectSensor) {
{
_kinectSensor->Close(); _kinectSensor->Close();
} }
@ -532,8 +487,6 @@ void KinectPlugin::pluginUpdate(float deltaTime, const controller::InputCalibrat
return; return;
} }
//qDebug() << __FUNCTION__ << "deltaTime:" << deltaTime;
updateBody(); // updates _joints updateBody(); // updates _joints
std::vector<KinectJoint> joints = _joints; std::vector<KinectJoint> joints = _joints;
@ -554,7 +507,6 @@ void KinectPlugin::saveSettings() const {
settings.setValue(QString("enabled"), _enabled); settings.setValue(QString("enabled"), _enabled);
} }
settings.endGroup(); settings.endGroup();
qDebug() << __FUNCTION__ << "_enabled:" << _enabled;
} }
void KinectPlugin::loadSettings() { void KinectPlugin::loadSettings() {
@ -566,7 +518,6 @@ void KinectPlugin::loadSettings() {
_enabled = settings.value("enabled", QVariant(DEFAULT_ENABLED)).toBool(); _enabled = settings.value("enabled", QVariant(DEFAULT_ENABLED)).toBool();
} }
settings.endGroup(); settings.endGroup();
qDebug() << __FUNCTION__ << "_enabled:" << _enabled;
} }
// //
@ -577,9 +528,9 @@ void KinectPlugin::loadSettings() {
controller::Input::NamedVector KinectPlugin::InputDevice::getAvailableInputs() const { controller::Input::NamedVector KinectPlugin::InputDevice::getAvailableInputs() const {
static controller::Input::NamedVector availableInputs; static controller::Input::NamedVector availableInputs;
if (availableInputs.size() == 0) { if (availableInputs.size() == 0) {
for (int i = 0; i < controller::NUM_STANDARD_POSES; i++) { for (int i = 0; i < KinectJointIndex::Size; i++) {
auto channel = static_cast<controller::StandardPoseChannel>(i); auto channel = KinectJointIndexToPoseIndex(static_cast<KinectJointIndex>(i));
availableInputs.push_back(makePair(channel, controllerJointName(channel))); availableInputs.push_back(makePair(channel, getControllerJointName(channel)));
} }
}; };
return availableInputs; return availableInputs;
@ -596,6 +547,8 @@ void KinectPlugin::InputDevice::update(float deltaTime, const controller::InputC
int poseIndex = KinectJointIndexToPoseIndex((KinectJointIndex)i); int poseIndex = KinectJointIndexToPoseIndex((KinectJointIndex)i);
glm::vec3 linearVel, angularVel; glm::vec3 linearVel, angularVel;
const glm::vec3& pos = joints[i].pos; const glm::vec3& pos = joints[i].pos;
// FIXME - left over from neuron, needs to be turned into orientations from kinect SDK
const glm::vec3& rotEuler = joints[i].euler; const glm::vec3& rotEuler = joints[i].euler;
if (Vectors::ZERO == pos && Vectors::ZERO == rotEuler) { if (Vectors::ZERO == pos && Vectors::ZERO == rotEuler) {
@ -604,6 +557,7 @@ void KinectPlugin::InputDevice::update(float deltaTime, const controller::InputC
} }
// FIXME - left over from neuron, needs to be turned into orientations from kinect SDK
glm::quat rot = eulerToQuat(rotEuler); glm::quat rot = eulerToQuat(rotEuler);
if (i < prevJoints.size()) { if (i < prevJoints.size()) {
linearVel = (pos - (prevJoints[i].pos * METERS_PER_CENTIMETER)) / deltaTime; // m/s linearVel = (pos - (prevJoints[i].pos * METERS_PER_CENTIMETER)) / deltaTime; // m/s
@ -612,14 +566,9 @@ void KinectPlugin::InputDevice::update(float deltaTime, const controller::InputC
angularVel = glm::vec3(d.x, d.y, d.z) / (0.5f * deltaTime); // radians/s angularVel = glm::vec3(d.x, d.y, d.z) / (0.5f * deltaTime); // radians/s
} }
_poseStateMap[poseIndex] = controller::Pose(pos, rot, linearVel, angularVel); _poseStateMap[poseIndex] = controller::Pose(pos, rot, linearVel, angularVel);
if (poseIndex == controller::LEFT_HAND ||
poseIndex == controller::RIGHT_HAND) {
//qDebug() << __FUNCTION__ << "_poseStateMap[]=" << _poseStateMap[poseIndex].translation;
}
} }
// FIXME - left over from neuron
_poseStateMap[controller::RIGHT_HAND_THUMB1] = controller::Pose(rightHandThumb1DefaultAbsTranslation, glm::quat(), glm::vec3(), glm::vec3()); _poseStateMap[controller::RIGHT_HAND_THUMB1] = controller::Pose(rightHandThumb1DefaultAbsTranslation, glm::quat(), glm::vec3(), glm::vec3());
_poseStateMap[controller::LEFT_HAND_THUMB1] = controller::Pose(leftHandThumb1DefaultAbsTranslation, glm::quat(), glm::vec3(), glm::vec3()); _poseStateMap[controller::LEFT_HAND_THUMB1] = controller::Pose(leftHandThumb1DefaultAbsTranslation, glm::quat(), glm::vec3(), glm::vec3());
} }