////  Webcam.cpp//  interface////  Created by Andrzej Kapolka on 6/17/13.//  Copyright (c) 2013 High Fidelity, Inc. All rights reserved. #include <QTimer>#include <QtDebug> #include <vpx_encoder.h>#include <vp8cx.h> #ifdef __APPLE__#include <UVCCameraControl.hpp>#endif #include <SharedUtil.h> #include "Application.h"#include "Webcam.h"#include "avatar/Face.h" using namespace cv;using namespace std; #ifdef HAVE_OPENNIusing namespace xn;#endif // register types with Qt metatype systemint jointVectorMetaType = qRegisterMetaType<JointVector>("JointVector");int matMetaType = qRegisterMetaType<Mat>("cv::Mat");int rotatedRectMetaType = qRegisterMetaType<RotatedRect>("cv::RotatedRect"); Webcam::Webcam() : _enabled(false), _active(false), _colorTextureID(0), _depthTextureID(0), _skeletonTrackingOn(false) {    // the grabber simply runs as fast as possible    _grabber = new FrameGrabber();    _grabber->moveToThread(&_grabberThread);} void Webcam::setEnabled(bool enabled) {    if (_enabled == enabled) {        return;    }    if ((_enabled = enabled)) {        _grabberThread.start();        _startTimestamp = 0;        _frameCount = 0;         // let the grabber know we're ready for the first frame        QMetaObject::invokeMethod(_grabber, "reset");        QMetaObject::invokeMethod(_grabber, "grabFrame");     } else {        QMetaObject::invokeMethod(_grabber, "shutdown");        _active = false;    }} const float UNINITIALIZED_FACE_DEPTH = 0.0f; void Webcam::reset() {    _initialFaceRect = RotatedRect();    _initialFaceDepth = UNINITIALIZED_FACE_DEPTH;     if (_enabled) {        // send a message to the grabber        QMetaObject::invokeMethod(_grabber, "reset");    }} void Webcam::renderPreview(int screenWidth, int screenHeight) {    if (_enabled) {        glEnable(GL_TEXTURE_2D);        glColor3f(1.0f, 1.0f, 1.0f);                const int PREVIEW_HEIGHT = 200;        int previewWidth = _textureSize.width * PREVIEW_HEIGHT / _textureSize.height;        int top = screenHeight - 600;        int left = screenWidth - previewWidth - 10;        if (_colorTextureID != 0) {            glBindTexture(GL_TEXTURE_2D, _colorTextureID);                    glBegin(GL_QUADS);                glTexCoord2f(0, 0);                glVertex2f(left, top);                glTexCoord2f(1, 0);                glVertex2f(left + previewWidth, top);                glTexCoord2f(1, 1);                glVertex2f(left + previewWidth, top + PREVIEW_HEIGHT);                glTexCoord2f(0, 1);                glVertex2f(left, top + PREVIEW_HEIGHT);            glEnd();        }                if (_depthTextureID != 0) {            glBindTexture(GL_TEXTURE_2D, _depthTextureID);            glBegin(GL_QUADS);                glTexCoord2f(0, 0);                glVertex2f(left, top - PREVIEW_HEIGHT);                glTexCoord2f(1, 0);                glVertex2f(left + previewWidth, top - PREVIEW_HEIGHT);                glTexCoord2f(1, 1);                glVertex2f(left + previewWidth, top);                glTexCoord2f(0, 1);                glVertex2f(left, top);            glEnd();             glBindTexture(GL_TEXTURE_2D, 0);            glDisable(GL_TEXTURE_2D);             if (!_joints.isEmpty()) {                glColor3f(1.0f, 0.0f, 0.0f);                glPointSize(4.0f);                glBegin(GL_POINTS);                    float projectedScale = PREVIEW_HEIGHT / _textureSize.height;                    foreach (const Joint& joint, _joints) {                        if (joint.isValid) {                            glVertex2f(left + joint.projected.x * projectedScale,                                top - PREVIEW_HEIGHT + joint.projected.y * projectedScale);                        }                    }                glEnd();                glPointSize(1.0f);            }        } else {            glBindTexture(GL_TEXTURE_2D, 0);            glDisable(GL_TEXTURE_2D);        }         glColor3f(1.0f, 1.0f, 1.0f);        glBegin(GL_LINE_LOOP);            Point2f facePoints[4];            _faceRect.points(facePoints);            float xScale = previewWidth / _textureSize.width;            float yScale = PREVIEW_HEIGHT / _textureSize.height;            glVertex2f(left + facePoints[0].x * xScale, top + facePoints[0].y * yScale);            glVertex2f(left + facePoints[1].x * xScale, top + facePoints[1].y * yScale);            glVertex2f(left + facePoints[2].x * xScale, top + facePoints[2].y * yScale);            glVertex2f(left + facePoints[3].x * xScale, top + facePoints[3].y * yScale);        glEnd();         const int MAX_FPS_CHARACTERS = 30;        char fps[MAX_FPS_CHARACTERS];        sprintf(fps, "FPS: %d", (int)(roundf(_frameCount * 1000000.0f / (usecTimestampNow() - _startTimestamp))));        drawtext(left, top + PREVIEW_HEIGHT + 20, 0.10, 0, 1, 0, fps);    }} Webcam::~Webcam() {    // stop the grabber thread    _grabberThread.quit();    _grabberThread.wait();     delete _grabber;} const float METERS_PER_MM = 1.0f / 1000.0f; void Webcam::setFrame(const Mat& color, int format, const Mat& depth, float midFaceDepth,        float aspectRatio, const RotatedRect& faceRect, bool sending, const JointVector& joints) {    if (!_enabled) {        return; // was queued before we shut down; ignore    }    if (!color.empty()) {        IplImage colorImage = color;        glPixelStorei(GL_UNPACK_ROW_LENGTH, colorImage.widthStep / 3);        if (_colorTextureID == 0) {            glGenTextures(1, &_colorTextureID);            glBindTexture(GL_TEXTURE_2D, _colorTextureID);            glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, _textureSize.width = colorImage.width,                _textureSize.height = colorImage.height, 0, format, GL_UNSIGNED_BYTE, colorImage.imageData);            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);                    } else {            glBindTexture(GL_TEXTURE_2D, _colorTextureID);            glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, _textureSize.width, _textureSize.height, format,                GL_UNSIGNED_BYTE, colorImage.imageData);        }    } else if (_colorTextureID != 0) {        glDeleteTextures(1, &_colorTextureID);        _colorTextureID = 0;    }        if (!depth.empty()) {        IplImage depthImage = depth;        glPixelStorei(GL_UNPACK_ROW_LENGTH, depthImage.widthStep);        if (_depthTextureID == 0) {            glGenTextures(1, &_depthTextureID);            glBindTexture(GL_TEXTURE_2D, _depthTextureID);            glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE, depthImage.width, depthImage.height, 0,                GL_LUMINANCE, GL_UNSIGNED_BYTE, depthImage.imageData);            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);         } else {            glBindTexture(GL_TEXTURE_2D, _depthTextureID);            glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, _textureSize.width = depthImage.width,                _textureSize.height = depthImage.height, GL_LUMINANCE, GL_UNSIGNED_BYTE, depthImage.imageData);        }    } else if (_depthTextureID != 0) {        glDeleteTextures(1, &_depthTextureID);        _depthTextureID = 0;    }    glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);    glBindTexture(GL_TEXTURE_2D, 0);     // store our various data, update our frame count for fps computation    _aspectRatio = aspectRatio;    _faceRect = faceRect;    _sending = sending;    _joints = _skeletonTrackingOn ? joints : JointVector();    _frameCount++;     const int MAX_FPS = 60;    const int MIN_FRAME_DELAY = 1000000 / MAX_FPS;    uint64_t now = usecTimestampNow();    int remaining = MIN_FRAME_DELAY;    if (_startTimestamp == 0) {        _startTimestamp = now;    } else {        remaining -= (now - _lastFrameTimestamp);    }    _lastFrameTimestamp = now;     // see if we have joint data    if (!_joints.isEmpty()) {        _estimatedJoints.resize(NUM_AVATAR_JOINTS);        glm::vec3 origin;        if (_joints[AVATAR_JOINT_LEFT_HIP].isValid && _joints[AVATAR_JOINT_RIGHT_HIP].isValid) {            origin = glm::mix(_joints[AVATAR_JOINT_LEFT_HIP].position, _joints[AVATAR_JOINT_RIGHT_HIP].position, 0.5f);         } else if (_joints[AVATAR_JOINT_TORSO].isValid) {            const glm::vec3 TORSO_TO_PELVIS = glm::vec3(0.0f, -0.09f, -0.01f);            origin = _joints[AVATAR_JOINT_TORSO].position + TORSO_TO_PELVIS;        }        for (int i = 0; i < NUM_AVATAR_JOINTS; i++) {            if (!_joints[i].isValid) {                continue;            }            const float JOINT_SMOOTHING = 0.5f;            _estimatedJoints[i].isValid = true;            _estimatedJoints[i].position = glm::mix(_joints[i].position - origin,                _estimatedJoints[i].position, JOINT_SMOOTHING);            _estimatedJoints[i].rotation = safeMix(_joints[i].rotation,                _estimatedJoints[i].rotation, JOINT_SMOOTHING);        }        _estimatedRotation = safeEulerAngles(_estimatedJoints[AVATAR_JOINT_HEAD_BASE].rotation);        _estimatedPosition = _estimatedJoints[AVATAR_JOINT_HEAD_BASE].position;     } else {        // roll is just the angle of the face rect        const float ROTATION_SMOOTHING = 0.95f;        _estimatedRotation.z = glm::mix(_faceRect.angle, _estimatedRotation.z, ROTATION_SMOOTHING);         // determine position based on translation and scaling of the face rect/mean face depth        if (_initialFaceRect.size.area() == 0) {            _initialFaceRect = _faceRect;            _estimatedPosition = glm::vec3();            _initialFaceDepth = midFaceDepth;         } else {            float proportion, z;            if (midFaceDepth == UNINITIALIZED_FACE_DEPTH) {                proportion = sqrtf(_initialFaceRect.size.area() / (float)_faceRect.size.area());                const float INITIAL_DISTANCE_TO_CAMERA = 0.333f;                z = INITIAL_DISTANCE_TO_CAMERA * proportion - INITIAL_DISTANCE_TO_CAMERA;             } else {                z = (midFaceDepth - _initialFaceDepth) * METERS_PER_MM;                proportion = midFaceDepth / _initialFaceDepth;            }            const float POSITION_SCALE = 0.5f;            _estimatedPosition = glm::vec3(                (_faceRect.center.x - _initialFaceRect.center.x) * proportion * POSITION_SCALE / _textureSize.width,                (_faceRect.center.y - _initialFaceRect.center.y) * proportion * POSITION_SCALE / _textureSize.width,                z);        }    }     // note that we have data    _active = true;     // let the grabber know we're ready for the next frame    QTimer::singleShot(qMax((int)remaining / 1000, 0), _grabber, SLOT(grabFrame()));} FrameGrabber::FrameGrabber() : _initialized(false), _videoSendMode(FULL_FRAME_VIDEO), _depthOnly(false), _capture(0),    _searchWindow(0, 0, 0, 0), _smoothedMidFaceDepth(UNINITIALIZED_FACE_DEPTH), _colorCodec(), _depthCodec(), _frameCount(0) {} FrameGrabber::~FrameGrabber() {    if (_initialized) {        shutdown();    }} #ifdef HAVE_OPENNIstatic AvatarJointID xnToAvatarJoint(XnSkeletonJoint joint) {    switch (joint) {        case XN_SKEL_HEAD: return AVATAR_JOINT_HEAD_TOP;        case XN_SKEL_NECK: return AVATAR_JOINT_HEAD_BASE;        case XN_SKEL_TORSO: return AVATAR_JOINT_CHEST;         case XN_SKEL_LEFT_SHOULDER: return AVATAR_JOINT_RIGHT_ELBOW;        case XN_SKEL_LEFT_ELBOW: return AVATAR_JOINT_RIGHT_WRIST;         case XN_SKEL_RIGHT_SHOULDER: return AVATAR_JOINT_LEFT_ELBOW;        case XN_SKEL_RIGHT_ELBOW: return AVATAR_JOINT_LEFT_WRIST;         case XN_SKEL_LEFT_HIP: return AVATAR_JOINT_RIGHT_KNEE;        case XN_SKEL_LEFT_KNEE: return AVATAR_JOINT_RIGHT_HEEL;        case XN_SKEL_LEFT_FOOT: return AVATAR_JOINT_RIGHT_TOES;         case XN_SKEL_RIGHT_HIP: return AVATAR_JOINT_LEFT_KNEE;        case XN_SKEL_RIGHT_KNEE: return AVATAR_JOINT_LEFT_HEEL;        case XN_SKEL_RIGHT_FOOT: return AVATAR_JOINT_LEFT_TOES;         default: return AVATAR_JOINT_NULL;    }} static int getParentJoint(XnSkeletonJoint joint) {    switch (joint) {        case XN_SKEL_HEAD: return XN_SKEL_NECK;        case XN_SKEL_TORSO: return -1;         case XN_SKEL_LEFT_ELBOW: return XN_SKEL_LEFT_SHOULDER;        case XN_SKEL_LEFT_HAND: return XN_SKEL_LEFT_ELBOW;         case XN_SKEL_RIGHT_ELBOW: return XN_SKEL_RIGHT_SHOULDER;        case XN_SKEL_RIGHT_HAND: return XN_SKEL_RIGHT_ELBOW;         case XN_SKEL_LEFT_KNEE: return XN_SKEL_LEFT_HIP;        case XN_SKEL_LEFT_FOOT: return XN_SKEL_LEFT_KNEE;         case XN_SKEL_RIGHT_KNEE: return XN_SKEL_RIGHT_HIP;        case XN_SKEL_RIGHT_FOOT: return XN_SKEL_RIGHT_KNEE;         default: return XN_SKEL_TORSO;    }} static glm::vec3 xnToGLM(const XnVector3D& vector, bool flip = false) {    return glm::vec3(vector.X * (flip ? -1 : 1), vector.Y, vector.Z);} static glm::quat xnToGLM(const XnMatrix3X3& matrix) {    glm::quat rotation = glm::quat_cast(glm::mat3(        matrix.elements[0], matrix.elements[1], matrix.elements[2],        matrix.elements[3], matrix.elements[4], matrix.elements[5],        matrix.elements[6], matrix.elements[7], matrix.elements[8]));    return glm::quat(rotation.w, -rotation.x, rotation.y, rotation.z);} static void XN_CALLBACK_TYPE newUser(UserGenerator& generator, XnUserID id, void* cookie) {    qDebug("Found user %d.\n", id);    generator.GetSkeletonCap().RequestCalibration(id, false);} static void XN_CALLBACK_TYPE lostUser(UserGenerator& generator, XnUserID id, void* cookie) {    qDebug("Lost user %d.\n", id);} static void XN_CALLBACK_TYPE calibrationStarted(SkeletonCapability& capability, XnUserID id, void* cookie) {    qDebug("Calibration started for user %d.\n", id);} static void XN_CALLBACK_TYPE calibrationCompleted(SkeletonCapability& capability,        XnUserID id, XnCalibrationStatus status, void* cookie) {    if (status == XN_CALIBRATION_STATUS_OK) {        qDebug("Calibration completed for user %d.\n", id);        capability.StartTracking(id);     } else {        qDebug("Calibration failed to user %d.\n", id);        capability.RequestCalibration(id, true);    }}#endif void FrameGrabber::cycleVideoSendMode() {    _videoSendMode = (VideoSendMode)((_videoSendMode + 1) % VIDEO_SEND_MODE_COUNT);    _searchWindow = cv::Rect(0, 0, 0, 0);     destroyCodecs();} void FrameGrabber::setDepthOnly(bool depthOnly) {    _depthOnly = depthOnly;    destroyCodecs();} void FrameGrabber::reset() {    _searchWindow = cv::Rect(0, 0, 0, 0); #ifdef HAVE_OPENNI    if (_userGenerator.IsValid() && _userGenerator.GetSkeletonCap().IsTracking(_userID)) {        _userGenerator.GetSkeletonCap().RequestCalibration(_userID, true);    }#endif} void FrameGrabber::shutdown() {    if (_capture != 0) {        cvReleaseCapture(&_capture);        _capture = 0;    }    destroyCodecs();    _initialized = false;     // send an empty video message to indicate that we're no longer sending    QMetaObject::invokeMethod(Application::getInstance(), "sendAvatarFaceVideoMessage",            Q_ARG(int, ++_frameCount), Q_ARG(QByteArray, QByteArray()));     thread()->quit();} static Point clip(const Point& point, const Rect& bounds) {    return Point(glm::clamp(point.x, bounds.x, bounds.x + bounds.width - 1),        glm::clamp(point.y, bounds.y, bounds.y + bounds.height - 1));} void FrameGrabber::grabFrame() {    if (!(_initialized || init())) {        return;    }    int format = GL_BGR;    Mat color, depth;    JointVector joints; #ifdef HAVE_OPENNI    if (_depthGenerator.IsValid()) {        _xnContext.WaitAnyUpdateAll();        color = Mat(_imageMetaData.YRes(), _imageMetaData.XRes(), CV_8UC3, (void*)_imageGenerator.GetImageMap());        format = GL_RGB;         depth = Mat(_depthMetaData.YRes(), _depthMetaData.XRes(), CV_16UC1, (void*)_depthGenerator.GetDepthMap());         _userID = 0;        XnUInt16 userCount = 1;        _userGenerator.GetUsers(&_userID, userCount);        if (userCount > 0 && _userGenerator.GetSkeletonCap().IsTracking(_userID)) {            joints.resize(NUM_AVATAR_JOINTS);            const int MAX_ACTIVE_JOINTS = 16;            XnSkeletonJoint activeJoints[MAX_ACTIVE_JOINTS];            XnUInt16 activeJointCount = MAX_ACTIVE_JOINTS;            _userGenerator.GetSkeletonCap().EnumerateActiveJoints(activeJoints, activeJointCount);            XnSkeletonJointTransformation transform;            for (int i = 0; i < activeJointCount; i++) {                AvatarJointID avatarJoint = xnToAvatarJoint(activeJoints[i]);                if (avatarJoint == AVATAR_JOINT_NULL) {                    continue;                }                _userGenerator.GetSkeletonCap().GetSkeletonJoint(_userID, activeJoints[i], transform);                XnVector3D projected;                _depthGenerator.ConvertRealWorldToProjective(1, &transform.position.position, &projected);                glm::quat rotation = xnToGLM(transform.orientation.orientation);                int parentJoint = getParentJoint(activeJoints[i]);                if (parentJoint != -1) {                    XnSkeletonJointOrientation parentOrientation;                    _userGenerator.GetSkeletonCap().GetSkeletonJointOrientation(                        _userID, (XnSkeletonJoint)parentJoint, parentOrientation);                    rotation = glm::inverse(xnToGLM(parentOrientation.orientation)) * rotation;                }                joints[avatarJoint] = Joint(xnToGLM(transform.position.position, true) * METERS_PER_MM,                    rotation, xnToGLM(projected));            }        }    }#endif     if (color.empty()) {        IplImage* image = cvQueryFrame(_capture);        if (image == 0) {            // try again later            QMetaObject::invokeMethod(this, "grabFrame", Qt::QueuedConnection);            return;        }        // make sure it's in the format we expect        if (image->nChannels != 3 || image->depth != IPL_DEPTH_8U || image->dataOrder != IPL_DATA_ORDER_PIXEL ||                image->origin != 0) {            qDebug("Invalid webcam image format.\n");            return;        }        color = image;    }     const int ENCODED_FACE_WIDTH = 128;    const int ENCODED_FACE_HEIGHT = 128;    int encodedWidth;    int encodedHeight;    float colorBitrateMultiplier = 1.0f;    float depthBitrateMultiplier = 1.0f;    Mat faceTransform;    float aspectRatio;    if (_videoSendMode == FULL_FRAME_VIDEO) {        // no need to find the face if we're sending full frame video        _smoothedFaceRect = RotatedRect(Point2f(color.cols / 2.0f, color.rows / 2.0f), Size2f(color.cols, color.rows), 0.0f);        encodedWidth = color.cols;

Lines 1–500 of 878. Load more