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

Merge pull request #568 from ey6es/opencv

Browse source 
Head roll, lean from webcam data.
This commit is contained in:
Philip Rosedale 2013-06-21 11:41:21 -07:00
commit 1f6fd31b52
10 changed files with 26386 additions and 47 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

26161
interface/resources/haarcascades/haarcascade_frontalface_alt.xml Normal file
View file

File diff suppressed because it is too large Load diff

20
interface/src/Application.cpp
View file

@ -310,7 +310,7 @@ void Application::paintGL() {
glEnable(GL_LINE_SMOOTH);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
float headCameraScale = _serialHeadSensor.active ? _headCameraPitchYawScale : 1.0f;
float headCameraScale = _serialHeadSensor.isActive() ? _headCameraPitchYawScale : 1.0f;
if (_myCamera.getMode() == CAMERA_MODE_MIRROR) {
_myCamera.setTightness (100.0f);
@ -781,7 +781,7 @@ void Application::timer() {
gettimeofday(&_timerStart, NULL);
// if we haven't detected gyros, check for them now
if (!_serialHeadSensor.active) {
if (!_serialHeadSensor.isActive()) {
_serialHeadSensor.pair();
}
@ -1582,7 +1582,7 @@ void Application::update(float deltaTime) {
}
// Read serial port interface devices
if (_serialHeadSensor.active) {
if (_serialHeadSensor.isActive()) {
_serialHeadSensor.readData(deltaTime);
}
@ -1660,8 +1660,10 @@ void Application::update(float deltaTime) {
void Application::updateAvatar(float deltaTime) {
if (_serialHeadSensor.active) {
// Update my avatar's head position from gyros and/or webcam
_myAvatar.updateHeadFromGyrosAndOrWebcam();
if (_serialHeadSensor.isActive()) {
// Update avatar head translation
if (_gyroLook->isChecked()) {
@ -1670,10 +1672,7 @@ void Application::updateAvatar(float deltaTime) {
headPosition *= HEAD_OFFSET_SCALING;
_myCamera.setEyeOffsetPosition(headPosition);
}
// Update my avatar's head position from gyros
_myAvatar.updateHeadFromGyros(deltaTime, &_serialHeadSensor);
// Grab latest readings from the gyros
float measuredPitchRate = _serialHeadSensor.getLastPitchRate();
float measuredYawRate = _serialHeadSensor.getLastYawRate();
@ -2528,9 +2527,10 @@ void Application::resetSensors() {
_headMouseX = _mouseX = _glWidget->width() / 2;
_headMouseY = _mouseY = _glWidget->height() / 2;
if (_serialHeadSensor.active) {
if (_serialHeadSensor.isActive()) {
_serialHeadSensor.resetAverages();
}
_webcam.reset();
QCursor::setPos(_headMouseX, _headMouseY);
_myAvatar.reset();
_myTransmitter.resetLevels();

1
interface/src/Application.h
View file

@ -76,6 +76,7 @@ public:
VoxelSystem* getVoxels() { return &_voxels; }
QSettings* getSettings() { return _settings; }
Environment* getEnvironment() { return &_environment; }
SerialInterface* getSerialHeadSensor() { return &_serialHeadSensor; }
Webcam* getWebcam() { return &_webcam; }
bool shouldEchoAudio() { return _echoAudioMode->isChecked(); }

27
interface/src/Avatar.cpp
View file

@ -280,22 +280,33 @@ void Avatar::reset() {
}
// Update avatar head rotation with sensor data
void Avatar::updateHeadFromGyros(float deltaTime, SerialInterface* serialInterface) {
const float AMPLIFY_PITCH = 2.f;
const float AMPLIFY_YAW = 2.f;
const float AMPLIFY_ROLL = 2.f;
void Avatar::updateHeadFromGyrosAndOrWebcam() {
const float AMPLIFY_PITCH = 1.f;
const float AMPLIFY_YAW = 1.f;
const float AMPLIFY_ROLL = 1.f;
glm::vec3 estimatedRotation = serialInterface->getEstimatedRotation();
SerialInterface* gyros = Application::getInstance()->getSerialHeadSensor();
Webcam* webcam = Application::getInstance()->getWebcam();
glm::vec3 estimatedPosition, estimatedRotation;
if (gyros->isActive()) {
estimatedPosition = gyros->getEstimatedPosition();
estimatedRotation = gyros->getEstimatedRotation();
} else if (webcam->isActive()) {
estimatedPosition = webcam->getEstimatedPosition();
estimatedRotation = webcam->getEstimatedRotation();
}
_head.setPitch(estimatedRotation.x * AMPLIFY_PITCH);
_head.setYaw(estimatedRotation.y * AMPLIFY_YAW);
_head.setRoll(estimatedRotation.z * AMPLIFY_ROLL);
// Update torso lean distance based on accelerometer data
glm::vec3 estimatedPosition = serialInterface->getEstimatedPosition() * _leanScale;
const float TORSO_LENGTH = 0.5f;
const float MAX_LEAN = 45.0f;
_head.setLeanSideways(glm::clamp(glm::degrees(atanf(-estimatedPosition.x / TORSO_LENGTH)), -MAX_LEAN, MAX_LEAN));
_head.setLeanForward(glm::clamp(glm::degrees(atanf(estimatedPosition.z / TORSO_LENGTH)), -MAX_LEAN, MAX_LEAN));
_head.setLeanSideways(glm::clamp(glm::degrees(atanf(-estimatedPosition.x * _leanScale / TORSO_LENGTH)),
-MAX_LEAN, MAX_LEAN));
_head.setLeanForward(glm::clamp(glm::degrees(atanf(estimatedPosition.z * _leanScale / TORSO_LENGTH)),
-MAX_LEAN, MAX_LEAN));
}
float Avatar::getAbsoluteHeadYaw() const {

2
interface/src/Avatar.h
View file

@ -86,7 +86,7 @@ public:
void reset();
void simulate(float deltaTime, Transmitter* transmitter);
void updateThrust(float deltaTime, Transmitter * transmitter);
void updateHeadFromGyros(float frametime, SerialInterface * serialInterface);
void updateHeadFromGyrosAndOrWebcam();
void updateFromMouse(int mouseX, int mouseY, int screenWidth, int screenHeight);
void addBodyYaw(float y) {_bodyYaw += y;};
void render(bool lookingInMirror, bool renderAvatarBalls);

2
interface/src/Camera.cpp
View file

@ -22,7 +22,7 @@ const float CAMERA_THIRD_PERSON_MODE_DISTANCE = 1.5f;
const float CAMERA_THIRD_PERSON_MODE_TIGHTNESS = 8.0f;
const float CAMERA_MIRROR_MODE_UP_SHIFT = 0.0f;
const float CAMERA_MIRROR_MODE_DISTANCE = 0.2f;
const float CAMERA_MIRROR_MODE_DISTANCE = 0.3f;
const float CAMERA_MIRROR_MODE_TIGHTNESS = 100.0f;
Camera::Camera() {

40
interface/src/SerialInterface.cpp
View file

@ -5,18 +5,23 @@
// Read interface data from the gyros/accelerometer Invensense board using the SerialUSB
//
#include "SerialInterface.h"
#include "SharedUtil.h"
#include "Util.h"
#include <glm/gtx/vector_angle.hpp>
#include <math.h>
#ifdef __APPLE__
#include <regex.h>
#include <sys/time.h>
#include <string>
#endif
#include <math.h>
#include <glm/gtx/vector_angle.hpp>
#include <SharedUtil.h>
#include "Application.h"
#include "SerialInterface.h"
#include "Util.h"
#include "Webcam.h"
const short NO_READ_MAXIMUM_MSECS = 3000;
const int GRAVITY_SAMPLES = 60; // Use the first few samples to baseline values
const int SENSOR_FUSION_SAMPLES = 20;
@ -103,7 +108,7 @@ void SerialInterface::initializePort(char* portname) {
printLog("Connected.\n");
resetSerial();
active = true;
_active = true;
#endif
}
@ -301,8 +306,17 @@ void SerialInterface::readData(float deltaTime) {
_estimatedVelocity += deltaTime * _estimatedAcceleration;
_estimatedPosition += deltaTime * _estimatedVelocity;
_estimatedVelocity *= DECAY_VELOCITY;
_estimatedPosition *= DECAY_POSITION;
// Attempt to fuse gyro position with webcam position
Webcam* webcam = Application::getInstance()->getWebcam();
if (webcam->isActive()) {
const float WEBCAM_POSITION_FUSION = 0.5f;
_estimatedPosition = glm::mix(_estimatedPosition, webcam->getEstimatedPosition(), WEBCAM_POSITION_FUSION);
} else {
_estimatedPosition *= DECAY_POSITION;
}
// Accumulate a set of initial baseline readings for setting gravity
if (totalSamples == 0) {
_gravity = _lastAcceleration;
@ -327,6 +341,12 @@ void SerialInterface::readData(float deltaTime) {
_estimatedRotation = safeEulerAngles(estimatedRotation);
// Fuse gyro roll with webcam roll
if (webcam->isActive()) {
_estimatedRotation.z = glm::mix(_estimatedRotation.z, webcam->getEstimatedRotation().z,
1.0f / SENSOR_FUSION_SAMPLES);
}
totalSamples++;
}
@ -359,7 +379,7 @@ void SerialInterface::resetAverages() {
void SerialInterface::resetSerial() {
#ifdef __APPLE__
resetAverages();
active = false;
_active = false;
gettimeofday(&lastGoodRead, NULL);
#endif
}

5
interface/src/SerialInterface.h
View file

@ -24,7 +24,7 @@ extern const bool USING_INVENSENSE_MPU9150;
class SerialInterface {
public:
SerialInterface() : active(false),
SerialInterface() : _active(false),
_gravity(0, 0, 0),
_averageRotationRates(0, 0, 0),
_averageAcceleration(0, 0, 0),
@ -58,12 +58,13 @@ public:
void renderLevels(int width, int height);
void resetAverages();
bool active;
bool isActive() const { return _active; }
private:
void initializePort(char* portname);
void resetSerial();
bool _active;
int _serialDescriptor;
int totalSamples;
timeval lastGoodRead;

143
interface/src/Webcam.cpp
View file

@ -8,8 +8,6 @@
#include <QTimer>
#include <QtDebug>
#include <opencv2/opencv.hpp>
#include <Log.h>
#include <SharedUtil.h>
@ -20,7 +18,14 @@
#include "Application.h"
#include "Webcam.h"
Webcam::Webcam() : _enabled(false), _frameTextureID(0) {
using namespace cv;
using namespace std;
// register OpenCV matrix type with Qt metatype system
int matMetaType = qRegisterMetaType<Mat>("cv::Mat");
int rotatedRectMetaType = qRegisterMetaType<RotatedRect>("cv::RotatedRect");
Webcam::Webcam() : _enabled(false), _active(false), _frameTextureID(0) {
// the grabber simply runs as fast as possible
_grabber = new FrameGrabber();
_grabber->moveToThread(&_grabberThread);
@ -36,10 +41,21 @@ void Webcam::setEnabled(bool enabled) {
_frameCount = 0;
// let the grabber know we're ready for the first frame
QMetaObject::invokeMethod(_grabber, "reset");
QMetaObject::invokeMethod(_grabber, "grabFrame");
} else {
_grabberThread.quit();
_active = false;
}
}
void Webcam::reset() {
_initialFaceRect = RotatedRect();
if (_enabled) {
// send a message to the grabber
QMetaObject::invokeMethod(_grabber, "reset");
}
}
@ -66,6 +82,17 @@ void Webcam::renderPreview(int screenWidth, int screenHeight) {
glBindTexture(GL_TEXTURE_2D, 0);
glDisable(GL_TEXTURE_2D);
glBegin(GL_LINE_LOOP);
Point2f facePoints[4];
_faceRect.points(facePoints);
float xScale = previewWidth / (float)_frameWidth;
float yScale = PREVIEW_HEIGHT / (float)_frameHeight;
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();
char fps[20];
sprintf(fps, "FPS: %d", (int)(roundf(_frameCount * 1000000.0f / (usecTimestampNow() - _startTimestamp))));
drawtext(left, top + PREVIEW_HEIGHT + 20, 0.10, 0, 1, 0, fps);
@ -80,25 +107,26 @@ Webcam::~Webcam() {
delete _grabber;
}
void Webcam::setFrame(void* image) {
IplImage* img = static_cast<IplImage*>(image);
glPixelStorei(GL_UNPACK_ROW_LENGTH, img->widthStep / 3);
void Webcam::setFrame(const Mat& frame, const RotatedRect& faceRect) {
IplImage image = frame;
glPixelStorei(GL_UNPACK_ROW_LENGTH, image.widthStep / 3);
if (_frameTextureID == 0) {
glGenTextures(1, &_frameTextureID);
glBindTexture(GL_TEXTURE_2D, _frameTextureID);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, _frameWidth = img->width, _frameHeight = img->height, 0, GL_BGR,
GL_UNSIGNED_BYTE, img->imageData);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, _frameWidth = image.width, _frameHeight = image.height, 0, GL_BGR,
GL_UNSIGNED_BYTE, image.imageData);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
printLog("Capturing webcam at %dx%d.\n", _frameWidth, _frameHeight);
} else {
glBindTexture(GL_TEXTURE_2D, _frameTextureID);
glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, _frameWidth, _frameHeight, GL_BGR, GL_UNSIGNED_BYTE, img->imageData);
glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, _frameWidth, _frameHeight, GL_BGR, GL_UNSIGNED_BYTE, image.imageData);
}
glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
glBindTexture(GL_TEXTURE_2D, 0);
// update our frame count for fps computation
// store our face rect, update our frame count for fps computation
_faceRect = faceRect;
_frameCount++;
const int MAX_FPS = 60;
@ -112,16 +140,55 @@ void Webcam::setFrame(void* image) {
}
_lastFrameTimestamp = now;
// roll is just the angle of the face rect (correcting for 180 degree rotations)
float roll = faceRect.angle;
if (roll < -90.0f) {
roll += 180.0f;
} else if (roll > 90.0f) {
roll -= 180.0f;
}
const float ROTATION_SMOOTHING = 0.95f;
_estimatedRotation.z = glm::mix(roll, _estimatedRotation.z, ROTATION_SMOOTHING);
// determine position based on translation and scaling of the face rect
if (_initialFaceRect.size.area() == 0) {
_initialFaceRect = faceRect;
_estimatedPosition = glm::vec3();
} else {
float proportion = sqrtf(_initialFaceRect.size.area() / (float)faceRect.size.area());
const float DISTANCE_TO_CAMERA = 0.333f;
const float POSITION_SCALE = 0.5f;
float z = DISTANCE_TO_CAMERA * proportion - DISTANCE_TO_CAMERA;
glm::vec3 position = glm::vec3(
(faceRect.center.x - _initialFaceRect.center.x) * proportion * POSITION_SCALE / _frameWidth,
(faceRect.center.y - _initialFaceRect.center.y) * proportion * POSITION_SCALE / _frameWidth,
z);
const float POSITION_SMOOTHING = 0.95f;
_estimatedPosition = glm::mix(position, _estimatedPosition, POSITION_SMOOTHING);
}
// 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() : _capture(0), _searchWindow(0, 0, 0, 0) {
}
FrameGrabber::~FrameGrabber() {
if (_capture != 0) {
cvReleaseCapture(&_capture);
}
}
void FrameGrabber::reset() {
_searchWindow = Rect(0, 0, 0, 0);
}
void FrameGrabber::grabFrame() {
if (_capture == 0) {
if ((_capture = cvCaptureFromCAM(-1)) == 0) {
@ -134,20 +201,70 @@ void FrameGrabber::grabFrame() {
cvSetCaptureProperty(_capture, CV_CAP_PROP_FRAME_HEIGHT, IDEAL_FRAME_HEIGHT);
#ifdef __APPLE__
configureCamera(0x5ac, 0x8510, false, 0.99, 0.5, 0.5, 0.5, true, 0.5);
configureCamera(0x5ac, 0x8510, false, 0.975, 0.5, 1.0, 0.5, true, 0.5);
#else
cvSetCaptureProperty(_capture, CV_CAP_PROP_EXPOSURE, 0.5);
cvSetCaptureProperty(_capture, CV_CAP_PROP_CONTRAST, 0.5);
cvSetCaptureProperty(_capture, CV_CAP_PROP_SATURATION, 0.5);
cvSetCaptureProperty(_capture, CV_CAP_PROP_BRIGHTNESS, 0.5);
cvSetCaptureProperty(_capture, CV_CAP_PROP_HUE, 0.5);
cvSetCaptureProperty(_capture, CV_CAP_PROP_GAIN, 0.5);
#endif
switchToResourcesParentIfRequired();
if (!_faceCascade.load("resources/haarcascades/haarcascade_frontalface_alt.xml")) {
printLog("Failed to load Haar cascade for face tracking.\n");
}
}
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) {
printLog("Invalid webcam image format.\n");
return;
}
QMetaObject::invokeMethod(Application::getInstance()->getWebcam(), "setFrame", Q_ARG(void*, image));
// if we don't have a search window (yet), try using the face cascade
Mat frame = image;
int channels = 0;
float ranges[] = { 0, 180 };
const float* range = ranges;
if (_searchWindow.area() == 0) {
vector<Rect> faces;
_faceCascade.detectMultiScale(frame, faces, 1.1, 6);
if (!faces.empty()) {
_searchWindow = faces.front();
updateHSVFrame(frame);
Mat faceHsv(_hsvFrame, _searchWindow);
Mat faceMask(_mask, _searchWindow);
int sizes = 30;
calcHist(&faceHsv, 1, &channels, faceMask, _histogram, 1, &sizes, &range);
double min, max;
minMaxLoc(_histogram, &min, &max);
_histogram.convertTo(_histogram, -1, (max == 0.0) ? 0.0 : 255.0 / max);
}
}
RotatedRect faceRect;
if (_searchWindow.area() > 0) {
updateHSVFrame(frame);
calcBackProject(&_hsvFrame, 1, &channels, _histogram, _backProject, &range);
bitwise_and(_backProject, _mask, _backProject);
faceRect = CamShift(_backProject, _searchWindow, TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1));
_searchWindow = faceRect.boundingRect();
}
QMetaObject::invokeMethod(Application::getInstance()->getWebcam(), "setFrame",
Q_ARG(cv::Mat, frame), Q_ARG(cv::RotatedRect, faceRect));
}
void FrameGrabber::updateHSVFrame(const Mat& frame) {
cvtColor(frame, _hsvFrame, CV_BGR2HSV);
inRange(_hsvFrame, Scalar(0, 55, 65), Scalar(180, 256, 256), _mask);
}

32
interface/src/Webcam.h
View file

@ -9,9 +9,14 @@
#ifndef __interface__Webcam__
#define __interface__Webcam__
#include <QMetaType>
#include <QObject>
#include <QThread>
#include <glm/glm.hpp>
#include <opencv2/opencv.hpp>
#include "InterfaceConfig.h"
class QImage;
@ -28,12 +33,17 @@ public:
Webcam();
~Webcam();
const bool isActive() const { return _active; }
const glm::vec3& getEstimatedPosition() const { return _estimatedPosition; }
const glm::vec3& getEstimatedRotation() const { return _estimatedRotation; }
void reset();
void renderPreview(int screenWidth, int screenHeight);
public slots:
void setEnabled(bool enabled);
void setFrame(void* image);
void setFrame(const cv::Mat& image, const cv::RotatedRect& faceRect);
private:
@ -41,14 +51,20 @@ private:
FrameGrabber* _grabber;
bool _enabled;
bool _active;
int _frameWidth;
int _frameHeight;
GLuint _frameTextureID;
cv::RotatedRect _faceRect;
cv::RotatedRect _initialFaceRect;
long long _startTimestamp;
int _frameCount;
long long _lastFrameTimestamp;
glm::vec3 _estimatedPosition;
glm::vec3 _estimatedRotation;
};
class FrameGrabber : public QObject {
@ -56,16 +72,28 @@ class FrameGrabber : public QObject {
public:
FrameGrabber() : _capture(0) { }
FrameGrabber();
virtual ~FrameGrabber();
public slots:
void reset();
void grabFrame();
private:
void updateHSVFrame(const cv::Mat& frame);
CvCapture* _capture;
cv::CascadeClassifier _faceCascade;
cv::Mat _hsvFrame;
cv::Mat _mask;
cv::SparseMat _histogram;
cv::Mat _backProject;
cv::Rect _searchWindow;
};
Q_DECLARE_METATYPE(cv::Mat)
Q_DECLARE_METATYPE(cv::RotatedRect)
#endif /* defined(__interface__Webcam__) */