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"); }