From 2d921924bfc039eab5130bfa21a0ceff5c788620 Mon Sep 17 00:00:00 2001 From: Zu Date: Fri, 25 Jul 2014 16:52:29 +0800 Subject: [PATCH 1/6] added: Cara Face Tracker added: decode cara udp packets added: head rotation added: hook into hifi interface --- interface/src/Application.cpp | 18 +- interface/src/Application.h | 4 + interface/src/devices/CaraFaceTracker.cpp | 467 ++++++++++++++++++++++ interface/src/devices/CaraFaceTracker.h | 121 ++++++ 4 files changed, 608 insertions(+), 2 deletions(-) create mode 100644 interface/src/devices/CaraFaceTracker.cpp create mode 100644 interface/src/devices/CaraFaceTracker.h diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 6aa6fcd19e..a3d61a4d1e 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -1494,9 +1494,10 @@ glm::vec3 Application::getMouseVoxelWorldCoordinates(const VoxelDetail& mouseVox } FaceTracker* Application::getActiveFaceTracker() { - return _faceshift.isActive() ? static_cast(&_faceshift) : + return _cara.isActive() ? static_cast(&_cara) : + (_faceshift.isActive() ? static_cast(&_faceshift) : (_faceplus.isActive() ? static_cast(&_faceplus) : - (_visage.isActive() ? static_cast(&_visage) : NULL)); + (_visage.isActive() ? static_cast(&_visage) : NULL))); } struct SendVoxelsOperationArgs { @@ -1878,6 +1879,19 @@ void Application::updateVisage() { _visage.update(); } +void Application::updateCara() { + bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings); + PerformanceWarning warn(showWarnings, "Application::updateCara()"); + + // Update Cara + _cara.update(); + + // Copy angular velocity if measured by cara, to the head + if (_cara.isActive()) { + _myAvatar->getHead()->setAngularVelocity(_cara.getHeadAngularVelocity()); + } +} + void Application::updateMyAvatarLookAtPosition() { PerformanceTimer perfTimer("lookAt"); bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings); diff --git a/interface/src/Application.h b/interface/src/Application.h index e20e4e90d3..a356b26725 100644 --- a/interface/src/Application.h +++ b/interface/src/Application.h @@ -63,6 +63,7 @@ #include "devices/PrioVR.h" #include "devices/SixenseManager.h" #include "devices/Visage.h" +#include "devices/CaraFaceTracker.h" #include "models/ModelTreeRenderer.h" #include "particles/ParticleTreeRenderer.h" #include "renderer/AmbientOcclusionEffect.h" @@ -211,6 +212,7 @@ public: Faceplus* getFaceplus() { return &_faceplus; } Faceshift* getFaceshift() { return &_faceshift; } Visage* getVisage() { return &_visage; } + CaraFaceTracker* getCara() { return &_cara; } FaceTracker* getActiveFaceTracker(); SixenseManager* getSixenseManager() { return &_sixenseManager; } PrioVR* getPrioVR() { return &_prioVR; } @@ -382,6 +384,7 @@ private: void updateFaceplus(); void updateFaceshift(); void updateVisage(); + void updateCara(); void updateMyAvatarLookAtPosition(); void updateThreads(float deltaTime); void updateMetavoxels(float deltaTime); @@ -478,6 +481,7 @@ private: Faceplus _faceplus; Faceshift _faceshift; Visage _visage; + CaraFaceTracker _cara; SixenseManager _sixenseManager; PrioVR _prioVR; diff --git a/interface/src/devices/CaraFaceTracker.cpp b/interface/src/devices/CaraFaceTracker.cpp new file mode 100644 index 0000000000..b2698f2018 --- /dev/null +++ b/interface/src/devices/CaraFaceTracker.cpp @@ -0,0 +1,467 @@ +// +// CaraManager.cpp +// interface/src/devices +// +// Created by Li Zuwei on 7/22/14. +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "CaraFaceTracker.h" +#include + +//qt +#include +#include +#include +#include + +#define PI M_PI +#define RADTODEG(x) ( (x) * 180.0 / PI ) +#define DEGTORAD(x) ( (x) * PI / 180.0 ) + +static const QHostAddress CARA_FEATURE_POINT_SERVER_ADDR("127.0.0.1"); +static const quint16 CARA_FEATURE_POINT_SERVER_PORT = 36555; +static QString sampleJson = "[{\"id\":1,\"face\":{\"x\":248,\"y\":64,\"width\":278,\"height\":341},\"pose\":{\"roll\":2.62934,\"pitch\":-12.2318,\"yaw\":0.936743},\"feature_points\":[314,194,326,187,340,187,354,189,367,193,409,190,421,187,435,184,448,183,459,188,388,207,389,223,390,240,391,257,377,266,384,267,392,268,399,266,407,264,331,209,341,204,354,204,364,209,353,214,341,214,410,208,420,201,433,200,443,205,434,211,421,211,362,294,372,290,383,287,393,289,404,286,415,289,426,291,418,300,407,306,394,308,382,307,371,302,383,295,394,295,404,294,404,295,393,297,383,296],\"classifiers\":{\"emotion\":{\"smi\":-0.368829,\"sur\":-1.33334,\"neg\":0.00235828,\"att\":1},\"blink\":1}}]"; + +static const glm::vec3 DEFAULT_HEAD_ORIGIN(0.0f, 0.0f, 0.0f); +static const float TRANSLATION_SCALE = 1.0f; + +struct CaraPerson +{ + struct CaraPose + { + float roll, pitch, yaw; + CaraPose(): roll(0.0f), pitch(0.0f), yaw(0.0f) {} + }; + + struct CaraEmotion + { + float smile, surprise, negative, attention; + CaraEmotion(): smile(0.0f), surprise(0.0f), negative(0.0f), attention(0.0f) {} + }; + + enum CaraBlink + { + BLINK_NOT_AVAILABLE, + NO_BLINK, + BLINK + }; + + CaraPerson(): + id(-1), + blink(BLINK_NOT_AVAILABLE) + { + + } + + int id; + CaraPose pose; + CaraEmotion emotion; + CaraBlink blink; + + QString toString() + { + QString s = QString("id: %1, roll: %2, pitch: %3, yaw: %4, smi: %5, sur: %6, neg: %7, att: %8, blink: %9"). + arg(id). + arg(pose.roll). + arg(pose.pitch). + arg(pose.yaw). + arg(emotion.smile). + arg(emotion.surprise). + arg(emotion.negative). + arg(emotion.attention). + arg(blink); + + return s; + } +}; + +class CaraPacketDecoder +{ +public: + static CaraPerson extractOne(const QByteArray& buffer, QJsonParseError* jsonError) + { + CaraPerson person; + QJsonDocument dom = QJsonDocument::fromJson(buffer, jsonError); + + //check for errors + if(jsonError->error == QJsonParseError::NoError) + { + //read the dom structure and populate the blend shapes and head poses + qDebug() << "[Info] Cara Face Tracker Packet Parsing Successful!"; + + //begin extracting the packet + if(dom.isArray()) + { + QJsonArray people = dom.array(); + if(people.size() > 0) //extract the first person in the array + { + QJsonValue val = people.at(0); + if(val.isObject()) + { + QJsonObject personDOM = val.toObject(); + person.id = extractId(personDOM); + person.pose = extractPose(personDOM); + + //extract the classifier outputs + QJsonObject::const_iterator it = personDOM.constFind("classifiers"); + if(it != personDOM.constEnd()) + { + QJsonObject classifierDOM = (*it).toObject(); + person.emotion = extractEmotion(classifierDOM); + person.blink = extractBlink(classifierDOM); + } + } + } + } + } + + return person; + } + +private: + static int extractId(const QJsonObject& person) + { + int id = -1; + QJsonObject::const_iterator it = person.constFind("id"); + if(it != person.constEnd()) + id = (*it).toInt(-1); + return id; + } + + static CaraPerson::CaraPose extractPose(const QJsonObject& person) + { + CaraPerson::CaraPose pose; + QJsonObject::const_iterator it = person.constFind("pose"); + if(it != person.constEnd()) + { + QJsonObject poseDOM = (*it).toObject(); + + //look for the roll, pitch, yaw; + QJsonObject::const_iterator poseIt = poseDOM.constFind("roll"); + QJsonObject::const_iterator poseEnd = poseDOM.constEnd(); + if(poseIt != poseEnd) + pose.roll = (float)(*poseIt).toDouble(0.0); + + poseIt = poseDOM.constFind("pitch"); + if(poseIt != poseEnd) + pose.pitch = (float)(*poseIt).toDouble(0.0); + + poseIt = poseDOM.constFind("yaw"); + if(poseIt != poseEnd) + pose.yaw = (float)(*poseIt).toDouble(0.0); + } + return pose; + } + + static CaraPerson::CaraEmotion extractEmotion(const QJsonObject& classifiers) + { + CaraPerson::CaraEmotion emotion; + QJsonObject::const_iterator it = classifiers.constFind("emotion"); + if(it != classifiers.constEnd()) + { + QJsonObject emotionDOM = (*it).toObject(); + + //look for smile, surprise, negative, attention responses + QJsonObject::const_iterator emoEnd = emotionDOM.constEnd(); + QJsonObject::const_iterator emoIt = emotionDOM.constFind("smi"); + if(emoIt != emoEnd) + emotion.smile = (float)(*emoIt).toDouble(0.0); + + emoIt = emotionDOM.constFind("sur"); + if(emoIt != emoEnd) + emotion.surprise = (float)(*emoIt).toDouble(0.0); + + emoIt = emotionDOM.constFind("neg"); + if(emoIt != emoEnd) + emotion.negative = (float)(*emoIt).toDouble(0.0); + + emoIt = emotionDOM.constFind("att"); + if(emoIt != emoEnd) + emotion.attention = (float)(*emoIt).toDouble(0.0); + } + return emotion; + } + + static CaraPerson::CaraBlink extractBlink(const QJsonObject& classifiers) + { + CaraPerson::CaraBlink blink = CaraPerson::BLINK_NOT_AVAILABLE; + QJsonObject::const_iterator it = classifiers.constFind("blink"); + if(it != classifiers.constEnd()) + { + int b = (*it).toInt(CaraPerson::BLINK_NOT_AVAILABLE); + switch(b) + { + case CaraPerson::BLINK_NOT_AVAILABLE: + blink = CaraPerson::BLINK_NOT_AVAILABLE; + break; + case CaraPerson::NO_BLINK: + blink = CaraPerson::NO_BLINK; + break; + case CaraPerson::BLINK: + blink = CaraPerson::BLINK; + break; + default: + blink = CaraPerson::BLINK_NOT_AVAILABLE; + break; + } + } + return blink; + } +}; + +CaraFaceTracker::CaraFaceTracker() : + _lastReceiveTimestamp(0), + _previousPitch(0.0f), + _previousYaw(0.0f), + _previousRoll(0.0f), + _eyeGazeLeftPitch(0), + _eyeGazeLeftYaw(0), + _eyeGazeRightPitch(0), + _eyeGazeRightYaw(0), + _leftBlinkIndex(0), + _rightBlinkIndex(1), + _leftEyeOpenIndex(2), + _rightEyeOpenIndex(3), + _browDownLeftIndex(4), + _browDownRightIndex(5), + _browUpCenterIndex(6), + _browUpLeftIndex(7), + _browUpRightIndex(8), + _mouthSmileLeftIndex(9), + _mouthSmileRightIndex(10), + _jawOpenIndex(11) +{ + connect(&_udpSocket, SIGNAL(readyRead()), SLOT(readPendingDatagrams())); + connect(&_udpSocket, SIGNAL(error(QAbstractSocket::SocketError)), SLOT(socketErrorOccurred(QAbstractSocket::SocketError))); + connect(&_udpSocket, SIGNAL(stateChanged(QAbstractSocket::SocketState)), SLOT(socketStateChanged(QAbstractSocket::SocketState))); + + bindTo(CARA_FEATURE_POINT_SERVER_PORT); + + _headTranslation = DEFAULT_HEAD_ORIGIN; + _blendshapeCoefficients.resize(12); + _blendshapeCoefficients.fill(0.0f); + + qDebug() << sampleJson; +} + +CaraFaceTracker::CaraFaceTracker(const QHostAddress& host, quint16 port) : + _lastReceiveTimestamp(0), + _previousPitch(0.0f), + _previousYaw(0.0f), + _previousRoll(0.0f), + _eyeGazeLeftPitch(0), + _eyeGazeLeftYaw(0), + _eyeGazeRightPitch(0), + _eyeGazeRightYaw(0), + _leftBlinkIndex(0), + _rightBlinkIndex(1), + _leftEyeOpenIndex(2), + _rightEyeOpenIndex(3), + _browDownLeftIndex(4), + _browDownRightIndex(5), + _browUpCenterIndex(6), + _browUpLeftIndex(7), + _browUpRightIndex(8), + _mouthSmileLeftIndex(9), + _mouthSmileRightIndex(10), + _jawOpenIndex(11) +{ + connect(&_udpSocket, SIGNAL(readyRead()), SLOT(readPendingDatagrams())); + connect(&_udpSocket, SIGNAL(error(QAbstractSocket::SocketError)), SLOT(socketErrorOccurred(QAbstractSocket::SocketError))); + connect(&_udpSocket, SIGNAL(stateChanged(QAbstractSocket::SocketState)), SIGNAL(socketStateChanged(QAbstractSocket::SocketState))); + + bindTo(host, port); + + _headTranslation = DEFAULT_HEAD_ORIGIN * TRANSLATION_SCALE; + _blendshapeCoefficients.resize(12); //set the size of the blendshape coefficients + _blendshapeCoefficients.fill(0.0f); +} + +CaraFaceTracker::~CaraFaceTracker() +{ + if(_udpSocket.isOpen()) + _udpSocket.close(); +} + +void CaraFaceTracker::init() +{ + +} + +void CaraFaceTracker::reset() +{ + +} + +void CaraFaceTracker::bindTo(quint16 port) +{ + bindTo(QHostAddress::Any, port); +} + +void CaraFaceTracker::bindTo(const QHostAddress& host, quint16 port) +{ + if(_udpSocket.isOpen()) + _udpSocket.close(); + _udpSocket.bind(host, port); +} + +bool CaraFaceTracker::isActive() const +{ + static const int ACTIVE_TIMEOUT_USECS = 10000000; //10 secs + return (usecTimestampNow() - _lastReceiveTimestamp < ACTIVE_TIMEOUT_USECS); +} + +void CaraFaceTracker::update() +{ + // get the euler angles relative to the window + glm::vec3 eulers = glm::degrees(safeEulerAngles(_headRotation * glm::quat(glm::radians(glm::vec3( + (_eyeGazeLeftPitch + _eyeGazeRightPitch) / 2.0f, (_eyeGazeLeftYaw + _eyeGazeRightYaw) / 2.0f, 0.0f))))); + + //TODO: integrate when cara has eye gaze estimation + + _estimatedEyePitch = eulers.x; + _estimatedEyeYaw = eulers.y; +} + +//private slots and methods +void CaraFaceTracker::socketErrorOccurred(QAbstractSocket::SocketError socketError) +{ + qDebug() << "[Error] Cara Face Tracker Socket Error: " << _udpSocket.errorString(); +} + +void CaraFaceTracker::socketStateChanged(QAbstractSocket::SocketState socketState) +{ + QString state; + switch(socketState) + { + case QAbstractSocket::BoundState: state = "Bounded"; + break; + case QAbstractSocket::ClosingState: state = "Closing"; + break; + case QAbstractSocket::ConnectedState: state = "Connected"; + break; + case QAbstractSocket::ConnectingState: state = "Connecting"; + break; + case QAbstractSocket::HostLookupState: state = "Host Lookup"; + break; + case QAbstractSocket::ListeningState: state = "Listening"; + break; + case QAbstractSocket::UnconnectedState: state = "Unconnected"; + break; + } + qDebug() << "[Info] Cara Face Tracker Socket: " << socketState; +} + +void CaraFaceTracker::readPendingDatagrams() +{ + QByteArray buffer; + while (_udpSocket.hasPendingDatagrams()) { + buffer.resize(_udpSocket.pendingDatagramSize()); + _udpSocket.readDatagram(buffer.data(), buffer.size()); + decodePacket(buffer); + } +} + +void CaraFaceTracker::decodePacket(const QByteArray& buffer) +{ + /* + qDebug() << "---------- Received Message: "; + qDebug() < EPSILON) + { + float rMag = glm::length(glm::vec3(r.x, r.y, r.z)); + float AVERAGE_CARA_FRAME_TIME = 0.033f; + _headAngularVelocity = theta / AVERAGE_CARA_FRAME_TIME * glm::vec3(r.x, r.y, r.z) / rMag; + + _previousPitch = person.pose.pitch; + _previousYaw = person.pose.yaw; + _previousRoll = person.pose.roll; + + newRotation = glm::quat(glm::vec3(DEGTORAD(person.pose.pitch), DEGTORAD(person.pose.yaw), DEGTORAD(person.pose.roll))); + } + else + { + //no change in position + //qDebug() << "NO change in rotation"; + newRotation = glm::quat(glm::vec3(DEGTORAD(_previousPitch), DEGTORAD(_previousYaw), DEGTORAD(_previousRoll))); + _headAngularVelocity = glm::vec3(0,0,0); + } + + _headRotation = newRotation; + + //angular velocity of the head + //qDebug() << "pitch: " << _headAngularVelocity.x << " yaw: " << _headAngularVelocity.y << " roll: " <<_headAngularVelocity.z; + + //TODO: head translation, right now is 0 + + + //Do Blendshapes, clip between 0.0f to 1.0f, neg should be ignored + /* + //blend shapes + int _leftBlinkIndex; + int _rightBlinkIndex; + int _leftEyeOpenIndex; + int _rightEyeOpenIndex; + + // Brows + int _browDownLeftIndex; + int _browDownRightIndex; + int _browUpCenterIndex; + int _browUpLeftIndex; + int _browUpRightIndex; + int _mouthSmileLeftIndex; + int _mouthSmileRightIndex; + int _jawOpenIndex; + */ + + _blendshapeCoefficients[_leftBlinkIndex] = person.blink == CaraPerson::BLINK ? 1.0f : 0.0f; + _blendshapeCoefficients[_rightBlinkIndex] = person.blink == CaraPerson::BLINK ? 1.0f : 0.0f; + _blendshapeCoefficients[_browDownLeftIndex] = person.emotion.surprise < 0.0f ? 0.0f : person.emotion.surprise; + _blendshapeCoefficients[_browDownRightIndex] = person.emotion.surprise < 0.0f ? 0.0f : person.emotion.surprise; + _blendshapeCoefficients[_browUpCenterIndex] = person.emotion.surprise < 0.0f ? 0.0f : person.emotion.surprise; + _blendshapeCoefficients[_browUpLeftIndex] = person.emotion.surprise < 0.0f ? 0.0f : person.emotion.surprise; + _blendshapeCoefficients[_browUpRightIndex] = person.emotion.surprise < 0.0f ? 0.0f : person.emotion.surprise; + _blendshapeCoefficients[_jawOpenIndex] = person.emotion.surprise < 0.0f ? 0.0f : person.emotion.surprise; + _blendshapeCoefficients[_mouthSmileLeftIndex] = person.emotion.smile < 0.0f ? 0.0f : person.emotion.smile; + _blendshapeCoefficients[_mouthSmileRightIndex] = person.emotion.smile < 0.0f ? 0.0f : person.emotion.smile; + } + else + qDebug() << "[Error] Cara Face Tracker Decode Error: " << jsonError.errorString(); + + _lastReceiveTimestamp = usecTimestampNow(); +} + +float CaraFaceTracker::getBlendshapeCoefficient(int index) const +{ + return (index >= 0 && index < (int)_blendshapeCoefficients.size()) ? _blendshapeCoefficients[index] : 0.0f; +} + diff --git a/interface/src/devices/CaraFaceTracker.h b/interface/src/devices/CaraFaceTracker.h new file mode 100644 index 0000000000..4f777f959e --- /dev/null +++ b/interface/src/devices/CaraFaceTracker.h @@ -0,0 +1,121 @@ +// +// CaraManager.h +// interface/src/devices +// +// Created by Li Zuwei on 7/22/14. +// Copyright 2013 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hi_fi_Cara_h +#define hi_fi_Cara_h + +#include + +#include "FaceTracker.h" + +/*! + * \class CaraManager + * + * \brief Handles interaction with the Cara software, + * which provides head position/orientation and facial features. +**/ + +class CaraFaceTracker : public FaceTracker +{ + Q_OBJECT + +public: + CaraFaceTracker(); + CaraFaceTracker(const QHostAddress& host, quint16 port); + ~CaraFaceTracker(); + + //initialization + void init(); + void reset(); + + //sockets + void bindTo(quint16 port); + void bindTo(const QHostAddress& host, quint16 port); + bool isActive() const; + + //tracking + void update(); + + //head angular velocity + const glm::vec3& getHeadAngularVelocity() const { return _headAngularVelocity; } + + //eye gaze + float getEyeGazeLeftPitch() const { return _eyeGazeLeftPitch; } + float getEyeGazeLeftYaw() const { return _eyeGazeLeftYaw; } + + float getEyeGazeRightPitch() const { return _eyeGazeRightPitch; } + float getEyeGazeRightYaw() const { return _eyeGazeRightYaw; } + + //blend shapes + float getLeftBlink() const { return getBlendshapeCoefficient(_leftBlinkIndex); } + float getRightBlink() const { return getBlendshapeCoefficient(_rightBlinkIndex); } + float getLeftEyeOpen() const { return getBlendshapeCoefficient(_leftEyeOpenIndex); } + float getRightEyeOpen() const { return getBlendshapeCoefficient(_rightEyeOpenIndex); } + + float getBrowDownLeft() const { return getBlendshapeCoefficient(_browDownLeftIndex); } + float getBrowDownRight() const { return getBlendshapeCoefficient(_browDownRightIndex); } + float getBrowUpCenter() const { return getBlendshapeCoefficient(_browUpCenterIndex); } + float getBrowUpLeft() const { return getBlendshapeCoefficient(_browUpLeftIndex); } + float getBrowUpRight() const { return getBlendshapeCoefficient(_browUpRightIndex); } + + float getMouthSize() const { return getBlendshapeCoefficient(_jawOpenIndex); } + float getMouthSmileLeft() const { return getBlendshapeCoefficient(_mouthSmileLeftIndex); } + float getMouthSmileRight() const { return getBlendshapeCoefficient(_mouthSmileRightIndex); } + +private slots: + + //sockets + void socketErrorOccurred(QAbstractSocket::SocketError socketError); + void readPendingDatagrams(); + void socketStateChanged(QAbstractSocket::SocketState socketState); + +private: + void decodePacket(const QByteArray& buffer); + float getBlendshapeCoefficient(int index) const; + + // sockets + QUdpSocket _udpSocket; + quint64 _lastReceiveTimestamp; + + //head tracking + glm::vec3 _headAngularVelocity; + + //pose history + float _previousPitch; + float _previousYaw; + float _previousRoll; + + // eye gaze degrees + float _eyeGazeLeftPitch; + float _eyeGazeLeftYaw; + float _eyeGazeRightPitch; + float _eyeGazeRightYaw; + + //blend shapes + int _leftBlinkIndex; + int _rightBlinkIndex; + int _leftEyeOpenIndex; + int _rightEyeOpenIndex; + + // Brows + int _browDownLeftIndex; + int _browDownRightIndex; + int _browUpCenterIndex; + int _browUpLeftIndex; + int _browUpRightIndex; + + int _mouthSmileLeftIndex; + int _mouthSmileRightIndex; + + int _jawOpenIndex; +}; + +#endif //endif hi_fi_CaraManager_h \ No newline at end of file From 98bb995aba6ae441bc083303eed11b29e11f6c7d Mon Sep 17 00:00:00 2001 From: Zu Date: Fri, 25 Jul 2014 17:32:04 +0800 Subject: [PATCH 2/6] added: noise filtering to reduce rotation jitter --- interface/src/devices/CaraFaceTracker.cpp | 19 ++++++++++++++++++- interface/src/devices/CaraFaceTracker.h | 13 ++++++++----- 2 files changed, 26 insertions(+), 6 deletions(-) diff --git a/interface/src/devices/CaraFaceTracker.cpp b/interface/src/devices/CaraFaceTracker.cpp index b2698f2018..8420c06623 100644 --- a/interface/src/devices/CaraFaceTracker.cpp +++ b/interface/src/devices/CaraFaceTracker.cpp @@ -1,5 +1,5 @@ // -// CaraManager.cpp +// CaraFaceTracker.cpp // interface/src/devices // // Created by Li Zuwei on 7/22/14. @@ -376,6 +376,7 @@ void CaraFaceTracker::decodePacket(const QByteArray& buffer) QElapsedTimer timer; timer.start(); + //decode the incoming udp packet QJsonParseError jsonError; CaraPerson person = CaraPacketDecoder::extractOne(buffer, &jsonError); @@ -402,6 +403,22 @@ void CaraFaceTracker::decodePacket(const QByteArray& buffer) float AVERAGE_CARA_FRAME_TIME = 0.033f; _headAngularVelocity = theta / AVERAGE_CARA_FRAME_TIME * glm::vec3(r.x, r.y, r.z) / rMag; + if(glm::abs(_headAngularVelocity.x) < 1.0f) + { + person.pose.pitch = _previousPitch; + //qDebug() << "NO change in pitch"; + } + if(glm::abs(person.pose.yaw - _previousYaw) < 2.5f) + { + qDebug() << "Yaw Diff: " << glm::abs(person.pose.yaw - _previousYaw); + person.pose.yaw = _previousYaw; + } + if(glm::abs(_headAngularVelocity.z) < 1.0f) + { + //qDebug() << "NO change in roll"; + person.pose.roll = _previousRoll; + } + _previousPitch = person.pose.pitch; _previousYaw = person.pose.yaw; _previousRoll = person.pose.roll; diff --git a/interface/src/devices/CaraFaceTracker.h b/interface/src/devices/CaraFaceTracker.h index 4f777f959e..3a34d94b12 100644 --- a/interface/src/devices/CaraFaceTracker.h +++ b/interface/src/devices/CaraFaceTracker.h @@ -1,5 +1,5 @@ // -// CaraManager.h +// CaraFaceTracker.h // interface/src/devices // // Created by Li Zuwei on 7/22/14. @@ -9,18 +9,21 @@ // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // -#ifndef hi_fi_Cara_h -#define hi_fi_Cara_h +#ifndef hi_fi_CaraFaceTracker_h +#define hi_fi_CaraFaceTracker_h #include #include "FaceTracker.h" /*! - * \class CaraManager + * \class CaraFaceTracker * * \brief Handles interaction with the Cara software, * which provides head position/orientation and facial features. + * \details By default, opens a udp socket with IPV4_ANY_ADDR with port 36555. + * User needs to run the Cara Face Detection UDP Client with the destination + * host address (eg: 127.0.0.1 for localhost) and destination port 36555. **/ class CaraFaceTracker : public FaceTracker @@ -118,4 +121,4 @@ private: int _jawOpenIndex; }; -#endif //endif hi_fi_CaraManager_h \ No newline at end of file +#endif //endif hi_fi_CaraFaceTracker_h \ No newline at end of file From 1bc6d4bf0f122619ec7ab6376333b2952e5df438 Mon Sep 17 00:00:00 2001 From: Zu Date: Fri, 25 Jul 2014 18:05:17 +0800 Subject: [PATCH 3/6] fix: tuned the roll and pitch jitter, round to nearest 1 dp instead of whole number --- interface/src/devices/CaraFaceTracker.cpp | 36 +++++++++++++++-------- 1 file changed, 24 insertions(+), 12 deletions(-) diff --git a/interface/src/devices/CaraFaceTracker.cpp b/interface/src/devices/CaraFaceTracker.cpp index 8420c06623..d5c30d6369 100644 --- a/interface/src/devices/CaraFaceTracker.cpp +++ b/interface/src/devices/CaraFaceTracker.cpp @@ -386,9 +386,14 @@ void CaraFaceTracker::decodePacket(const QByteArray& buffer) //qDebug() << "Parsing took: " << timer.elapsed() << " msecs"; //do some noise filtering to the head poses - person.pose.roll = glm::round(person.pose.roll); - person.pose.pitch = glm::round(person.pose.pitch); - person.pose.yaw = glm::round(person.pose.yaw); + //person.pose.roll = glm::round(person.pose.roll); + //person.pose.pitch = glm::round(person.pose.pitch); + //person.pose.yaw = glm::round(person.pose.yaw); + + //reduce the noise first by truncating to 1 dp + person.pose.roll = glm::floor(person.pose.roll * 10) / 10; + person.pose.pitch = glm::floor(person.pose.pitch * 10) / 10; + person.pose.yaw = glm::floor(person.pose.yaw * 10) / 10; qDebug() << person.toString(); @@ -403,32 +408,39 @@ void CaraFaceTracker::decodePacket(const QByteArray& buffer) float AVERAGE_CARA_FRAME_TIME = 0.033f; _headAngularVelocity = theta / AVERAGE_CARA_FRAME_TIME * glm::vec3(r.x, r.y, r.z) / rMag; - if(glm::abs(_headAngularVelocity.x) < 1.0f) + //use the angular velocity for roll and pitch, if it's below the threshold don't move + if(glm::abs(_headAngularVelocity.x) < 1.2f) { person.pose.pitch = _previousPitch; //qDebug() << "NO change in pitch"; } - if(glm::abs(person.pose.yaw - _previousYaw) < 2.5f) - { - qDebug() << "Yaw Diff: " << glm::abs(person.pose.yaw - _previousYaw); - person.pose.yaw = _previousYaw; - } - if(glm::abs(_headAngularVelocity.z) < 1.0f) + if(glm::abs(_headAngularVelocity.z) < 1.2f) { //qDebug() << "NO change in roll"; person.pose.roll = _previousRoll; } + //for yaw, the jitter is great, you can't use angular velocity because it swings too much + //use the previous and current yaw, calculate the + //abs difference and move it the difference is above a certain angle. (this will introduce some + //jerks but will not encounter lag) + if(glm::abs(person.pose.yaw - _previousYaw) < 2.5f) // < than 2.5 deg, no move + { + qDebug() << "Yaw Diff: " << glm::abs(person.pose.yaw - _previousYaw); + person.pose.yaw = _previousYaw; + } + + //update the previous angles _previousPitch = person.pose.pitch; _previousYaw = person.pose.yaw; _previousRoll = person.pose.roll; - newRotation = glm::quat(glm::vec3(DEGTORAD(person.pose.pitch), DEGTORAD(person.pose.yaw), DEGTORAD(person.pose.roll))); + //set the new rotation + newRotation = glm::quat(glm::vec3(DEGTORAD(person.pose.pitch), DEGTORAD(person.pose.yaw), DEGTORAD(-person.pose.roll))); } else { //no change in position - //qDebug() << "NO change in rotation"; newRotation = glm::quat(glm::vec3(DEGTORAD(_previousPitch), DEGTORAD(_previousYaw), DEGTORAD(_previousRoll))); _headAngularVelocity = glm::vec3(0,0,0); } From cce962fb168790e7d8319f9a6111f1e3c4378bf6 Mon Sep 17 00:00:00 2001 From: Zu Date: Tue, 29 Jul 2014 11:07:04 +0800 Subject: [PATCH 4/6] added: smile, surprise, anger to blendshapes fix: roll, yaw direction added: source comments removed: debug print outs --- interface/src/devices/CaraFaceTracker.cpp | 133 ++++++++-------------- 1 file changed, 50 insertions(+), 83 deletions(-) diff --git a/interface/src/devices/CaraFaceTracker.cpp b/interface/src/devices/CaraFaceTracker.cpp index d5c30d6369..71ab94b06a 100644 --- a/interface/src/devices/CaraFaceTracker.cpp +++ b/interface/src/devices/CaraFaceTracker.cpp @@ -28,6 +28,7 @@ static QString sampleJson = "[{\"id\":1,\"face\":{\"x\":248,\"y\":64,\"width\":2 static const glm::vec3 DEFAULT_HEAD_ORIGIN(0.0f, 0.0f, 0.0f); static const float TRANSLATION_SCALE = 1.0f; +static const int NUM_BLENDSHAPE_COEFF = 30; struct CaraPerson { @@ -91,7 +92,7 @@ public: if(jsonError->error == QJsonParseError::NoError) { //read the dom structure and populate the blend shapes and head poses - qDebug() << "[Info] Cara Face Tracker Packet Parsing Successful!"; + //qDebug() << "[Info] Cara Face Tracker Packet Parsing Successful!"; //begin extracting the packet if(dom.isArray()) @@ -224,16 +225,16 @@ CaraFaceTracker::CaraFaceTracker() : _eyeGazeRightYaw(0), _leftBlinkIndex(0), _rightBlinkIndex(1), - _leftEyeOpenIndex(2), - _rightEyeOpenIndex(3), - _browDownLeftIndex(4), - _browDownRightIndex(5), - _browUpCenterIndex(6), - _browUpLeftIndex(7), - _browUpRightIndex(8), - _mouthSmileLeftIndex(9), - _mouthSmileRightIndex(10), - _jawOpenIndex(11) + _leftEyeOpenIndex(8), + _rightEyeOpenIndex(9), + _browDownLeftIndex(14), + _browDownRightIndex(15), + _browUpCenterIndex(16), + _browUpLeftIndex(17), + _browUpRightIndex(18), + _mouthSmileLeftIndex(28), + _mouthSmileRightIndex(29), + _jawOpenIndex(21) { connect(&_udpSocket, SIGNAL(readyRead()), SLOT(readPendingDatagrams())); connect(&_udpSocket, SIGNAL(error(QAbstractSocket::SocketError)), SLOT(socketErrorOccurred(QAbstractSocket::SocketError))); @@ -242,10 +243,10 @@ CaraFaceTracker::CaraFaceTracker() : bindTo(CARA_FEATURE_POINT_SERVER_PORT); _headTranslation = DEFAULT_HEAD_ORIGIN; - _blendshapeCoefficients.resize(12); + _blendshapeCoefficients.resize(NUM_BLENDSHAPE_COEFF); _blendshapeCoefficients.fill(0.0f); - qDebug() << sampleJson; + //qDebug() << sampleJson; } CaraFaceTracker::CaraFaceTracker(const QHostAddress& host, quint16 port) : @@ -259,16 +260,16 @@ CaraFaceTracker::CaraFaceTracker(const QHostAddress& host, quint16 port) : _eyeGazeRightYaw(0), _leftBlinkIndex(0), _rightBlinkIndex(1), - _leftEyeOpenIndex(2), - _rightEyeOpenIndex(3), - _browDownLeftIndex(4), - _browDownRightIndex(5), - _browUpCenterIndex(6), - _browUpLeftIndex(7), - _browUpRightIndex(8), - _mouthSmileLeftIndex(9), - _mouthSmileRightIndex(10), - _jawOpenIndex(11) + _leftEyeOpenIndex(8), + _rightEyeOpenIndex(9), + _browDownLeftIndex(14), + _browDownRightIndex(15), + _browUpCenterIndex(16), + _browUpLeftIndex(17), + _browUpRightIndex(18), + _mouthSmileLeftIndex(28), + _mouthSmileRightIndex(29), + _jawOpenIndex(21) { connect(&_udpSocket, SIGNAL(readyRead()), SLOT(readPendingDatagrams())); connect(&_udpSocket, SIGNAL(error(QAbstractSocket::SocketError)), SLOT(socketErrorOccurred(QAbstractSocket::SocketError))); @@ -277,7 +278,7 @@ CaraFaceTracker::CaraFaceTracker(const QHostAddress& host, quint16 port) : bindTo(host, port); _headTranslation = DEFAULT_HEAD_ORIGIN * TRANSLATION_SCALE; - _blendshapeCoefficients.resize(12); //set the size of the blendshape coefficients + _blendshapeCoefficients.resize(NUM_BLENDSHAPE_COEFF); //set the size of the blendshape coefficients _blendshapeCoefficients.fill(0.0f); } @@ -311,7 +312,7 @@ void CaraFaceTracker::bindTo(const QHostAddress& host, quint16 port) bool CaraFaceTracker::isActive() const { - static const int ACTIVE_TIMEOUT_USECS = 10000000; //10 secs + static const int ACTIVE_TIMEOUT_USECS = 3000000; //3 secs return (usecTimestampNow() - _lastReceiveTimestamp < ACTIVE_TIMEOUT_USECS); } @@ -367,37 +368,22 @@ void CaraFaceTracker::readPendingDatagrams() } void CaraFaceTracker::decodePacket(const QByteArray& buffer) -{ - /* - qDebug() << "---------- Received Message: "; - qDebug() < EPSILON) { float rMag = glm::length(glm::vec3(r.x, r.y, r.z)); - float AVERAGE_CARA_FRAME_TIME = 0.033f; + const float AVERAGE_CARA_FRAME_TIME = 0.033f; + const float ANGULAR_VELOCITY_MIN = 1.2f; + const float YAW_STANDARD_DEV_DEG = 2.5f; + _headAngularVelocity = theta / AVERAGE_CARA_FRAME_TIME * glm::vec3(r.x, r.y, r.z) / rMag; //use the angular velocity for roll and pitch, if it's below the threshold don't move - if(glm::abs(_headAngularVelocity.x) < 1.2f) - { - person.pose.pitch = _previousPitch; - //qDebug() << "NO change in pitch"; - } - if(glm::abs(_headAngularVelocity.z) < 1.2f) - { - //qDebug() << "NO change in roll"; + if(glm::abs(_headAngularVelocity.x) < ANGULAR_VELOCITY_MIN) + person.pose.pitch = _previousPitch; + + if(glm::abs(_headAngularVelocity.z) < ANGULAR_VELOCITY_MIN) person.pose.roll = _previousRoll; - } //for yaw, the jitter is great, you can't use angular velocity because it swings too much //use the previous and current yaw, calculate the - //abs difference and move it the difference is above a certain angle. (this will introduce some - //jerks but will not encounter lag) - if(glm::abs(person.pose.yaw - _previousYaw) < 2.5f) // < than 2.5 deg, no move + //abs difference and move it the difference is above the standard deviation which is around 2.5 + // (this will introduce some jerks but will not encounter lag) + if(glm::abs(person.pose.yaw - _previousYaw) < YAW_STANDARD_DEV_DEG) // < the standard deviation 2.5 deg, no move { - qDebug() << "Yaw Diff: " << glm::abs(person.pose.yaw - _previousYaw); + //qDebug() << "Yaw Diff: " << glm::abs(person.pose.yaw - _previousYaw); person.pose.yaw = _previousYaw; } @@ -441,41 +425,24 @@ void CaraFaceTracker::decodePacket(const QByteArray& buffer) else { //no change in position - newRotation = glm::quat(glm::vec3(DEGTORAD(_previousPitch), DEGTORAD(_previousYaw), DEGTORAD(_previousRoll))); + newRotation = glm::quat(glm::vec3(DEGTORAD(_previousPitch), DEGTORAD(_previousYaw), DEGTORAD(-_previousRoll))); _headAngularVelocity = glm::vec3(0,0,0); } - _headRotation = newRotation; - - //angular velocity of the head - //qDebug() << "pitch: " << _headAngularVelocity.x << " yaw: " << _headAngularVelocity.y << " roll: " <<_headAngularVelocity.z; + //update to new rotation angles + _headRotation = newRotation; //TODO: head translation, right now is 0 - //Do Blendshapes, clip between 0.0f to 1.0f, neg should be ignored - /* - //blend shapes - int _leftBlinkIndex; - int _rightBlinkIndex; - int _leftEyeOpenIndex; - int _rightEyeOpenIndex; - - // Brows - int _browDownLeftIndex; - int _browDownRightIndex; - int _browUpCenterIndex; - int _browUpLeftIndex; - int _browUpRightIndex; - int _mouthSmileLeftIndex; - int _mouthSmileRightIndex; - int _jawOpenIndex; - */ - + //Do Blendshapes, clip between 0.0f to 1.0f, neg should be ignored _blendshapeCoefficients[_leftBlinkIndex] = person.blink == CaraPerson::BLINK ? 1.0f : 0.0f; _blendshapeCoefficients[_rightBlinkIndex] = person.blink == CaraPerson::BLINK ? 1.0f : 0.0f; - _blendshapeCoefficients[_browDownLeftIndex] = person.emotion.surprise < 0.0f ? 0.0f : person.emotion.surprise; - _blendshapeCoefficients[_browDownRightIndex] = person.emotion.surprise < 0.0f ? 0.0f : person.emotion.surprise; + + //anger and surprised are mutually exclusive so we could try use this fact to determine + //whether to down the brows or up the brows + _blendshapeCoefficients[_browDownLeftIndex] = person.emotion.negative < 0.0f ? 0.0f : person.emotion.negative; + _blendshapeCoefficients[_browDownRightIndex] = person.emotion.negative < 0.0f ? 0.0f : person.emotion.negative; _blendshapeCoefficients[_browUpCenterIndex] = person.emotion.surprise < 0.0f ? 0.0f : person.emotion.surprise; _blendshapeCoefficients[_browUpLeftIndex] = person.emotion.surprise < 0.0f ? 0.0f : person.emotion.surprise; _blendshapeCoefficients[_browUpRightIndex] = person.emotion.surprise < 0.0f ? 0.0f : person.emotion.surprise; From 113a173577efa944480d208921fb6c5a54d7ff45 Mon Sep 17 00:00:00 2001 From: Zu Date: Wed, 30 Jul 2014 09:30:55 +0800 Subject: [PATCH 5/6] fix: update code style to conform to hifi convention --- interface/src/devices/CaraFaceTracker.cpp | 260 +++++++++++----------- interface/src/devices/CaraFaceTracker.h | 3 +- 2 files changed, 125 insertions(+), 138 deletions(-) diff --git a/interface/src/devices/CaraFaceTracker.cpp b/interface/src/devices/CaraFaceTracker.cpp index 71ab94b06a..0638ba5f26 100644 --- a/interface/src/devices/CaraFaceTracker.cpp +++ b/interface/src/devices/CaraFaceTracker.cpp @@ -30,32 +30,38 @@ static const glm::vec3 DEFAULT_HEAD_ORIGIN(0.0f, 0.0f, 0.0f); static const float TRANSLATION_SCALE = 1.0f; static const int NUM_BLENDSHAPE_COEFF = 30; -struct CaraPerson -{ - struct CaraPose - { +struct CaraPerson { + struct CaraPose { float roll, pitch, yaw; - CaraPose(): roll(0.0f), pitch(0.0f), yaw(0.0f) {} + CaraPose() : + roll(0.0f), + pitch(0.0f), + yaw(0.0f) + { + } }; - struct CaraEmotion - { + struct CaraEmotion { float smile, surprise, negative, attention; - CaraEmotion(): smile(0.0f), surprise(0.0f), negative(0.0f), attention(0.0f) {} + CaraEmotion(): + smile(0.0f), + surprise(0.0f), + negative(0.0f), + attention(0.0f) + { + } }; - enum CaraBlink - { + enum CaraBlink { BLINK_NOT_AVAILABLE, NO_BLINK, BLINK }; - CaraPerson(): + CaraPerson() : id(-1), - blink(BLINK_NOT_AVAILABLE) - { - + blink(BLINK_NOT_AVAILABLE) + { } int id; @@ -63,8 +69,7 @@ struct CaraPerson CaraEmotion emotion; CaraBlink blink; - QString toString() - { + QString toString() { QString s = QString("id: %1, roll: %2, pitch: %3, yaw: %4, smi: %5, sur: %6, neg: %7, att: %8, blink: %9"). arg(id). arg(pose.roll). @@ -75,42 +80,35 @@ struct CaraPerson arg(emotion.negative). arg(emotion.attention). arg(blink); - return s; } }; -class CaraPacketDecoder -{ +class CaraPacketDecoder { public: - static CaraPerson extractOne(const QByteArray& buffer, QJsonParseError* jsonError) - { + static CaraPerson extractOne(const QByteArray& buffer, QJsonParseError* jsonError) { CaraPerson person; QJsonDocument dom = QJsonDocument::fromJson(buffer, jsonError); //check for errors - if(jsonError->error == QJsonParseError::NoError) - { + if(jsonError->error == QJsonParseError::NoError) { //read the dom structure and populate the blend shapes and head poses //qDebug() << "[Info] Cara Face Tracker Packet Parsing Successful!"; //begin extracting the packet - if(dom.isArray()) - { - QJsonArray people = dom.array(); - if(people.size() > 0) //extract the first person in the array - { + if(dom.isArray()) { + QJsonArray people = dom.array(); + //extract the first person in the array + if(people.size() > 0) { QJsonValue val = people.at(0); - if(val.isObject()) - { + if(val.isObject()) { QJsonObject personDOM = val.toObject(); person.id = extractId(personDOM); person.pose = extractPose(personDOM); //extract the classifier outputs QJsonObject::const_iterator it = personDOM.constFind("classifiers"); - if(it != personDOM.constEnd()) - { + if(it != personDOM.constEnd()) { QJsonObject classifierDOM = (*it).toObject(); person.emotion = extractEmotion(classifierDOM); person.blink = extractBlink(classifierDOM); @@ -124,90 +122,85 @@ public: } private: - static int extractId(const QJsonObject& person) - { + static int extractId(const QJsonObject& person) { int id = -1; QJsonObject::const_iterator it = person.constFind("id"); - if(it != person.constEnd()) + if(it != person.constEnd()) { id = (*it).toInt(-1); + } return id; } - static CaraPerson::CaraPose extractPose(const QJsonObject& person) - { + static CaraPerson::CaraPose extractPose(const QJsonObject& person) { CaraPerson::CaraPose pose; QJsonObject::const_iterator it = person.constFind("pose"); - if(it != person.constEnd()) - { + if(it != person.constEnd()) { QJsonObject poseDOM = (*it).toObject(); //look for the roll, pitch, yaw; QJsonObject::const_iterator poseIt = poseDOM.constFind("roll"); QJsonObject::const_iterator poseEnd = poseDOM.constEnd(); - if(poseIt != poseEnd) + if(poseIt != poseEnd) { pose.roll = (float)(*poseIt).toDouble(0.0); - + } poseIt = poseDOM.constFind("pitch"); - if(poseIt != poseEnd) + if(poseIt != poseEnd) { pose.pitch = (float)(*poseIt).toDouble(0.0); - + } poseIt = poseDOM.constFind("yaw"); - if(poseIt != poseEnd) + if(poseIt != poseEnd) { pose.yaw = (float)(*poseIt).toDouble(0.0); + } } return pose; } - static CaraPerson::CaraEmotion extractEmotion(const QJsonObject& classifiers) - { + static CaraPerson::CaraEmotion extractEmotion(const QJsonObject& classifiers) { CaraPerson::CaraEmotion emotion; QJsonObject::const_iterator it = classifiers.constFind("emotion"); - if(it != classifiers.constEnd()) - { + if(it != classifiers.constEnd()) { QJsonObject emotionDOM = (*it).toObject(); //look for smile, surprise, negative, attention responses QJsonObject::const_iterator emoEnd = emotionDOM.constEnd(); QJsonObject::const_iterator emoIt = emotionDOM.constFind("smi"); - if(emoIt != emoEnd) + if(emoIt != emoEnd) { emotion.smile = (float)(*emoIt).toDouble(0.0); - + } emoIt = emotionDOM.constFind("sur"); - if(emoIt != emoEnd) + if(emoIt != emoEnd) { emotion.surprise = (float)(*emoIt).toDouble(0.0); - + } emoIt = emotionDOM.constFind("neg"); - if(emoIt != emoEnd) + if(emoIt != emoEnd) { emotion.negative = (float)(*emoIt).toDouble(0.0); - + } emoIt = emotionDOM.constFind("att"); - if(emoIt != emoEnd) + if(emoIt != emoEnd) { emotion.attention = (float)(*emoIt).toDouble(0.0); + } } return emotion; } - static CaraPerson::CaraBlink extractBlink(const QJsonObject& classifiers) - { + static CaraPerson::CaraBlink extractBlink(const QJsonObject& classifiers) { CaraPerson::CaraBlink blink = CaraPerson::BLINK_NOT_AVAILABLE; QJsonObject::const_iterator it = classifiers.constFind("blink"); - if(it != classifiers.constEnd()) - { + if(it != classifiers.constEnd()) { int b = (*it).toInt(CaraPerson::BLINK_NOT_AVAILABLE); - switch(b) - { - case CaraPerson::BLINK_NOT_AVAILABLE: - blink = CaraPerson::BLINK_NOT_AVAILABLE; - break; - case CaraPerson::NO_BLINK: - blink = CaraPerson::NO_BLINK; - break; - case CaraPerson::BLINK: - blink = CaraPerson::BLINK; - break; - default: - blink = CaraPerson::BLINK_NOT_AVAILABLE; - break; + switch(b) { + case CaraPerson::BLINK_NOT_AVAILABLE: + blink = CaraPerson::BLINK_NOT_AVAILABLE; + break; + case CaraPerson::NO_BLINK: + blink = CaraPerson::NO_BLINK; + break; + case CaraPerson::BLINK: + blink = CaraPerson::BLINK; + break; + default: + blink = CaraPerson::BLINK_NOT_AVAILABLE; + break; } } return blink; @@ -219,9 +212,9 @@ CaraFaceTracker::CaraFaceTracker() : _previousPitch(0.0f), _previousYaw(0.0f), _previousRoll(0.0f), - _eyeGazeLeftPitch(0), - _eyeGazeLeftYaw(0), - _eyeGazeRightPitch(0), + _eyeGazeLeftPitch(0.0f), + _eyeGazeLeftYaw(0.0f), + _eyeGazeRightPitch(0.0f), _eyeGazeRightYaw(0), _leftBlinkIndex(0), _rightBlinkIndex(1), @@ -234,7 +227,7 @@ CaraFaceTracker::CaraFaceTracker() : _browUpRightIndex(18), _mouthSmileLeftIndex(28), _mouthSmileRightIndex(29), - _jawOpenIndex(21) + _jawOpenIndex(21) { connect(&_udpSocket, SIGNAL(readyRead()), SLOT(readPendingDatagrams())); connect(&_udpSocket, SIGNAL(error(QAbstractSocket::SocketError)), SLOT(socketErrorOccurred(QAbstractSocket::SocketError))); @@ -254,10 +247,10 @@ CaraFaceTracker::CaraFaceTracker(const QHostAddress& host, quint16 port) : _previousPitch(0.0f), _previousYaw(0.0f), _previousRoll(0.0f), - _eyeGazeLeftPitch(0), - _eyeGazeLeftYaw(0), - _eyeGazeRightPitch(0), - _eyeGazeRightYaw(0), + _eyeGazeLeftPitch(0.0f), + _eyeGazeLeftYaw(0.0f), + _eyeGazeRightPitch(0.0f), + _eyeGazeRightYaw(0.0f), _leftBlinkIndex(0), _rightBlinkIndex(1), _leftEyeOpenIndex(8), @@ -269,7 +262,7 @@ CaraFaceTracker::CaraFaceTracker(const QHostAddress& host, quint16 port) : _browUpRightIndex(18), _mouthSmileLeftIndex(28), _mouthSmileRightIndex(29), - _jawOpenIndex(21) + _jawOpenIndex(21) { connect(&_udpSocket, SIGNAL(readyRead()), SLOT(readPendingDatagrams())); connect(&_udpSocket, SIGNAL(error(QAbstractSocket::SocketError)), SLOT(socketErrorOccurred(QAbstractSocket::SocketError))); @@ -282,42 +275,36 @@ CaraFaceTracker::CaraFaceTracker(const QHostAddress& host, quint16 port) : _blendshapeCoefficients.fill(0.0f); } -CaraFaceTracker::~CaraFaceTracker() -{ +CaraFaceTracker::~CaraFaceTracker() { if(_udpSocket.isOpen()) _udpSocket.close(); } -void CaraFaceTracker::init() -{ +void CaraFaceTracker::init() { } -void CaraFaceTracker::reset() -{ +void CaraFaceTracker::reset() { } -void CaraFaceTracker::bindTo(quint16 port) -{ +void CaraFaceTracker::bindTo(quint16 port) { bindTo(QHostAddress::Any, port); } -void CaraFaceTracker::bindTo(const QHostAddress& host, quint16 port) -{ - if(_udpSocket.isOpen()) +void CaraFaceTracker::bindTo(const QHostAddress& host, quint16 port) { + if(_udpSocket.isOpen()) { _udpSocket.close(); + } _udpSocket.bind(host, port); } -bool CaraFaceTracker::isActive() const -{ +bool CaraFaceTracker::isActive() const { static const int ACTIVE_TIMEOUT_USECS = 3000000; //3 secs return (usecTimestampNow() - _lastReceiveTimestamp < ACTIVE_TIMEOUT_USECS); } -void CaraFaceTracker::update() -{ +void CaraFaceTracker::update() { // get the euler angles relative to the window glm::vec3 eulers = glm::degrees(safeEulerAngles(_headRotation * glm::quat(glm::radians(glm::vec3( (_eyeGazeLeftPitch + _eyeGazeRightPitch) / 2.0f, (_eyeGazeLeftYaw + _eyeGazeRightYaw) / 2.0f, 0.0f))))); @@ -329,36 +316,39 @@ void CaraFaceTracker::update() } //private slots and methods -void CaraFaceTracker::socketErrorOccurred(QAbstractSocket::SocketError socketError) -{ +void CaraFaceTracker::socketErrorOccurred(QAbstractSocket::SocketError socketError) { qDebug() << "[Error] Cara Face Tracker Socket Error: " << _udpSocket.errorString(); } -void CaraFaceTracker::socketStateChanged(QAbstractSocket::SocketState socketState) -{ +void CaraFaceTracker::socketStateChanged(QAbstractSocket::SocketState socketState) { QString state; - switch(socketState) - { - case QAbstractSocket::BoundState: state = "Bounded"; - break; - case QAbstractSocket::ClosingState: state = "Closing"; - break; - case QAbstractSocket::ConnectedState: state = "Connected"; - break; - case QAbstractSocket::ConnectingState: state = "Connecting"; - break; - case QAbstractSocket::HostLookupState: state = "Host Lookup"; - break; - case QAbstractSocket::ListeningState: state = "Listening"; - break; - case QAbstractSocket::UnconnectedState: state = "Unconnected"; - break; + switch(socketState) { + case QAbstractSocket::BoundState: + state = "Bounded"; + break; + case QAbstractSocket::ClosingState: + state = "Closing"; + break; + case QAbstractSocket::ConnectedState: + state = "Connected"; + break; + case QAbstractSocket::ConnectingState: + state = "Connecting"; + break; + case QAbstractSocket::HostLookupState: + state = "Host Lookup"; + break; + case QAbstractSocket::ListeningState: + state = "Listening"; + break; + case QAbstractSocket::UnconnectedState: + state = "Unconnected"; + break; } qDebug() << "[Info] Cara Face Tracker Socket: " << socketState; } -void CaraFaceTracker::readPendingDatagrams() -{ +void CaraFaceTracker::readPendingDatagrams() { QByteArray buffer; while (_udpSocket.hasPendingDatagrams()) { buffer.resize(_udpSocket.pendingDatagramSize()); @@ -367,14 +357,12 @@ void CaraFaceTracker::readPendingDatagrams() } } -void CaraFaceTracker::decodePacket(const QByteArray& buffer) -{ +void CaraFaceTracker::decodePacket(const QByteArray& buffer) { //decode the incoming udp packet QJsonParseError jsonError; CaraPerson person = CaraPacketDecoder::extractOne(buffer, &jsonError); - if(jsonError.error == QJsonParseError::NoError) - { + if(jsonError.error == QJsonParseError::NoError) { //do some noise filtering to the head poses //reduce the noise first by truncating to 1 dp person.pose.roll = glm::floor(person.pose.roll * 10) / 10; @@ -388,8 +376,7 @@ void CaraFaceTracker::decodePacket(const QByteArray& buffer) // Compute angular velocity of the head glm::quat r = newRotation * glm::inverse(_headRotation); float theta = 2 * acos(r.w); - if (theta > EPSILON) - { + if (theta > EPSILON) { float rMag = glm::length(glm::vec3(r.x, r.y, r.z)); const float AVERAGE_CARA_FRAME_TIME = 0.033f; const float ANGULAR_VELOCITY_MIN = 1.2f; @@ -398,18 +385,21 @@ void CaraFaceTracker::decodePacket(const QByteArray& buffer) _headAngularVelocity = theta / AVERAGE_CARA_FRAME_TIME * glm::vec3(r.x, r.y, r.z) / rMag; //use the angular velocity for roll and pitch, if it's below the threshold don't move - if(glm::abs(_headAngularVelocity.x) < ANGULAR_VELOCITY_MIN) - person.pose.pitch = _previousPitch; + if(glm::abs(_headAngularVelocity.x) < ANGULAR_VELOCITY_MIN) { + person.pose.pitch = _previousPitch; + } - if(glm::abs(_headAngularVelocity.z) < ANGULAR_VELOCITY_MIN) + if(glm::abs(_headAngularVelocity.z) < ANGULAR_VELOCITY_MIN) { person.pose.roll = _previousRoll; + } //for yaw, the jitter is great, you can't use angular velocity because it swings too much //use the previous and current yaw, calculate the //abs difference and move it the difference is above the standard deviation which is around 2.5 // (this will introduce some jerks but will not encounter lag) - if(glm::abs(person.pose.yaw - _previousYaw) < YAW_STANDARD_DEV_DEG) // < the standard deviation 2.5 deg, no move - { + + // < the standard deviation 2.5 deg, no move + if(glm::abs(person.pose.yaw - _previousYaw) < YAW_STANDARD_DEV_DEG) { //qDebug() << "Yaw Diff: " << glm::abs(person.pose.yaw - _previousYaw); person.pose.yaw = _previousYaw; } @@ -422,8 +412,7 @@ void CaraFaceTracker::decodePacket(const QByteArray& buffer) //set the new rotation newRotation = glm::quat(glm::vec3(DEGTORAD(person.pose.pitch), DEGTORAD(person.pose.yaw), DEGTORAD(-person.pose.roll))); } - else - { + else { //no change in position newRotation = glm::quat(glm::vec3(DEGTORAD(_previousPitch), DEGTORAD(_previousYaw), DEGTORAD(-_previousRoll))); _headAngularVelocity = glm::vec3(0,0,0); @@ -434,7 +423,6 @@ void CaraFaceTracker::decodePacket(const QByteArray& buffer) //TODO: head translation, right now is 0 - //Do Blendshapes, clip between 0.0f to 1.0f, neg should be ignored _blendshapeCoefficients[_leftBlinkIndex] = person.blink == CaraPerson::BLINK ? 1.0f : 0.0f; _blendshapeCoefficients[_rightBlinkIndex] = person.blink == CaraPerson::BLINK ? 1.0f : 0.0f; @@ -450,14 +438,14 @@ void CaraFaceTracker::decodePacket(const QByteArray& buffer) _blendshapeCoefficients[_mouthSmileLeftIndex] = person.emotion.smile < 0.0f ? 0.0f : person.emotion.smile; _blendshapeCoefficients[_mouthSmileRightIndex] = person.emotion.smile < 0.0f ? 0.0f : person.emotion.smile; } - else + else { qDebug() << "[Error] Cara Face Tracker Decode Error: " << jsonError.errorString(); + } _lastReceiveTimestamp = usecTimestampNow(); } -float CaraFaceTracker::getBlendshapeCoefficient(int index) const -{ +float CaraFaceTracker::getBlendshapeCoefficient(int index) const { return (index >= 0 && index < (int)_blendshapeCoefficients.size()) ? _blendshapeCoefficients[index] : 0.0f; } diff --git a/interface/src/devices/CaraFaceTracker.h b/interface/src/devices/CaraFaceTracker.h index 3a34d94b12..f51fed0f1b 100644 --- a/interface/src/devices/CaraFaceTracker.h +++ b/interface/src/devices/CaraFaceTracker.h @@ -26,8 +26,7 @@ * host address (eg: 127.0.0.1 for localhost) and destination port 36555. **/ -class CaraFaceTracker : public FaceTracker -{ +class CaraFaceTracker : public FaceTracker { Q_OBJECT public: From 1e719f1a88bea72d3b5824bbae4e81269fa6b68c Mon Sep 17 00:00:00 2001 From: Zu Date: Wed, 30 Jul 2014 10:04:20 +0800 Subject: [PATCH 6/6] fixed: restyled code for better readability --- interface/src/devices/CaraFaceTracker.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/interface/src/devices/CaraFaceTracker.cpp b/interface/src/devices/CaraFaceTracker.cpp index 0638ba5f26..9c67163dca 100644 --- a/interface/src/devices/CaraFaceTracker.cpp +++ b/interface/src/devices/CaraFaceTracker.cpp @@ -24,7 +24,15 @@ static const QHostAddress CARA_FEATURE_POINT_SERVER_ADDR("127.0.0.1"); static const quint16 CARA_FEATURE_POINT_SERVER_PORT = 36555; -static QString sampleJson = "[{\"id\":1,\"face\":{\"x\":248,\"y\":64,\"width\":278,\"height\":341},\"pose\":{\"roll\":2.62934,\"pitch\":-12.2318,\"yaw\":0.936743},\"feature_points\":[314,194,326,187,340,187,354,189,367,193,409,190,421,187,435,184,448,183,459,188,388,207,389,223,390,240,391,257,377,266,384,267,392,268,399,266,407,264,331,209,341,204,354,204,364,209,353,214,341,214,410,208,420,201,433,200,443,205,434,211,421,211,362,294,372,290,383,287,393,289,404,286,415,289,426,291,418,300,407,306,394,308,382,307,371,302,383,295,394,295,404,294,404,295,393,297,383,296],\"classifiers\":{\"emotion\":{\"smi\":-0.368829,\"sur\":-1.33334,\"neg\":0.00235828,\"att\":1},\"blink\":1}}]"; +static QString sampleJson = "[{\"id\":1, \ + \"face\":{\"x\":248,\"y\":64,\"width\":278,\"height\":341}, \ + \"pose\":{\"roll\":2.62934,\"pitch\":-12.2318,\"yaw\":0.936743}, \ + \"feature_points\":[314,194,326,187,340,187,354,189,367,193,409,190,421,187,435,184,448,183,459,188, \ + 388,207,389,223,390,240,391,257,377,266,384,267,392,268,399,266,407,264,331,209, \ + 341,204,354,204,364,209,353,214,341,214,410,208,420,201,433,200,443,205,434,211, \ + 421,211,362,294,372,290,383,287,393,289,404,286,415,289,426,291,418,300,407,306, \ + 394,308,382,307,371,302,383,295,394,295,404,294,404,295,393,297,383,296], \ + \"classifiers\":{\"emotion\":{\"smi\":-0.368829,\"sur\":-1.33334,\"neg\":0.00235828,\"att\":1},\"blink\":1}}]"; static const glm::vec3 DEFAULT_HEAD_ORIGIN(0.0f, 0.0f, 0.0f); static const float TRANSLATION_SCALE = 1.0f;