void WebcamDevice::faceRT()
{
cv::Mat rvecs;
std::vector<cv::Point2f> pointsVisage2D;
std::vector<cv::Point2f> moyPointsVisage2D;
bool faceDetected = detecterVisage(&pointsVisage2D);
if(faceDetected)
{
_nbrLoopSinceLastDetection = 0;
_images.push_back(pointsVisage2D);
}
else
_nbrLoopSinceLastDetection++;
if((_images.size() > NBRSAVEDIMAGES || _nbrLoopSinceLastDetection > NBRSAVEDIMAGES) && !_images.empty())
_images.erase(_images.begin());
if(!_images.empty())
{
for(int i = 0; i < NBRFACEPOINTSDETECTED; i++)
{
cv::Point2f coordonee(0.0f, 0.0f);
for(int j = 0; j < _images.size(); j++)
{
coordonee.x += _images[j][i].x;
coordonee.y += _images[j][i].y;
}
coordonee.x /= _images.size();
coordonee.y /= _images.size();
moyPointsVisage2D.push_back(coordonee);
}
cv::solvePnP(_pointsVisage3D, moyPointsVisage2D, _cameraMatrix, _distCoeffs, rvecs, _tvecs);
_rotVecs = cv::Mat(3, 3, CV_64F);
cv::Rodrigues(rvecs, _rotVecs);
emit updateScene(_rotVecs, _tvecs);
}
}
void main()
{
bool patternfound = false;
bool resetAuto = false;
int nbImages = 0;
double moyFinale = 0;
bool detectionVisage = false;
int nbrLoopSinceLastDetection = 0;
int criticalValueOfLoopWithoutDetection = 15;
std::cout << "initialisation de Chehra..." << std::endl;
Chehra chehra;
std::cout << "done" << std::endl;
cv::Mat cameraMatrix, distCoeffs;
cv::Mat rvecs, tvecs;
std::vector<cv::Point2f> imagePoints;
std::vector<cv::Point2f> pointsVisage2D;
std::vector<cv::Point2f> moyPointsVisage2D;
std::vector<cv::Point3f> pointsVisage3D;
std::vector<cv::Point3f> visage;
std::vector<double> distances;
double moyDistances;
std::vector<std::vector<cv::Point2f>> images;
std::vector<cv::Mat> frames;
double s = 10.0f;
osg::Matrixd matrixS; // scale
matrixS.set(
s, 0, 0, 0,
0, s, 0, 0,
0, 0, s, 0,
0, 0, 0, 1);
pointsVisage3D.push_back(cv::Point3f(90,0,-80));
pointsVisage3D.push_back(cv::Point3f(-90,0,-80));
pointsVisage3D.push_back(cv::Point3f(0,0,0));
pointsVisage3D.push_back(cv::Point3f(600,0,600));
pointsVisage3D.push_back(cv::Point3f(0,0,600));
pointsVisage3D.push_back(cv::Point3f(-600,0,600));
/*
pointsVisage3D.push_back(cv::Point3f(13.1, -98.1,108.3)); // exterieur narine gauche
pointsVisage3D.push_back(cv::Point3f(-13.1, -98.1,108.3)); // exterieur narine droite
pointsVisage3D.push_back(cv::Point3f(0, -87.2, 124.2)); // bout du nez
pointsVisage3D.push_back(cv::Point3f(44.4, -57.9, 83.7)); // exterieur oeil gauche
pointsVisage3D.push_back(cv::Point3f(0, 55.4, 101.4)); // haut du nez, centre des yeux
pointsVisage3D.push_back(cv::Point3f(-44.4, -57.9, 83.7)); // exterieur oeil droit
*/
cv::FileStorage fs("../rsc/intrinsicMatrix.yml", cv::FileStorage::READ);
fs["cameraMatrix"] >> cameraMatrix;
fs["distCoeffs"] >> distCoeffs;
double f = (cameraMatrix.at<double>(0, 0) + cameraMatrix.at<double>(1, 1)) / 2; // NEAR = distance focale ; si pixels carrés, fx = fy -> np
//mais est généralement différent de fy donc on prend (pour l'instant) par défaut la valeur médiane
double g = 2000 * f; // je sais pas pourquoi. au pif.
fs.release();
cv::VideoCapture vcap(0);
if(!vcap.isOpened())
{
std::cout << "FAIL!" << std::endl;
return;
}
cv::Mat *frame = new cv::Mat(cv::Mat::zeros(vcap.get(CV_CAP_PROP_FRAME_HEIGHT), vcap.get(CV_CAP_PROP_FRAME_WIDTH), CV_8UC3));
do
{
vcap >> *frame;
}while(frame->empty());
osg::ref_ptr<osg::Image> backgroundImage = new osg::Image;
backgroundImage->setImage(frame->cols, frame->rows, 3,
GL_RGB, GL_BGR, GL_UNSIGNED_BYTE,
(uchar*)(frame->data),
osg::Image::AllocationMode::NO_DELETE, 1);
// read the scene from the list of file specified commandline args.
osg::ref_ptr<osg::Group> group = new osg::Group;
osg::ref_ptr<osg::Geode> cam = createHUD(backgroundImage, vcap.get(CV_CAP_PROP_FRAME_WIDTH), vcap.get(CV_CAP_PROP_FRAME_HEIGHT), cameraMatrix.at<double>(0, 2), cameraMatrix.at<double>(1, 2), f);
std::cout << "initialisation de l'objet 3D..." << std::endl;
osg::ref_ptr<osg::Node> objet3D = osgDB::readNodeFile("../rsc/objets3D/brain.obj");
//osg::ref_ptr<osg::Node> objet3D = osgDB::readNodeFile("../rsc/objets3D/dumptruck.osgt");
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
osg::Sphere* unitSphere = new osg::Sphere(osg::Vec3(0, -1000, 1000), 100.0);
osg::ShapeDrawable* unitSphereDrawable = new osg::ShapeDrawable(unitSphere);
osg::Geode* objet3D = new osg::Geode();
objet3D->addDrawable(unitSphereDrawable);
*/
//osg::StateSet* sphereStateset = unitSphereDrawable->getOrCreateStateSet();
//sphereStateset->setMode(GL_DEPTH_TEST,osg::StateAttribute::OFF);
//sphereStateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
std::cout << "done" << std::endl;
osg::StateSet* obectStateset = objet3D->getOrCreateStateSet();
obectStateset->setMode(GL_LIGHTING,osg::StateAttribute::OFF);
obectStateset->setMode(GL_DEPTH_TEST,osg::StateAttribute::OFF);
osg::ref_ptr<osg::MatrixTransform> mat = new osg::MatrixTransform();
// construct the viewer.
osgViewer::CompositeViewer compositeViewer;
osgViewer::View* viewer = new osgViewer::View;
osgViewer::View* viewer2 = new osgViewer::View;
// add the HUD subgraph.
group->addChild(cam);
mat->addChild(objet3D);
group->addChild(mat);
osg::Matrixd projectionMatrix;
projectionMatrix.makeFrustum(
-cameraMatrix.at<double>(0, 2), vcap.get(CV_CAP_PROP_FRAME_WIDTH) - cameraMatrix.at<double>(0, 2),
-cameraMatrix.at<double>(1, 2), vcap.get(CV_CAP_PROP_FRAME_HEIGHT) - cameraMatrix.at<double>(1, 2),
f, g);
osg::Vec3d eye(0.0f, 0.0f, 0.0f), target(0.0f, g, 0.0f), normal(0.0f, 0.0f, 1.0f);
// set the scene to render
viewer->setSceneData(group.get());
viewer->setUpViewInWindow(0, 0, 1920 / 2, 1080 / 2);
viewer->getCamera()->setProjectionMatrix(projectionMatrix);
viewer->getCamera()->setViewMatrixAsLookAt(eye, target, normal);
viewer2->setSceneData(group.get());
viewer2->setUpViewInWindow(1920 / 2, 0, 1920 / 2, 1080 / 2);
viewer2->getCamera()->setProjectionMatrix(projectionMatrix);
osg::Vec3d eye2(4 * f, 3 * f / 2, 0.0f), target2(0.0f, f, 0.0f), normal2(0.0f, 0.0f, 1.0f);
viewer2->getCamera()->setViewMatrixAsLookAt(eye2, target2, normal2);
compositeViewer.addView(viewer2);
compositeViewer.addView(viewer);
compositeViewer.realize(); // set up windows and associated threads.
do
{
patternfound = false;
resetAuto = false;
detectionVisage = false;
moyPointsVisage2D.clear();
pointsVisage2D.clear();
visage.clear();
moyDistances = 0;
distances.clear();
std::cout << "recherche de pattern" << std::endl
<< "nbr images sauvegardees : " << images.size() << std::endl;
vcap >> *frame;
frames.push_back(*frame);
detectionVisage = detecterVisage(frame, &chehra, &pointsVisage2D, &visage);
if(detectionVisage)
{
images.push_back(pointsVisage2D);
nbrLoopSinceLastDetection = 0;
group->addChild(mat);
}
else
nbrLoopSinceLastDetection++;
if((images.size() > NBRSAVEDIMAGES || nbrLoopSinceLastDetection > criticalValueOfLoopWithoutDetection) && !images.empty())
images.erase(images.begin());
if(images.empty())
group->removeChild(mat);
else
{
//cv::cornerSubPix(*frame, pointsVisage2D, cv::Size(5, 5), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
for(int i = 0; i < NBRFACEPOINTSDETECTED; i++)
{
cv::Point2f coordonee(0.0f, 0.0f);
for(int j = 0; j < images.size(); j++)
{
coordonee.x += images[j][i].x;
coordonee.y += images[j][i].y;
}
coordonee.x /= images.size();
coordonee.y /= images.size();
moyPointsVisage2D.push_back(coordonee);
}
cv::solvePnP(pointsVisage3D, moyPointsVisage2D, cameraMatrix, distCoeffs, rvecs, tvecs);
cv::Mat rotVec(3, 3, CV_64F);
cv::Rodrigues(rvecs, rotVec);
imagePoints = dessinerPoints(frame, pointsVisage3D, rotVec, tvecs, cameraMatrix, distCoeffs);
double t3 = tvecs.at<double>(2, 0);
double t1 = tvecs.at<double>(0, 0);
double t2 = tvecs.at<double>(1, 0) + t3 / 27.5; // and now, magic !
double r11 = rotVec.at<double>(0, 0);
double r12 = rotVec.at<double>(0, 1);
double r13 = rotVec.at<double>(0, 2);
double r21 = rotVec.at<double>(1, 0);
double r22 = rotVec.at<double>(1, 1);
double r23 = rotVec.at<double>(1, 2);
double r31 = rotVec.at<double>(2, 0);
double r32 = rotVec.at<double>(2, 1);
double r33 = rotVec.at<double>(2, 2);
osg::Matrixd matrixR; // rotation (transposee de rotVec)
matrixR.set(
r11, r21, r31, 0,
r12, r22, r32, 0,
r13, r23, r33, 0,
0, 0, 0, 1);
osg::Matrixd matrixT; // translation
matrixT.makeTranslate(t1, t2, t3);
osg::Matrixd matrix90; // rotation de repere entre opencv et osg
matrix90.makeRotate(osg::Quat(osg::DegreesToRadians(-90.0f), osg::Vec3d(1.0, 0.0, 0.0)));
mat->setMatrix(matrixS * matrixR * matrixT * matrix90);
// Calcul d'erreur de reprojection
double moy = 0;
for(int i = 0; i < pointsVisage2D.size(); i++)
{
double d = sqrt(pow(pointsVisage2D[i].y - imagePoints[i].y, 2) + pow(pointsVisage2D[i].x - imagePoints[i].x, 2));
distances.push_back(d);
moy += d;
}
moyDistances = moy / pointsVisage2D.size();
if(moyDistances > 2) // si l'ecart de reproj est trop grand, reset
resetAuto = true;
}
backgroundImage->dirty();
compositeViewer.frame();
}while(!compositeViewer.done());
}
void main()
{
bool patternfound = false;
bool reset = false;
bool resetAuto = false;
int nbImages = 0;
double moyFinale = 0;
char key = 0;
bool detectionMire = false;
bool detectionVisage = false;
int cpt = 0, moyCpt = 0, i = 0;
std::cout << "initialisation de Chehra..." << std::endl;
Chehra chehra;
std::cout << "done" << std::endl;
cv::TermCriteria termcrit(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03);
cv::Size winSize(31, 31);
cv::Mat cameraMatrix, distCoeffs;
cv::Mat imCalib;
cv::Mat imCalibColor;
cv::Mat imCalibNext;
cv::Mat rvecs, tvecs;
cv::Mat Rc, C = cv::Mat(3, 1, CV_64F), rotVecInv;
std::vector<cv::Point2f> imagePoints;
std::vector<cv::Point3f> objectPoints;
std::vector<cv::Point3f> cubeObjectPoints;
std::vector<cv::Point3f> dessinPointsVisage;
std::vector<std::vector<cv::Point2f>> chessCornersInit(2);
std::vector<std::vector<cv::Point2f>> pointsVisageInit(2);
std::vector<cv::Point3f> chessCorners3D;
std::vector<cv::Point3f> pointsVisage3D;
std::vector<cv::Point3f> visage;
std::vector<double> distances;
double moyDistances;
// Creation des coins de la mire
for(int x = 0; x < COLCHESSBOARD; x++)
for(int y = 0; y < ROWCHESSBOARD; y++)
chessCorners3D.push_back(cv::Point3f(x * SIZEMIRE, y * SIZEMIRE, 0.0f));
// Creation des points a projeter
for(int x = 0; x < COLCHESSBOARD; x++)
for(int y = 0; y < ROWCHESSBOARD; y++)
objectPoints.push_back(cv::Point3f(x * SIZEMIRE, y * SIZEMIRE, 0.0f));
cv::FileStorage fs("../rsc/intrinsicMatrix.yml", cv::FileStorage::READ);
fs["cameraMatrix"] >> cameraMatrix;
fs["distCoeffs"] >> distCoeffs;
double f = (cameraMatrix.at<double>(0, 0) + cameraMatrix.at<double>(1, 1)) / 2; // NEAR = distance focale ; si pixels carrés, fx = fy -> np
//mais est généralement différent de fy donc on prend (pour l'instant) par défaut la valeur médiane
double g = 2000 * f; // je sais pas pourquoi. au pif.
fs.release();
cv::VideoCapture vcap(0);
if(!vcap.isOpened()){
std::cout << "FAIL!" << std::endl;
return;
}
cv::Mat *frame = new cv::Mat(cv::Mat::zeros(vcap.get(CV_CAP_PROP_FRAME_HEIGHT), vcap.get(CV_CAP_PROP_FRAME_WIDTH), CV_8UC3));
do
{
vcap >> *frame;
}while(frame->empty());
osg::ref_ptr<osg::Image> backgroundImage = new osg::Image;
backgroundImage->setImage(frame->cols, frame->rows, 3,
GL_RGB, GL_BGR, GL_UNSIGNED_BYTE,
(uchar*)(frame->data),
osg::Image::AllocationMode::NO_DELETE, 1);
// read the scene from the list of file specified commandline args.
osg::ref_ptr<osg::Group> group = new osg::Group;
osg::ref_ptr<osg::Geode> cam = createHUD(backgroundImage, vcap.get(CV_CAP_PROP_FRAME_WIDTH), vcap.get(CV_CAP_PROP_FRAME_HEIGHT), cameraMatrix.at<double>(0, 2), cameraMatrix.at<double>(1, 2), f);
std::cout << "initialisation de l'objet 3D..." << std::endl;
osg::ref_ptr<osg::Node> objet3D = osgDB::readNodeFile("../rsc/objets3D/Creature.obj");
std::cout << "done" << std::endl;
osg::StateSet* obectStateset = objet3D->getOrCreateStateSet();
obectStateset->setMode(GL_DEPTH_TEST,osg::StateAttribute::OFF);
osg::ref_ptr<osg::MatrixTransform> mat = new osg::MatrixTransform();
osg::ref_ptr<osg::PositionAttitudeTransform> pat = new osg::PositionAttitudeTransform();
// construct the viewer.
osgViewer::CompositeViewer compositeViewer;
osgViewer::View* viewer = new osgViewer::View;
osgViewer::View* viewer2 = new osgViewer::View;
// add the HUD subgraph.
group->addChild(cam);
mat->addChild(objet3D);
pat->addChild(mat);
group->addChild(pat);
pat->setScale(osg::Vec3d(3, 3, 3));
osg::Matrixd projectionMatrix;
projectionMatrix.makeFrustum(
-cameraMatrix.at<double>(0, 2), vcap.get(CV_CAP_PROP_FRAME_WIDTH) - cameraMatrix.at<double>(0, 2),
-cameraMatrix.at<double>(1, 2), vcap.get(CV_CAP_PROP_FRAME_HEIGHT) - cameraMatrix.at<double>(1, 2),
f, g);
osg::Vec3d eye(0.0f, 0.0f, 0.0f), target(0.0f, g, 0.0f), normal(0.0f, 0.0f, 1.0f);
// set the scene to render
viewer->setSceneData(group.get());
viewer->setUpViewInWindow(0, 0, 1920 / 2, 1080 / 2);
viewer->getCamera()->setProjectionMatrix(projectionMatrix);
viewer->getCamera()->setViewMatrixAsLookAt(eye, target, normal);
viewer2->setSceneData(group.get());
viewer2->setUpViewInWindow(1920 / 2, 0, 1920 / 2, 1080 / 2);
viewer2->getCamera()->setProjectionMatrix(projectionMatrix);
osg::Vec3d eye2(4 * f, 3 * f / 2, 0.0f), target2(0.0f, f, 0.0f), normal2(0.0f, 0.0f, 1.0f);
viewer2->getCamera()->setViewMatrixAsLookAt(eye2, target2, normal2);
compositeViewer.addView(viewer);
compositeViewer.addView(viewer2);
compositeViewer.realize(); // set up windows and associated threads.
do
{
group->removeChild(pat);
patternfound = false;
resetAuto = false;
detectionMire = false;
detectionVisage = false;
imagePoints.clear();
chessCornersInit[0].clear();
chessCornersInit[1].clear();
pointsVisageInit[0].clear();
pointsVisageInit[1].clear();
pointsVisage3D.clear();
dessinPointsVisage.clear();
visage.clear();
moyDistances = 0;
distances.clear();
imCalibNext.release();
std::cout << "recherche de pattern" << std::endl;
time_t start = clock();
double timer = 0;
do
{
start = clock();
vcap >> *frame;
backgroundImage->dirty();
//detectionMire = detecterMire(frame, &chessCornersInit[1], &imCalibNext);
detectionVisage = detecterVisage(frame, &chehra, &pointsVisageInit[1], &visage, &pointsVisage3D, &imCalibNext);
cpt++;
double duree = (clock() - start)/(double) CLOCKS_PER_SEC;
timer += duree;
if(timer >= 1){
std::cout << cpt << " fps" << std::endl;
moyCpt += cpt;
timer = 0;
duree = 0;
i++;
cpt = 0;
start = clock();
}
compositeViewer.frame();
}while(!detectionMire && !detectionVisage && !compositeViewer.done());
if(compositeViewer.done())
break;
std::cout << "pattern detectee" << std::endl << std::endl;
group->addChild(pat);
do
{
start = clock();
vcap >> *frame;
cv::Mat rotVec = trackingMire(frame, &imCalibNext, &pointsVisageInit, &pointsVisage3D, &cameraMatrix, &distCoeffs, &tvecs);
//cv::Mat rotVec = trackingMire(frame, &imCalibNext, &chessCornersInit, &chessCorners3D, &cameraMatrix, &distCoeffs, &tvecs);
//imagePoints = dessinerPoints(frame, objectPoints, rotVec, tvecs, cameraMatrix, distCoeffs);
imagePoints = dessinerPoints(frame, pointsVisage3D, rotVec, tvecs, cameraMatrix, distCoeffs);
double r11 = rotVec.at<double>(0, 0);
double r21 = rotVec.at<double>(1, 0);
double r31 = rotVec.at<double>(2, 0);
double r32 = rotVec.at<double>(2, 1);
double r33 = rotVec.at<double>(2, 2);
osg::Matrixd matrixR;
matrixR.makeRotate(
atan2(r32, r33), osg::Vec3d(1.0, 0.0, 0.0),
-atan2(-r31, sqrt((r32 * r32) + (r33 * r33))), osg::Vec3d(0.0, 0.0, 1.0),
atan2(r21, r11), osg::Vec3d(0.0, 1.0, 0.0));
mat->setMatrix(matrixR);
pat->setPosition(osg::Vec3d(tvecs.at<double>(0, 0), tvecs.at<double>(2, 0), -tvecs.at<double>(1, 0)));
//std::cout << "x = " << tvecs.at<double>(0, 0) << " - y = " << tvecs.at<double>(1, 0) << " - z = " << tvecs.at<double>(2, 0) << std::endl;
// Calcul d'erreur de reprojection
double moy = 0;
for(int j = 0; j < pointsVisageInit[1].size() ; j++)
{
double d = sqrt(pow(pointsVisageInit[0][j].y - imagePoints[j].y, 2) + pow(pointsVisageInit[0][j].x - imagePoints[j].x, 2));
distances.push_back(d);
moy += d;
}
moyDistances = moy / pointsVisageInit[1].size();
if(moyDistances > 1) // si l'ecart de reproj est trop grand, reset
resetAuto = true;
double duree = (clock() - start)/(double) CLOCKS_PER_SEC;
std::cout << (int)(1/duree) << " fps" << std::endl;
moyCpt += (int)(1/duree);
duree = 0;
i++;
backgroundImage->dirty();
compositeViewer.frame();
}while(!compositeViewer.done() && !resetAuto);
}while(!compositeViewer.done());
std::cout << std::endl << "Moyenne des fps : " << moyCpt/i << std::endl;
std::system("PAUSE");
}
