├── 3DModel ├── model3D.yml ├── model3D.yml~ └── refU.txt ├── README.md ├── base.h ├── calib ├── POSIT.cpp ├── POSIT.h ├── calib.cpp ├── calib.h ├── pro │ ├── Makefile │ ├── POSIT.o │ ├── calib.o │ ├── pro │ ├── pro.pro │ ├── pro.pro~ │ └── util.o └── util │ ├── calib_util.cpp │ └── calib_util.h ├── data ├── cameraMatrix.yml ├── facialFeaturePoint.yml └── test.png ├── frontalUtil.cpp ├── frontalUtil.h ├── frontalization.cpp ├── frontalization.h ├── main.cpp └── pro ├── Makefile ├── frontal.png ├── frontalization ├── frontalization.pro └── image.png /3DModel/model3D.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | width: 320 3 | height: 320 4 | refUFile: "/home/cheng/workshop/github/faceFrontalization/3DModel/refU.txt" 5 | refURows: 102400 6 | refUCols: 3 7 | refXY: !!opencv-matrix 8 | rows: 49 9 | cols: 2 10 | dt: "f" 11 | data: [109.7365952,115.8851547,117.585701,110.8398132,127.8898621,110.0792923,137.8934937,110.5729599,147.7483978,113.332695,173.720932,112.1959,184.4673615,109.0394211,195.5401764,108.2654266,206.2992554,109.8522644,214.4782104,116.6021042,161.9249268,125.9964905,161.4693451,138.6013641,161.1615601,150.8859863,161.2645569,163.5360718,150.4766998,173.6735229,155.7660217,176.0691986,161.9886169,177.1124573,168.1548004,176.3821106,174.1511536,174.6470642,125.4865341,130.1312561,131.114563,124.7741852,138.9476013,124.4113541,144.8603668,130.2280273,138.4181519,132.2924957,131.6833954,132.1276398,178.2443237,130.5578003,184.9634094,125.5698242,192.8355713,125.2144699,199.9768372,131.114502,192.6046906,133.1011505,185.3194885,132.5821075,138.8685913,195.9920044,144.9897003,190.948288,153.2016754,188.8756409,161.6029663,191.1163788,170.1511383,189.280304,178.0612183,193.1295929,184.1129303,199.3013916,178.2834015,204.1846619,170.4006195,207.3748932,161.4348755,207.7410278,152.7237854,206.1211395,145.3255615,201.7760773,152.5141144,195.8942108,161.9247742,197.1711731,170.8932343,197.4502563,171.0696259,197.1178284,161.7276611,197.2687683,152.252182,195.7824249] 12 | threedee: !!opencv-matrix 13 | rows: 49 14 | cols: 3 15 | dt: "f" 16 | data: [-55.42033386,-82.67904663,61.11323929,-45.56922531,-98.42118835,64.81201172,-34.78852844,-105.9593811,65.05863953,-24.44763374,-110.6387177,63.57116699,-14.3771553,-112.2763596,61.40605164,11.63015842,-111.4056396,62.49211121,21.70399094,-110.074791,65.63617706,34.15935135,-104.3227386,67.2592926,45.2833252,-93.72779083,66.32575989,56.06071472,-70.6840744,61.12577438,-0.3818820119,-110.4471664,48.54393387,-1.384994507,-119.8051071,35.085289,-1.384994507,-126.5091019,23.13986397,-1.384994507,-137.019577,10.58758545,-12.34908581,-113.5865936,0.4774355292,-6.316779613,-118.6960068,-1.360358119,-0.3991972804,-118.9716034,-2.337758303,5.562378407,-115.6807251,-1.452229977,11.60267067,-112.4466019,-0.5517185926,-39.01011276,-89.75563049,45.83440399,-32.44721985,-94.54811096,50.70966721,-24.16884041,-94.43888855,51.75328064,-18.02840424,-92.18225098,45.68160248,-25.21068382,-94.3058548,43.47608566,-31.36531639,-95.33740234,43.41532135,16.36264992,-90.32823944,44.75436783,23.37151146,-96.46141052,49.53823471,31.65464592,-95.9850235,50.60459137,39.55066681,-87.54508972,44.92396927,31.72645569,-94.88024139,42.40751648,23.43865776,-95.08405304,42.39592361,-23.74625587,-103.8953094,-22.13973618,-17.3444252,-113.2277451,-16.48893547,-9.289386749,-117.8630142,-14.23037624,-0.3994419873,-119.0920792,-16.13157845,7.5184865,-117.25914,-14.26472187,15.58408165,-112.8749924,-18.50679398,21.95779037,-104.6422577,-25.13134384,15.5699234,-113.2850189,-29.4509716,7.546066284,-115.7505112,-32.21264648,-1.384994507,-110.1305695,-33.74151611,-9.361989975,-113.3951263,-31.43560219,-17.44602585,-110.1015625,-27.7213974,-9.358950615,-113.5821915,-21.45105934,-0.3885091543,-113.7097549,-22.43847466,8.607043266,-112.3714447,-22.53634071,8.607043266,-112.3714447,-22.53634071,-0.3885091543,-113.7097549,-22.43847466,-10.35987949,-113.3532944,-21.46733284] 17 | outA: !!opencv-matrix 18 | rows: 3 19 | cols: 3 20 | dt: "f" 21 | data: [492.30771036, 0, 160, 0, 492.307710365, 160, 0, 0, 1] 22 | -------------------------------------------------------------------------------- /3DModel/model3D.yml~: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | width: 320 3 | height: 320 4 | refUFile: "refU.txt" 5 | refURows: 102400 6 | refUCols: 3 7 | refXY: !!opencv-matrix 8 | rows: 49 9 | cols: 2 10 | dt: "f" 11 | data: [109.7365952,115.8851547,117.585701,110.8398132,127.8898621,110.0792923,137.8934937,110.5729599,147.7483978,113.332695,173.720932,112.1959,184.4673615,109.0394211,195.5401764,108.2654266,206.2992554,109.8522644,214.4782104,116.6021042,161.9249268,125.9964905,161.4693451,138.6013641,161.1615601,150.8859863,161.2645569,163.5360718,150.4766998,173.6735229,155.7660217,176.0691986,161.9886169,177.1124573,168.1548004,176.3821106,174.1511536,174.6470642,125.4865341,130.1312561,131.114563,124.7741852,138.9476013,124.4113541,144.8603668,130.2280273,138.4181519,132.2924957,131.6833954,132.1276398,178.2443237,130.5578003,184.9634094,125.5698242,192.8355713,125.2144699,199.9768372,131.114502,192.6046906,133.1011505,185.3194885,132.5821075,138.8685913,195.9920044,144.9897003,190.948288,153.2016754,188.8756409,161.6029663,191.1163788,170.1511383,189.280304,178.0612183,193.1295929,184.1129303,199.3013916,178.2834015,204.1846619,170.4006195,207.3748932,161.4348755,207.7410278,152.7237854,206.1211395,145.3255615,201.7760773,152.5141144,195.8942108,161.9247742,197.1711731,170.8932343,197.4502563,171.0696259,197.1178284,161.7276611,197.2687683,152.252182,195.7824249] 12 | threedee: !!opencv-matrix 13 | rows: 49 14 | cols: 3 15 | dt: "f" 16 | data: [-55.42033386,-82.67904663,61.11323929,-45.56922531,-98.42118835,64.81201172,-34.78852844,-105.9593811,65.05863953,-24.44763374,-110.6387177,63.57116699,-14.3771553,-112.2763596,61.40605164,11.63015842,-111.4056396,62.49211121,21.70399094,-110.074791,65.63617706,34.15935135,-104.3227386,67.2592926,45.2833252,-93.72779083,66.32575989,56.06071472,-70.6840744,61.12577438,-0.3818820119,-110.4471664,48.54393387,-1.384994507,-119.8051071,35.085289,-1.384994507,-126.5091019,23.13986397,-1.384994507,-137.019577,10.58758545,-12.34908581,-113.5865936,0.4774355292,-6.316779613,-118.6960068,-1.360358119,-0.3991972804,-118.9716034,-2.337758303,5.562378407,-115.6807251,-1.452229977,11.60267067,-112.4466019,-0.5517185926,-39.01011276,-89.75563049,45.83440399,-32.44721985,-94.54811096,50.70966721,-24.16884041,-94.43888855,51.75328064,-18.02840424,-92.18225098,45.68160248,-25.21068382,-94.3058548,43.47608566,-31.36531639,-95.33740234,43.41532135,16.36264992,-90.32823944,44.75436783,23.37151146,-96.46141052,49.53823471,31.65464592,-95.9850235,50.60459137,39.55066681,-87.54508972,44.92396927,31.72645569,-94.88024139,42.40751648,23.43865776,-95.08405304,42.39592361,-23.74625587,-103.8953094,-22.13973618,-17.3444252,-113.2277451,-16.48893547,-9.289386749,-117.8630142,-14.23037624,-0.3994419873,-119.0920792,-16.13157845,7.5184865,-117.25914,-14.26472187,15.58408165,-112.8749924,-18.50679398,21.95779037,-104.6422577,-25.13134384,15.5699234,-113.2850189,-29.4509716,7.546066284,-115.7505112,-32.21264648,-1.384994507,-110.1305695,-33.74151611,-9.361989975,-113.3951263,-31.43560219,-17.44602585,-110.1015625,-27.7213974,-9.358950615,-113.5821915,-21.45105934,-0.3885091543,-113.7097549,-22.43847466,8.607043266,-112.3714447,-22.53634071,8.607043266,-112.3714447,-22.53634071,-0.3885091543,-113.7097549,-22.43847466,-10.35987949,-113.3532944,-21.46733284] 17 | outA: !!opencv-matrix 18 | rows: 3 19 | cols: 3 20 | dt: "f" 21 | data: [492.30771036, 0, 160, 0, 492.307710365, 160, 0, 0, 1] -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ##Face Frontalization 2 | To align a face in profile to front view. This system is an implemetation of paper "Effective Face Frontalization in Unconstrained Images". 3 | 4 | Tal Hassner, Shai Harel, Eran Paz, Roee Enbar" 5 | 6 | ##Usage 7 | frontalization [queryImage] [facialPointsFile] [3DModelFile] 8 | 9 | (A demo : ./frontalization image.png ../data/facialFeaturePoint.yml ../3DModel/model3D.yml Before running the code, you should modify the "refU.txt" path in 3DModel/model3D.yml according to your settings. Otherwise, you will see "line: 73, file: ../frontalUtil.cpp, message: open /home/cheng/workshop/github/faceFrontalization/3DModel/refU.txt error") 10 | 11 | ####Parameters 12 | frontalization : program name 13 | query image : input image 14 | facialPointsFile : facial feature points detected by SDM 15 | 3DModelFile : predefined 3D model (already provided) 16 | 17 | 18 | ##Demo 19 | The left one is input image and the right one is output. 20 | ![p1](pro/image.png) 21 | ![p2](pro/frontal.png) 22 | 23 | 24 | ##Reference 25 | "Effective Face Frontalization in Unconstrained Images" 26 | by Tal Hassner, Shai Harel, Eran Paz, Roee Enbar 27 | 28 | 29 | 35 | -------------------------------------------------------------------------------- /base.h: -------------------------------------------------------------------------------- 1 | #ifndef BASE_H 2 | #define BASE_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | using namespace std; 9 | using namespace cv; 10 | 11 | typedef struct model3D 12 | { 13 | Mat_ refU; // mapping between reference image and 3d model (Nx3) 14 | Size sizeU; // size of refU, just for easy access 15 | Mat_ refXY; // detected facial points in reference image 16 | Mat_ threedee; // matching points of refXY in 3D model 17 | Mat_ outA; // intrinsic matrix of camera 18 | }model3D,*model3DP; 19 | 20 | 21 | #ifndef DEBUGMSG 22 | #define DEBUGMSG(msg) cout << "line: " <<__LINE__ \ 23 | << ", file: " << __FILE__ \ 24 | << ", message: " << msg << endl; 25 | #endif 26 | 27 | 28 | #endif // BASE_H 29 | -------------------------------------------------------------------------------- /calib/POSIT.cpp: -------------------------------------------------------------------------------- 1 | #include "POSIT.h" 2 | #include 3 | 4 | using namespace std; 5 | 6 | POSIT::POSIT() { 7 | } 8 | 9 | POSIT::~POSIT() { 10 | cvReleasePOSITObject(&positObject); 11 | // cvReleaseMat(&intrinsics); 12 | } 13 | 14 | void POSIT::initialize(double cameraMatrix[3 * 3], const double &aWidth, const double &aHeight, 15 | const double &nearPlane, const double &farPlane) { 16 | width = aWidth; 17 | height = aHeight; 18 | 19 | //Generate four model points 20 | //The first one must be (0,0,0) so we shift all 21 | shift[0] = modelPoints[0].x; 22 | shift[1] = modelPoints[0].y; 23 | shift[2] = modelPoints[0].z; 24 | 25 | for (unsigned int i = 0; i < modelPoints.size(); i++) { 26 | modelPoints[i].x -= shift[0]; 27 | modelPoints[i].y -= shift[1]; 28 | modelPoints[i].z -= shift[2]; 29 | } 30 | 31 | //Create the POSIT object with the model points 32 | positObject = cvCreatePOSITObject(&modelPoints[0], (int) modelPoints.size()); 33 | 34 | // intrinsics = cvCreateMat( 3, 3, CV_32F ); 35 | // cvSetZero( intrinsics ); 36 | // initializeIntrinsics( width, height ); 37 | // createOpenGLProjectionMatrix( width, height, nearPlane, farPlane ); 38 | for (int i = 0; i < 9; i++) { 39 | this->cameraMatrix[i] = cameraMatrix[i]; 40 | } 41 | //POSIT receives a single focal length 42 | // this->cameraMatrix[0]= FOCAL_LENGTH; 43 | // this->cameraMatrix[1*3+1]= FOCAL_LENGTH; 44 | this->cameraMatrix[1 * 3 + 1] = this->cameraMatrix[0]; 45 | } 46 | 47 | void transpose(float *in, int m, int n, float *out) { 48 | for (int i = 0; i < n; i++) { 49 | for (int j = 0; j < m; j++) { 50 | out[i * m + j] = in[j * n + i]; 51 | } 52 | } 53 | } 54 | 55 | void POSIT::poseEstimation() { 56 | // CvMatr32f rotation_matrix = new float[9]; 57 | // CvVect32f translation_vector = new float[3]; 58 | //set posit termination criteria: 100 max iterations, convergence epsilon 1.0e-5 59 | CvTermCriteria criteria = cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 100, 1.0e-4f); 60 | // cvPOSIT( positObject, &srcImagePoints[0], FOCAL_LENGTH, criteria, rotation_matrix, translation_vector ); 61 | cvPOSIT(positObject, &srcImagePoints[0], cameraMatrix[0], criteria, rotation_matrix, 62 | translation_vector); 63 | 64 | createOpenGLMatrixFrom(rotation_matrix, translation_vector); 65 | 66 | float rotation_matrix_trans[9]; 67 | transpose(rotation_matrix, 3, 3, rotation_matrix_trans); 68 | 69 | // RT= [R' T']; 70 | float RT[4][4]; 71 | for (int i = 0; i < 4; i++) { 72 | for (int j = 0; j < 4; j++) { 73 | RT[i][j] = 0; 74 | } 75 | } 76 | for (int i = 0; i < 3; i++) { 77 | for (int j = 0; j < 3; j++) { 78 | RT[i][j] = rotation_matrix_trans[i * 3 + j]; 79 | } 80 | } 81 | for (int j = 0; j < 3; j++) { 82 | RT[3][j] = translation_vector[j]; 83 | } 84 | RT[3][3] = 1; 85 | 86 | CvMat RTMatrix = cvMat(4, 4, CV_32F, RT); 87 | 88 | // shiftR is the translation matrix, inverting the shift we've done before calling POSIT 89 | float shiftR[4][4]; 90 | for (int i = 0; i < 4; i++) { 91 | for (int j = 0; j < 4; j++) { 92 | shiftR[i][j] = 0; 93 | } 94 | } 95 | shiftR[0][0] = shiftR[1][1] = shiftR[2][2] = shiftR[3][3] = 1; 96 | for (int j = 0; j < 3; j++) { 97 | shiftR[j][3] = -shift[j]; 98 | } 99 | CvMat shiftRMatrix = cvMat(4, 4, CV_32F, shiftR); 100 | 101 | // result = [R' T']*shiftR' 102 | float result[16]; 103 | CvMat resultMatrix = cvMat(4, 4, CV_32F, result); 104 | cvGEMM(&RTMatrix, &shiftRMatrix, 1.0, NULL, 0.0, &resultMatrix, CV_GEMM_A_T); 105 | 106 | // return R and T from result 107 | for (int i = 0; i < 3; i++) { 108 | for (int j = 0; j < 3; j++) { 109 | rotation_matrix[i * 3 + j] = result[i * 4 + j]; 110 | } 111 | } 112 | for (int j = 0; j < 3; j++) { 113 | translation_vector[j] = result[j * 4 + 3]; 114 | } 115 | // printMatrix(RT[0],4,4); 116 | // printMatrix(shiftR[0],4,4); 117 | // printMatrix(result,4,4); 118 | 119 | //Show the results 120 | #ifdef _DEBUG 121 | cout << "\n\n-......- POSE ESTIMATED -......-\n"; 122 | cout << "\n-.- MODEL POINTS -.-\n"; 123 | for ( size_t p=0; p &projectedPoints) { 168 | // The origin of the coordinates system is in the centre of the image 169 | projectedPoints.clear(); 170 | CvMat poseMatrix = cvMat(4, 4, CV_32F, pose); 171 | for (size_t p = 0; p < modelPoints.size(); p++) { 172 | float modelPoint[] = { modelPoints[p].x, modelPoints[p].y, modelPoints[p].z, 1.0f }; 173 | CvMat modelPointMatrix = cvMat(4, 1, CV_32F, modelPoint); 174 | float point3D[4]; 175 | CvMat point3DMatrix = cvMat(4, 1, CV_32F, point3D); 176 | cvGEMM(&poseMatrix, &modelPointMatrix, 1.0, NULL, 0.0, &point3DMatrix, CV_GEMM_A_T); 177 | 178 | //Project the transformed 3D points 179 | CvPoint2D32f point2D = cvPoint2D32f(0.0, 0.0); 180 | if (point3D[2] != 0) { 181 | point2D.x = cameraMatrix[0 * 3 + 0] * point3D[0] / point3D[2]; 182 | point2D.y = cameraMatrix[1 * 3 + 1] * point3D[1] / point3D[2]; 183 | } 184 | projectedPoints.push_back(point2D); 185 | } 186 | } 187 | 188 | -------------------------------------------------------------------------------- /calib/POSIT.h: -------------------------------------------------------------------------------- 1 | /**************************************** 2 | * * 3 | * OPENCV POSIT TUTORIAL * 4 | * Javier Barandiaran Martirena * 5 | * 2006 * 6 | * * 7 | ****************************************/ 8 | #pragma once 9 | 10 | #include 11 | #include 12 | //#include 13 | #include 14 | 15 | #define FOCAL_LENGTH 760.0 16 | 17 | class POSIT { 18 | public: 19 | POSIT(); 20 | virtual ~POSIT(); 21 | 22 | void initialize(double cameraMatrix[3 * 3], const double &aWidth, const double &aHeight, 23 | const double &nearPlane, const double &farPlane); 24 | 25 | void poseEstimation(); 26 | void 27 | createOpenGLMatrixFrom(const CvMatr32f &rotationMatrix, 28 | const CvVect32f &translationVector); 29 | void projectModelPoints(float *pose, std::vector &projectedPoints); 30 | 31 | double width, height; //Image size 32 | double cameraMatrix[3 * 3]; 33 | float posePOSIT[16]; 34 | float poseReal[16]; 35 | float projectionMatrix[16]; 36 | float shift[3]; 37 | 38 | std::vector modelPoints; 39 | std::vector srcImagePoints; 40 | std::vector estimatedImagePoints; 41 | 42 | CvPOSITObject* positObject; 43 | 44 | //mine 45 | float rotation_matrix[9]; 46 | float translation_vector[3]; 47 | 48 | }; 49 | -------------------------------------------------------------------------------- /calib/calib.cpp: -------------------------------------------------------------------------------- 1 | #include "calib.h" 2 | 3 | int cvStdErrReport2(int code, const char *func_name, const char *err_msg, const char *file, 4 | int line, void*) { 5 | // ostringstream s; 6 | // s << "OpenCV ERROR: " << cvErrorStr(code) << " (" << (err_msg ? err_msg : "no description") 7 | // << ")" << endl; 8 | // throw s.str().c_str(); 9 | sout << "OpenCV ERROR: " << cvErrorStr(code) << " (" << (err_msg ? err_msg : "no description") 10 | << ")" << endl; 11 | sout << (func_name ? func_name : "") << " " << (file != NULL ? file : "") << " " 12 | << line << endl; 13 | printf("OpenCV ERROR: %s (%s)\n\tin function ", cvErrorStr(code), err_msg ? err_msg 14 | : "no description"); 15 | printf("%s, %s(%d)\n", func_name ? func_name : "", file != NULL ? file : "", line); 16 | 17 | return 0; 18 | } 19 | 20 | double distanceMSE(int & numPoints, CvPoint2D64d *& imagePoints, double *& imgPointsIn) { 21 | double dist = 0; 22 | for (int i = 0; i < numPoints; i++) { 23 | double dist_x = (imagePoints + i)->x - imgPointsIn[i * TWO + 0]; 24 | double dist_y = (imagePoints + i)->y - imgPointsIn[i * TWO + 1]; 25 | dist += dist_x * dist_x + dist_y * dist_y; 26 | } 27 | return dist / numPoints; 28 | } 29 | 30 | // taken from OpenCV 1.1 31 | void _cvRQDecomp3x3(const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ, CvMat *matrixQx, 32 | CvMat *matrixQy, CvMat *matrixQz, CvPoint3D64f *eulerAngles) { 33 | 34 | double _M[3][3], _R[3][3], _Q[3][3]; 35 | CvMat M = cvMat(3, 3, CV_64F, _M); 36 | CvMat R = cvMat(3, 3, CV_64F, _R); 37 | CvMat Q = cvMat(3, 3, CV_64F, _Q); 38 | double z, c, s; 39 | 40 | /* Validate parameters. */ 41 | /* CV_ASSERT( CV_IS_MAT(matrixM) && CV_IS_MAT(matrixR) && CV_IS_MAT(matrixQ) && 42 | matrixM->cols == 3 && matrixM->rows == 3 && 43 | CV_ARE_SIZES_EQ(matrixM, matrixR) && CV_ARE_SIZES_EQ(matrixM, matrixQ)); 44 | */ 45 | cvConvert(matrixM, &M); 46 | 47 | { 48 | /* Find Givens rotation Q_x for x axis (left multiplication). */ 49 | /* 50 | ( 1 0 0 ) 51 | Qx = ( 0 c s ), c = m33/sqrt(m32^2 + m33^2), s = m32/sqrt(m32^2 + m33^2) 52 | ( 0 -s c ) 53 | */ 54 | s = _M[2][1]; 55 | c = _M[2][2]; 56 | z = 1. / sqrt(c * c + s * s + DBL_EPSILON); 57 | c *= z; 58 | s *= z; 59 | 60 | double _Qx[3][3] = { { 1, 0, 0 }, { 0, c, s }, { 0, -s, c } }; 61 | CvMat Qx = cvMat(3, 3, CV_64F, _Qx); 62 | 63 | cvMatMul(&M, &Qx, &R); 64 | assert(fabs(_R[2][1]) < FLT_EPSILON); 65 | _R[2][1] = 0; 66 | 67 | /* Find Givens rotation for y axis. */ 68 | /* 69 | ( c 0 s ) 70 | Qy = ( 0 1 0 ), c = m33/sqrt(m31^2 + m33^2), s = m31/sqrt(m31^2 + m33^2) 71 | (-s 0 c ) 72 | */ 73 | s = _R[2][0]; 74 | c = _R[2][2]; 75 | z = 1. / sqrt(c * c + s * s + DBL_EPSILON); 76 | c *= z; 77 | s *= z; 78 | 79 | double _Qy[3][3] = { { c, 0, s }, { 0, 1, 0 }, { -s, 0, c } }; 80 | CvMat Qy = cvMat(3, 3, CV_64F, _Qy); 81 | cvMatMul(&R, &Qy, &M); 82 | 83 | assert(fabs(_M[2][0]) < FLT_EPSILON); 84 | _M[2][0] = 0; 85 | 86 | /* Find Givens rotation for z axis. */ 87 | /* 88 | ( c s 0 ) 89 | Qz = (-s c 0 ), c = m22/sqrt(m21^2 + m22^2), s = m21/sqrt(m21^2 + m22^2) 90 | ( 0 0 1 ) 91 | */ 92 | 93 | s = _M[1][0]; 94 | c = _M[1][1]; 95 | z = 1. / sqrt(c * c + s * s + DBL_EPSILON); 96 | c *= z; 97 | s *= z; 98 | 99 | double _Qz[3][3] = { { c, s, 0 }, { -s, c, 0 }, { 0, 0, 1 } }; 100 | CvMat Qz = cvMat(3, 3, CV_64F, _Qz); 101 | 102 | cvMatMul(&M, &Qz, &R); 103 | assert(fabs(_R[1][0]) < FLT_EPSILON); 104 | _R[1][0] = 0; 105 | 106 | // Solve the decomposition ambiguity. 107 | // Diagonal entries of R, except the last one, shall be positive. 108 | // Further rotate R by 180 degree if necessary 109 | if (_R[0][0] < 0) { 110 | if (_R[1][1] < 0) { 111 | // rotate around z for 180 degree, i.e. a rotation matrix of 112 | // [-1, 0, 0], 113 | // [ 0, -1, 0], 114 | // [ 0, 0, 1] 115 | _R[0][0] *= -1; 116 | _R[0][1] *= -1; 117 | _R[1][1] *= -1; 118 | 119 | _Qz[0][0] *= -1; 120 | _Qz[0][1] *= -1; 121 | _Qz[1][0] *= -1; 122 | _Qz[1][1] *= -1; 123 | } else { 124 | // rotate around y for 180 degree, i.e. a rotation matrix of 125 | // [-1, 0, 0], 126 | // [ 0, 1, 0], 127 | // [ 0, 0, -1] 128 | _R[0][0] *= -1; 129 | _R[0][2] *= -1; 130 | _R[1][2] *= -1; 131 | _R[2][2] *= -1; 132 | 133 | cvTranspose(&Qz, &Qz); 134 | 135 | _Qy[0][0] *= -1; 136 | _Qy[0][2] *= -1; 137 | _Qy[2][0] *= -1; 138 | _Qy[2][2] *= -1; 139 | } 140 | } else if (_R[1][1] < 0) { 141 | // ??? for some reason, we never get here ??? 142 | 143 | // rotate around x for 180 degree, i.e. a rotation matrix of 144 | // [ 1, 0, 0], 145 | // [ 0, -1, 0], 146 | // [ 0, 0, -1] 147 | _R[0][1] *= -1; 148 | _R[0][2] *= -1; 149 | _R[1][1] *= -1; 150 | _R[1][2] *= -1; 151 | _R[2][2] *= -1; 152 | 153 | cvTranspose(&Qz, &Qz); 154 | cvTranspose(&Qy, &Qy); 155 | 156 | _Qx[1][1] *= -1; 157 | _Qx[1][2] *= -1; 158 | _Qx[2][1] *= -1; 159 | _Qx[2][2] *= -1; 160 | } 161 | 162 | // calculate the euler angle 163 | if (eulerAngles) { 164 | eulerAngles->x = acos(_Qx[1][1]) * (_Qx[1][2] >= 0 ? 1 : -1) * (180.0 / CV_PI); 165 | eulerAngles->y = acos(_Qy[0][0]) * (_Qy[0][2] >= 0 ? 1 : -1) * (180.0 / CV_PI); 166 | eulerAngles->z = acos(_Qz[0][0]) * (_Qz[0][1] >= 0 ? 1 : -1) * (180.0 / CV_PI); 167 | } 168 | 169 | /* Calulate orthogonal matrix. */ 170 | /* 171 | Q = QzT * QyT * QxT 172 | */ 173 | cvGEMM(&Qz, &Qy, 1, 0, 0, &M, CV_GEMM_A_T + CV_GEMM_B_T); 174 | cvGEMM(&M, &Qx, 1, 0, 0, &Q, CV_GEMM_B_T); 175 | 176 | /* Save R and Q matrices. */ 177 | cvConvert( &R, matrixR ); 178 | cvConvert( &Q, matrixQ ); 179 | 180 | if (matrixQx) 181 | cvConvert(&Qx, matrixQx); 182 | if (matrixQy) 183 | cvConvert(&Qy, matrixQy); 184 | if (matrixQz) 185 | cvConvert(&Qz, matrixQz); 186 | } 187 | 188 | } 189 | 190 | void doPOSIT(double cameraMatrix[3 * 3], CvPoint3D64d *& objectPoints, int & numPoints, 191 | CvPoint2D64d *& imagePoints, int & width, int & height, double rotMatrs[3 * 3], 192 | double transVects[3]) { 193 | double shiftI[2]; 194 | shiftI[0] = cameraMatrix[2]; //width/2; 195 | shiftI[1] = cameraMatrix[5]; //height/2; 196 | std::vector modelPoints; 197 | std::vector srcImagePoints; 198 | for (int i = 0; i < numPoints; i++) { 199 | modelPoints.push_back(cvPoint3D32f((objectPoints + i)->x, (objectPoints + i)->y, 200 | (objectPoints + i)->z)); 201 | srcImagePoints.push_back(cvPoint2D32f((imagePoints + i)->x - shiftI[0], 202 | (imagePoints + i)->y - shiftI[1])); 203 | } 204 | POSIT posit; 205 | posit.modelPoints = modelPoints; 206 | posit.srcImagePoints = srcImagePoints; 207 | posit.initialize(cameraMatrix, width, height, 0.001, 10000); 208 | posit.poseEstimation(); 209 | posit.projectModelPoints(posit.posePOSIT, posit.estimatedImagePoints); 210 | sout << "POSIT matrix:" << endl; 211 | printMatrix(posit.posePOSIT, 4, 4); 212 | sout << "POSIT reprojection:" << endl; 213 | for (vector::const_iterator vi = posit.estimatedImagePoints.begin(); vi 214 | != posit.estimatedImagePoints.end(); vi++) { 215 | CvPoint2D32f pt = *vi; 216 | sout << pt.x + shiftI[0] << tab << pt.y + shiftI[1] << endl; 217 | } 218 | 219 | //POSIT uses same focal length for x and y 220 | cameraMatrix[4] = cameraMatrix[0]; 221 | // cameraMatrix[1]=cameraMatrix[2]=cameraMatrix[3]=cameraMatrix[5]=cameraMatrix[6]=cameraMatrix[7]=0; 222 | cameraMatrix[8] = 1; 223 | 224 | for (int i = 0; i < 9; i++) { 225 | rotMatrs[i] = posit.rotation_matrix[i]; 226 | } 227 | 228 | for (int i = 0; i < 3; i++) { 229 | transVects[i] = posit.translation_vector[i]; 230 | } 231 | } 232 | 233 | 234 | void run(int width, int height, int numPoints, double *imgPointsIn, double *objPointsIn, 235 | double *AOutput, double *ROutput, double *TOutput, double *Ain, double max_mse, 236 | bool usePosit, bool onlyExtrinsic, int useExtrinsicGuess, double *Rin, double *Tin) { 237 | cvRedirectError(cvStdErrReport2); 238 | // cvRedirectError(cvNulDevReport); 239 | cvSetErrMode(CV_ErrModeParent); 240 | if (imgPointsIn == NULL || objPointsIn == NULL || numPoints == 0) { 241 | return; 242 | } 243 | CvSize image_size; 244 | image_size.width = width; 245 | image_size.height = height; 246 | CvPoint2D64d* imagePoints; 247 | CvPoint3D64d* objectPoints; 248 | CvPoint2D64d* reprojectPoints; 249 | 250 | double transVects[3]; 251 | double rotMatrs[3 * 3]; 252 | double cameraMatrix[3 * 3]; 253 | for (unsigned int i = 0; i < TRIPLET * TRIPLET; i++) { 254 | cameraMatrix[i] = Ain[i]; 255 | } 256 | // memset(cameraMatrix, 0, 9 * sizeof(cameraMatrix[0])); 257 | if (useExtrinsicGuess == 1) { 258 | for (unsigned int i = 0; i < TRIPLET * TRIPLET; i++) { 259 | rotMatrs[i] = Rin[i]; 260 | } 261 | for (unsigned int i = 0; i < TRIPLET; i++) { 262 | transVects[i] = Tin[i]; 263 | } 264 | } 265 | double distortion[4] = {0, 0, 0, 0}; 266 | 267 | int calibFlags; 268 | 269 | imagePoints = 0; 270 | objectPoints = 0; 271 | reprojectPoints = 0; 272 | 273 | /* Need to allocate memory */ 274 | imagePoints = (CvPoint2D64d*) cvAlloc(numPoints * sizeof(CvPoint2D64d)); 275 | 276 | objectPoints = (CvPoint3D64d*) cvAlloc(numPoints * sizeof(CvPoint3D64d)); 277 | 278 | /* Alloc memory for numbers */ 279 | int *numbers = (int*) cvAlloc(sizeof(int)); 280 | numbers[0] = numPoints; 281 | 282 | sout << "input points (x,y) and (X,Y,Z):" << endl; 283 | for (int i = 0; i < numPoints; i++) { 284 | (imagePoints + i)->x = imgPointsIn[i * TWO]; 285 | (imagePoints + i)->y = imgPointsIn[i * TWO + 1]; 286 | (objectPoints + i)->x = objPointsIn[i * TRIPLET]; 287 | (objectPoints + i)->y = objPointsIn[i * TRIPLET + 1]; 288 | (objectPoints + i)->z = objPointsIn[i * TRIPLET + 2]; 289 | if (DEBUG) { 290 | sout << (imagePoints + i)->x << tab << (imagePoints + i)->y << tab 291 | << (objectPoints + i)->x << tab << (objectPoints + i)->y << tab 292 | << (objectPoints + i)->z << endl; 293 | 294 | } 295 | } 296 | calibFlags = CV_CALIB_FIX_PRINCIPAL_POINT + CV_CALIB_ZERO_TANGENT_DIST 297 | + CV_CALIB_FIX_ASPECT_RATIO + CV_CALIB_USE_INTRINSIC_GUESS; 298 | 299 | CvMat point_counts = cvMat(1, 1, CV_32SC1, numbers); 300 | CvMat image_points, object_points; 301 | CvMat dist_coeffs = cvMat(4, 1, CV_64FC1, distortion); 302 | CvMat camera_matrix = cvMat(3, 3, CV_64FC1, cameraMatrix); 303 | CvMat rotation_matrices = cvMat(1, 9, CV_64FC1, rotMatrs); 304 | // double rotMatrsc[3]; 305 | // for (int i = 0; i < 3; i++) { 306 | // rotMatrsc[i] = 0; 307 | // } 308 | // CvMat rotation_matrices = cvMat(1, 3, CV_64FC1, rotMatrsc); 309 | CvMat translation_vectors = cvMat(1, 3, CV_64FC1, transVects); 310 | 311 | image_points = cvMat(numPoints, 1, CV_64FC2, imagePoints); 312 | object_points = cvMat(numPoints, 1, CV_64FC3, objectPoints); 313 | 314 | if (usePosit) { 315 | sout << "using POSIT." << endl; 316 | doPOSIT(cameraMatrix, objectPoints, numPoints, imagePoints, width, height, rotMatrs, 317 | transVects); 318 | } else if (onlyExtrinsic) { 319 | CvMat rotation_matrices2 = cvMat(3, 3, CV_64FC1, rotMatrs); 320 | 321 | double tmp[3]; 322 | CvMat rotation_matrices_tmp = cvMat(1, 3, CV_64FC1, tmp); 323 | cvRodrigues2(&rotation_matrices2, &rotation_matrices_tmp); 324 | sout << "using cvFindExtrinsic." << endl; 325 | cvFindExtrinsicCameraParams2(&object_points, &image_points, &camera_matrix, &dist_coeffs, 326 | &rotation_matrices_tmp, &translation_vectors, useExtrinsicGuess); 327 | //convert the result and store in rotMatrs 328 | cvRodrigues2(&rotation_matrices_tmp, &rotation_matrices2); 329 | } else { 330 | sout << "using cvCalibrate." << endl; 331 | cvCalibrateCamera2(&object_points, &image_points, &point_counts, image_size, 332 | &camera_matrix, &dist_coeffs, &rotation_matrices, &translation_vectors, calibFlags); 333 | } 334 | 335 | bool err = false; 336 | try { 337 | if (cvGetErrStatus() != CV_StsOk) { 338 | throw "Error"; 339 | } 340 | 341 | if (AOutput != NULL && ROutput != NULL && TOutput != NULL) { 342 | transpose(cameraMatrix, 3, 3, AOutput); 343 | transpose(rotMatrs, 3, 3, ROutput); 344 | transpose(transVects, 1, 3, TOutput); 345 | } 346 | if (DEBUG) { 347 | 348 | sout << "cameraMatrix:" << endl; 349 | printMatrix(cameraMatrix, 3, 3); 350 | sout << "distortion:" << endl; 351 | printMatrix(distortion, 1, 4); 352 | sout << "rotation:" << endl; 353 | printMatrix(rotMatrs, 3, 3); 354 | sout << "translation:" << endl; 355 | printMatrix(transVects, 1, 3); 356 | 357 | rotation_matrices = cvMat(3, 3, CV_64FC1, rotMatrs); 358 | for (int i = 0; i < numPoints; i++) { 359 | (imagePoints + i)->x = 0; 360 | (imagePoints + i)->y = 0; 361 | } 362 | image_points = cvMat(numPoints, 1, CV_64FC2, imagePoints); 363 | cvProjectPoints2(&object_points, &rotation_matrices, &translation_vectors, 364 | &camera_matrix, &dist_coeffs, &image_points); 365 | if (cvGetErrStatus() != CV_StsOk) { 366 | throw "Error"; 367 | } 368 | sout << "reprojection (with distortion):" << endl; 369 | for (int i = 0; i < numPoints; i++) { 370 | sout << (imagePoints + i)->x << tab << (imagePoints + i)->y << endl; 371 | } 372 | 373 | for (int i = 0; i < 4; i++) { 374 | distortion[i] = 0; 375 | } 376 | dist_coeffs = cvMat(4, 1, CV_64FC1, distortion); 377 | 378 | for (int i = 0; i < numPoints; i++) { 379 | (imagePoints + i)->x = 0; 380 | (imagePoints + i)->y = 0; 381 | } 382 | image_points = cvMat(numPoints, 1, CV_64FC2, imagePoints); 383 | cvProjectPoints2(&object_points, &rotation_matrices, &translation_vectors, 384 | &camera_matrix, &dist_coeffs, &image_points); 385 | if (cvGetErrStatus() != CV_StsOk) { 386 | throw "Error"; 387 | } 388 | sout << "reprojection (without distortion):" << endl; 389 | for (int i = 0; i < numPoints; i++) { 390 | sout << (imagePoints + i)->x << tab << (imagePoints + i)->y << endl; 391 | } 392 | 393 | double mse = distanceMSE(numPoints, imagePoints, imgPointsIn); 394 | sout << "mean square error: " << mse << endl; 395 | if (max_mse > 0 && mse > max_mse) { 396 | throw string("mean square error is bigger than max_mse: " + toString(max_mse)).c_str(); 397 | } 398 | 399 | double matrixR_[9]; 400 | CvMat matrixR = cvMat(3, 3, CV_64FC1, matrixR_); 401 | double matrixQ_[9]; 402 | CvMat matrixQ = cvMat(3, 3, CV_64FC1, matrixQ_); 403 | double matrixQx_[9]; 404 | CvMat matrixQx = cvMat(3, 3, CV_64FC1, matrixQx_); 405 | double matrixQy_[9]; 406 | CvMat matrixQy = cvMat(3, 3, CV_64FC1, matrixQy_); 407 | double matrixQz_[9]; 408 | CvMat matrixQz = cvMat(3, 3, CV_64FC1, matrixQz_); 409 | CvPoint3D64f eulerAngles; 410 | _cvRQDecomp3x3(&rotation_matrices, &matrixR, &matrixQ, &matrixQx, &matrixQy, &matrixQz, 411 | &eulerAngles); 412 | if (cvGetErrStatus() != CV_StsOk) { 413 | throw "Error"; 414 | } 415 | sout << "R" << endl; 416 | printMatrix(matrixR_, 3, 3); 417 | sout << "Q" << endl; 418 | printMatrix(matrixQ_, 3, 3); 419 | sout << "Qx" << endl; 420 | printMatrix(matrixQx_, 3, 3); 421 | sout << "Qy" << endl; 422 | printMatrix(matrixQy_, 3, 3); 423 | sout << "Qz" << endl; 424 | printMatrix(matrixQz_, 3, 3); 425 | sout << "euler angles:" << endl; 426 | sout << eulerAngles.x << tab << eulerAngles.y << tab << eulerAngles.z << endl; 427 | } 428 | } catch (const char *e) { 429 | printf(e); 430 | printf("\n"); 431 | sout << e << endl; 432 | err = true; 433 | // sout< 5 | #include 6 | #include 7 | #include 8 | #include "util/calib_util.h" 9 | #include 10 | #include 11 | #include "POSIT.h" 12 | 13 | using namespace std; 14 | 15 | const string programName = "calib"; 16 | 17 | int cvStdErrReport2(int code, const char *func_name, const char *err_msg, const char *file, int line, void*); 18 | double distanceMSE(int & numPoints, CvPoint2D64d *& imagePoints, double *& imgPointsIn); 19 | 20 | void _cvRQDecomp3x3(const CvMat *matrixM, 21 | CvMat *matrixR, 22 | CvMat *matrixQ, 23 | CvMat *matrixQx, 24 | CvMat *matrixQy, 25 | CvMat *matrixQz, 26 | CvPoint3D64f *eulerAngles); 27 | 28 | void doPOSIT(double cameraMatrix[3 * 3], 29 | CvPoint3D64d *& objectPoints, 30 | int & numPoints, 31 | CvPoint2D64d *& imagePoints, 32 | int & width, 33 | int & height, 34 | double rotMatrs[3 * 3], 35 | double transVects[3]); 36 | 37 | void run(int width, int height, int numPoints, double *imgPointsIn, double *objPointsIn, 38 | double *AOutput, double *ROutput, double *TOutput, double *Ain, double max_mse, 39 | bool usePosit, bool onlyExtrinsic, int useExtrinsicGuess, double *Rin, double *Tin); 40 | 41 | void shutdown(double *imgPoints, double *objPoints); 42 | 43 | #endif // CALIB_H 44 | -------------------------------------------------------------------------------- /calib/pro/Makefile: -------------------------------------------------------------------------------- 1 | ############################################################################# 2 | # Makefile for building: pro 3 | # Generated by qmake (2.01a) (Qt 4.8.1) on: Wed Dec 24 08:44:32 2014 4 | # Project: pro.pro 5 | # Template: app 6 | # Command: /usr/bin/qmake -o Makefile pro.pro 7 | ############################################################################# 8 | 9 | ####### Compiler, tools and options 10 | 11 | CC = gcc 12 | CXX = g++ 13 | DEFINES = -DQT_WEBKIT -DQT_NO_DEBUG -DQT_GUI_LIB -DQT_CORE_LIB -DQT_SHARED 14 | CFLAGS = -m64 -pipe -O2 -Wall -W -D_REENTRANT $(DEFINES) 15 | CXXFLAGS = -m64 -pipe -O2 -Wall -W -D_REENTRANT $(DEFINES) 16 | INCPATH = -I/usr/share/qt4/mkspecs/linux-g++-64 -I. -I/usr/include/qt4/QtCore -I/usr/include/qt4/QtGui -I/usr/include/qt4 -I. -I../util -I.. -I/usr/local/include/opencv -I-I/usr/local/include -I. 17 | LINK = g++ 18 | LFLAGS = -m64 -Wl,-O1 19 | LIBS = $(SUBLIBS) -L/usr/lib/x86_64-linux-gnu -L/usr/local/lib -lopencv_calib3d -lopencv_contrib -lopencv_core -lopencv_features2d -lopencv_flann -lopencv_highgui -lopencv_imgproc -lopencv_legacy -lopencv_ml -lopencv_nonfree -lopencv_objdetect -lopencv_photo -lopencv_stitching -lopencv_superres -lopencv_ts -lopencv_video -lopencv_videostab -lQtGui -lQtCore -lpthread 20 | AR = ar cqs 21 | RANLIB = 22 | QMAKE = /usr/bin/qmake 23 | TAR = tar -cf 24 | COMPRESS = gzip -9f 25 | COPY = cp -f 26 | SED = sed 27 | COPY_FILE = $(COPY) 28 | COPY_DIR = $(COPY) -r 29 | STRIP = strip 30 | INSTALL_FILE = install -m 644 -p 31 | INSTALL_DIR = $(COPY_DIR) 32 | INSTALL_PROGRAM = install -m 755 -p 33 | DEL_FILE = rm -f 34 | SYMLINK = ln -f -s 35 | DEL_DIR = rmdir 36 | MOVE = mv -f 37 | CHK_DIR_EXISTS= test -d 38 | MKDIR = mkdir -p 39 | 40 | ####### Output directory 41 | 42 | OBJECTS_DIR = ./ 43 | 44 | ####### Files 45 | 46 | SOURCES = ../calib.cpp \ 47 | ../POSIT.cpp \ 48 | ../util/util.cpp 49 | OBJECTS = calib.o \ 50 | POSIT.o \ 51 | util.o 52 | DIST = /usr/share/qt4/mkspecs/common/unix.conf \ 53 | /usr/share/qt4/mkspecs/common/linux.conf \ 54 | /usr/share/qt4/mkspecs/common/gcc-base.conf \ 55 | /usr/share/qt4/mkspecs/common/gcc-base-unix.conf \ 56 | /usr/share/qt4/mkspecs/common/g++-base.conf \ 57 | /usr/share/qt4/mkspecs/common/g++-unix.conf \ 58 | /usr/share/qt4/mkspecs/qconfig.pri \ 59 | /usr/share/qt4/mkspecs/modules/qt_webkit_version.pri \ 60 | /usr/share/qt4/mkspecs/features/qt_functions.prf \ 61 | /usr/share/qt4/mkspecs/features/qt_config.prf \ 62 | /usr/share/qt4/mkspecs/features/exclusive_builds.prf \ 63 | /usr/share/qt4/mkspecs/features/default_pre.prf \ 64 | /usr/share/qt4/mkspecs/features/release.prf \ 65 | /usr/share/qt4/mkspecs/features/default_post.prf \ 66 | /usr/share/qt4/mkspecs/features/unix/gdb_dwarf_index.prf \ 67 | /usr/share/qt4/mkspecs/features/warn_on.prf \ 68 | /usr/share/qt4/mkspecs/features/qt.prf \ 69 | /usr/share/qt4/mkspecs/features/unix/thread.prf \ 70 | /usr/share/qt4/mkspecs/features/moc.prf \ 71 | /usr/share/qt4/mkspecs/features/resources.prf \ 72 | /usr/share/qt4/mkspecs/features/uic.prf \ 73 | /usr/share/qt4/mkspecs/features/yacc.prf \ 74 | /usr/share/qt4/mkspecs/features/lex.prf \ 75 | /usr/share/qt4/mkspecs/features/include_source_dir.prf \ 76 | pro.pro 77 | QMAKE_TARGET = pro 78 | DESTDIR = 79 | TARGET = pro 80 | 81 | first: all 82 | ####### Implicit rules 83 | 84 | .SUFFIXES: .o .c .cpp .cc .cxx .C 85 | 86 | .cpp.o: 87 | $(CXX) -c $(CXXFLAGS) $(INCPATH) -o "$@" "$<" 88 | 89 | .cc.o: 90 | $(CXX) -c $(CXXFLAGS) $(INCPATH) -o "$@" "$<" 91 | 92 | .cxx.o: 93 | $(CXX) -c $(CXXFLAGS) $(INCPATH) -o "$@" "$<" 94 | 95 | .C.o: 96 | $(CXX) -c $(CXXFLAGS) $(INCPATH) -o "$@" "$<" 97 | 98 | .c.o: 99 | $(CC) -c $(CFLAGS) $(INCPATH) -o "$@" "$<" 100 | 101 | ####### Build rules 102 | 103 | all: Makefile $(TARGET) 104 | 105 | $(TARGET): $(OBJECTS) 106 | $(LINK) $(LFLAGS) -o $(TARGET) $(OBJECTS) $(OBJCOMP) $(LIBS) 107 | 108 | Makefile: pro.pro /usr/share/qt4/mkspecs/linux-g++-64/qmake.conf /usr/share/qt4/mkspecs/common/unix.conf \ 109 | /usr/share/qt4/mkspecs/common/linux.conf \ 110 | /usr/share/qt4/mkspecs/common/gcc-base.conf \ 111 | /usr/share/qt4/mkspecs/common/gcc-base-unix.conf \ 112 | /usr/share/qt4/mkspecs/common/g++-base.conf \ 113 | /usr/share/qt4/mkspecs/common/g++-unix.conf \ 114 | /usr/share/qt4/mkspecs/qconfig.pri \ 115 | /usr/share/qt4/mkspecs/modules/qt_webkit_version.pri \ 116 | /usr/share/qt4/mkspecs/features/qt_functions.prf \ 117 | /usr/share/qt4/mkspecs/features/qt_config.prf \ 118 | /usr/share/qt4/mkspecs/features/exclusive_builds.prf \ 119 | /usr/share/qt4/mkspecs/features/default_pre.prf \ 120 | /usr/share/qt4/mkspecs/features/release.prf \ 121 | /usr/share/qt4/mkspecs/features/default_post.prf \ 122 | /usr/share/qt4/mkspecs/features/unix/gdb_dwarf_index.prf \ 123 | /usr/share/qt4/mkspecs/features/warn_on.prf \ 124 | /usr/share/qt4/mkspecs/features/qt.prf \ 125 | /usr/share/qt4/mkspecs/features/unix/thread.prf \ 126 | /usr/share/qt4/mkspecs/features/moc.prf \ 127 | /usr/share/qt4/mkspecs/features/resources.prf \ 128 | /usr/share/qt4/mkspecs/features/uic.prf \ 129 | /usr/share/qt4/mkspecs/features/yacc.prf \ 130 | /usr/share/qt4/mkspecs/features/lex.prf \ 131 | /usr/share/qt4/mkspecs/features/include_source_dir.prf \ 132 | /usr/lib/x86_64-linux-gnu/libQtGui.prl \ 133 | /usr/lib/x86_64-linux-gnu/libQtCore.prl 134 | $(QMAKE) -o Makefile pro.pro 135 | /usr/share/qt4/mkspecs/common/unix.conf: 136 | /usr/share/qt4/mkspecs/common/linux.conf: 137 | /usr/share/qt4/mkspecs/common/gcc-base.conf: 138 | /usr/share/qt4/mkspecs/common/gcc-base-unix.conf: 139 | /usr/share/qt4/mkspecs/common/g++-base.conf: 140 | /usr/share/qt4/mkspecs/common/g++-unix.conf: 141 | /usr/share/qt4/mkspecs/qconfig.pri: 142 | /usr/share/qt4/mkspecs/modules/qt_webkit_version.pri: 143 | /usr/share/qt4/mkspecs/features/qt_functions.prf: 144 | /usr/share/qt4/mkspecs/features/qt_config.prf: 145 | /usr/share/qt4/mkspecs/features/exclusive_builds.prf: 146 | /usr/share/qt4/mkspecs/features/default_pre.prf: 147 | /usr/share/qt4/mkspecs/features/release.prf: 148 | /usr/share/qt4/mkspecs/features/default_post.prf: 149 | /usr/share/qt4/mkspecs/features/unix/gdb_dwarf_index.prf: 150 | /usr/share/qt4/mkspecs/features/warn_on.prf: 151 | /usr/share/qt4/mkspecs/features/qt.prf: 152 | /usr/share/qt4/mkspecs/features/unix/thread.prf: 153 | /usr/share/qt4/mkspecs/features/moc.prf: 154 | /usr/share/qt4/mkspecs/features/resources.prf: 155 | /usr/share/qt4/mkspecs/features/uic.prf: 156 | /usr/share/qt4/mkspecs/features/yacc.prf: 157 | /usr/share/qt4/mkspecs/features/lex.prf: 158 | /usr/share/qt4/mkspecs/features/include_source_dir.prf: 159 | /usr/lib/x86_64-linux-gnu/libQtGui.prl: 160 | /usr/lib/x86_64-linux-gnu/libQtCore.prl: 161 | qmake: FORCE 162 | @$(QMAKE) -o Makefile pro.pro 163 | 164 | dist: 165 | @$(CHK_DIR_EXISTS) .tmp/pro1.0.0 || $(MKDIR) .tmp/pro1.0.0 166 | $(COPY_FILE) --parents $(SOURCES) $(DIST) .tmp/pro1.0.0/ && $(COPY_FILE) --parents ../POSIT.h ../util/util.h .tmp/pro1.0.0/ && $(COPY_FILE) --parents ../calib.cpp ../POSIT.cpp ../util/util.cpp .tmp/pro1.0.0/ && (cd `dirname .tmp/pro1.0.0` && $(TAR) pro1.0.0.tar pro1.0.0 && $(COMPRESS) pro1.0.0.tar) && $(MOVE) `dirname .tmp/pro1.0.0`/pro1.0.0.tar.gz . && $(DEL_FILE) -r .tmp/pro1.0.0 167 | 168 | 169 | clean:compiler_clean 170 | -$(DEL_FILE) $(OBJECTS) 171 | -$(DEL_FILE) *~ core *.core 172 | 173 | 174 | ####### Sub-libraries 175 | 176 | distclean: clean 177 | -$(DEL_FILE) $(TARGET) 178 | -$(DEL_FILE) Makefile 179 | 180 | 181 | check: first 182 | 183 | mocclean: compiler_moc_header_clean compiler_moc_source_clean 184 | 185 | mocables: compiler_moc_header_make_all compiler_moc_source_make_all 186 | 187 | compiler_moc_header_make_all: 188 | compiler_moc_header_clean: 189 | compiler_rcc_make_all: 190 | compiler_rcc_clean: 191 | compiler_image_collection_make_all: qmake_image_collection.cpp 192 | compiler_image_collection_clean: 193 | -$(DEL_FILE) qmake_image_collection.cpp 194 | compiler_moc_source_make_all: 195 | compiler_moc_source_clean: 196 | compiler_uic_make_all: 197 | compiler_uic_clean: 198 | compiler_yacc_decl_make_all: 199 | compiler_yacc_decl_clean: 200 | compiler_yacc_impl_make_all: 201 | compiler_yacc_impl_clean: 202 | compiler_lex_make_all: 203 | compiler_lex_clean: 204 | compiler_clean: 205 | 206 | ####### Compile 207 | 208 | calib.o: ../calib.cpp ../util/util.h \ 209 | ../POSIT.h 210 | $(CXX) -c $(CXXFLAGS) $(INCPATH) -o calib.o ../calib.cpp 211 | 212 | POSIT.o: ../POSIT.cpp ../POSIT.h 213 | $(CXX) -c $(CXXFLAGS) $(INCPATH) -o POSIT.o ../POSIT.cpp 214 | 215 | util.o: ../util/util.cpp ../util/util.h 216 | $(CXX) -c $(CXXFLAGS) $(INCPATH) -o util.o ../util/util.cpp 217 | 218 | ####### Install 219 | 220 | install: FORCE 221 | 222 | uninstall: FORCE 223 | 224 | FORCE: 225 | 226 | -------------------------------------------------------------------------------- /calib/pro/POSIT.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shaoguangcheng/faceFrontalization/c5ba11e8a1ebcb47c43e8cb70e7e69b105de8ed7/calib/pro/POSIT.o -------------------------------------------------------------------------------- /calib/pro/calib.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shaoguangcheng/faceFrontalization/c5ba11e8a1ebcb47c43e8cb70e7e69b105de8ed7/calib/pro/calib.o -------------------------------------------------------------------------------- /calib/pro/pro: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shaoguangcheng/faceFrontalization/c5ba11e8a1ebcb47c43e8cb70e7e69b105de8ed7/calib/pro/pro -------------------------------------------------------------------------------- /calib/pro/pro.pro: -------------------------------------------------------------------------------- 1 | ###################################################################### 2 | # Automatically generated by qmake (2.01a) Wed Dec 24 08:43:53 2014 3 | ###################################################################### 4 | 5 | TEMPLATE = app 6 | TARGET = 7 | DEPENDPATH += . .. ../util 8 | INCLUDEPATH += . ../util .. 9 | 10 | # Input 11 | HEADERS += ../POSIT.h ../util/util.h 12 | SOURCES += ../calib.cpp ../POSIT.cpp ../util/util.cpp 13 | 14 | INCLUDEPATH += /usr/local/include/opencv -I/usr/local/include 15 | 16 | LIBS += -L/usr/local/lib -lopencv_calib3d -lopencv_contrib -lopencv_core -lopencv_features2d\ 17 | -lopencv_flann -lopencv_highgui -lopencv_imgproc -lopencv_legacy -lopencv_ml -lopencv_nonfree \ 18 | -lopencv_objdetect -lopencv_photo -lopencv_stitching -lopencv_superres -lopencv_ts -lopencv_video -lopencv_videostab 19 | -------------------------------------------------------------------------------- /calib/pro/pro.pro~: -------------------------------------------------------------------------------- 1 | ###################################################################### 2 | # Automatically generated by qmake (2.01a) Wed Dec 24 08:43:53 2014 3 | ###################################################################### 4 | 5 | TEMPLATE = app 6 | TARGET = 7 | DEPENDPATH += . .. ../util 8 | INCLUDEPATH += . ../util .. 9 | 10 | # Input 11 | HEADERS += ../POSIT.h ../util/util.h 12 | SOURCES += ../calib.cpp ../POSIT.cpp ../util/util.cpp 13 | -------------------------------------------------------------------------------- /calib/pro/util.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shaoguangcheng/faceFrontalization/c5ba11e8a1ebcb47c43e8cb70e7e69b105de8ed7/calib/pro/util.o -------------------------------------------------------------------------------- /calib/util/calib_util.cpp: -------------------------------------------------------------------------------- 1 | #include "calib_util.h" 2 | 3 | ostringstream sout(ostringstream::out); 4 | 5 | 6 | ostream& tab(ostream& output) 7 | { 8 | return output << '\t'; 9 | } 10 | 11 | void printMatrix(double *in, int m, int n) 12 | { 13 | for (int i = 0;i < m;i++) { 14 | for (int j = 0;j < n;j++) { 15 | sout << in[i*n+j] << tab; 16 | // sout << in[i*n+j] << ","; 17 | } 18 | 19 | sout << ";" << endl; 20 | } 21 | sout < 5 | #include 6 | //#include 7 | 8 | //#define printf mexPrintf 9 | 10 | using namespace std; 11 | 12 | //const bool DEBUG = true; 13 | const bool DEBUG = false; 14 | 15 | const unsigned int TWO = 2; 16 | const unsigned int TRIPLET = 3; 17 | const unsigned int QUADLET = 4; 18 | 19 | //debug output 20 | extern ostringstream sout; 21 | 22 | template < class T > 23 | string toString(const T &arg) 24 | { 25 | ostringstream out; 26 | 27 | out << arg; 28 | 29 | return (out.str()); 30 | } 31 | 32 | ostream& tab(ostream& output); 33 | 34 | void printMatrix(double *in, int m, int n); 35 | void printMatrix(float *in, int m, int n); 36 | 37 | void transposeAndFlipY(double *in, int m, int n, double *out); 38 | 39 | void transpose(double *in, int m, int n, double *out); 40 | 41 | void transpose3dim(unsigned char *image, int gWidth , int gHeight,unsigned char *imageOutput); 42 | void transpose3dimBGR(unsigned char *image, int gWidth , int gHeight,unsigned char *imageOutput); 43 | 44 | void getOpenGLMatrices(double *A, double *R, double *T, int width, int height, double mv[16], 45 | double projectionMatrix[16]); 46 | 47 | void getCameraMatricesFromOpenGL(double *A, double *R, double *T, int width, int height, 48 | double mv[16], double projectionMatrix[16]); 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /data/cameraMatrix.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | # calculated extrinsic Matrix of input image for test 3 | extrinsicMatrix: !!opencv-matrix 4 | rows: 3 5 | cols: 4 6 | dt: f 7 | data: [503.941932339908,118.085942638252,8.06973766667332,158431.711937862,16.4582457780299,179.710463667756,-485.180540705709,141616.165506840,0.0835271695546185,0.995626810060849,0.0418385830813235,809.815726578722] -------------------------------------------------------------------------------- /data/facialFeaturePoint.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | # facial feature point detected in input image (just for test) 3 | facialFeaturePoint: !!opencv-matrix 4 | rows: 49 5 | cols: 2 6 | dt: f 7 | data: [168.37146,130.16231,176.5083,129.45667,184.45398,131.14912,191.61346,133.18173,198.35126,135.88077,216.64844,134.49539,221.96793,132.42435,227.35556,130.74057,233.258,129.30713,239.41179,130.12273,208.82248,140.94389,210.46445,149.3075,211.95811,157.70355,213.30255,166.20233,197.90607,170.98959,204.52997,173.52286,211.83658,174.83069,216.85751,173.54343,221.30455,171.10268,174.78815,142.90129,180.67853,141.57053,187.05582,141.617,191.91983,143.43613,186.35339,144.47327,180.44705,144.54059,217.89557,142.58823,223.30064,140.68066,229.60941,140.81714,234.97939,143.00777,229.88396,143.98828,223.83194,143.74844,182.20935,185.49318,190.05634,180.92296,199.91772,179.84517,209.16241,180.79552,215.83176,180.15324,222.01828,181.18803,226.65286,185.22455,223.01099,194.84357,216.42622,201.92967,206.71686,204.02162,195.86269,201.85556,187.4185,195.15013,195.88486,183.57043,208.31128,184.06674,217.4205,183.82549,217.8656,194.48221,207.3927,197.17383,194.2406,194.92339] -------------------------------------------------------------------------------- /data/test.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shaoguangcheng/faceFrontalization/c5ba11e8a1ebcb47c43e8cb70e7e69b105de8ed7/data/test.png -------------------------------------------------------------------------------- /frontalUtil.cpp: -------------------------------------------------------------------------------- 1 | #include "frontalUtil.h" 2 | 3 | void read3DModelFromYML(const std::string & fileName, model3D& model) 4 | { 5 | FileStorage fs(fileName,FileStorage::READ); 6 | 7 | if(!fs.isOpened()){ 8 | string msg = "open " + fileName + " error"; 9 | DEBUGMSG(msg); 10 | return; 11 | } 12 | 13 | // read refU from YML 14 | string refUDataFile = static_cast(fs["refUFile"]); 15 | int refURows = static_cast(fs["refURows"]); 16 | int refUCols = static_cast(fs["refUCols"]); 17 | 18 | model.refU.create(refURows, refUCols); 19 | readMatFromFile(refUDataFile, model.refU); 20 | 21 | fs["refXY"] >> model.refXY; 22 | fs["threedee"] >> model.threedee; 23 | fs["outA"] >> model.outA; 24 | model.sizeU.width = int(fs["width"]); 25 | model.sizeU.height = int(fs["height"]) ; 26 | 27 | fs.release(); 28 | } 29 | 30 | void readFacialFeaturePointFromYML(const std::string& fileName, Mat_& facialFeaturePoint) 31 | { 32 | FileStorage fs(fileName,FileStorage::READ); 33 | 34 | if(!fs.isOpened()){ 35 | string msg = "open " + fileName + " error"; 36 | DEBUGMSG(msg); 37 | return; 38 | } 39 | 40 | fs["facialFeaturePoint"] >> facialFeaturePoint; 41 | 42 | fs.release(); 43 | } 44 | 45 | void readCameraMatrixFromYML(const std::string& fileName, Mat_& cameraMatrix) 46 | { 47 | FileStorage fs(fileName,FileStorage::READ); 48 | 49 | if(!fs.isOpened()){ 50 | string msg = "open " + fileName + " error"; 51 | DEBUGMSG(msg); 52 | return; 53 | } 54 | 55 | fs["extrinsicMatrix"] >> cameraMatrix; 56 | 57 | fs.release(); 58 | } 59 | 60 | 61 | void readMatFromFile(const string& fileName, Mat_& m) 62 | { 63 | CV_Assert(m.channels() == 1); 64 | 65 | if(NULL == m.data){ 66 | DEBUGMSG("Mat can not be alloced memory"); 67 | return; 68 | } 69 | 70 | FILE* fp = fopen(fileName.c_str(), "r"); 71 | if(NULL == fp){ 72 | string msg = "open " + fileName + " error"; 73 | DEBUGMSG(msg); 74 | return; 75 | } 76 | 77 | int rows = m.rows, cols = m.cols; 78 | float* mPtr; 79 | for(int i = 0; i < rows; ++i){ 80 | mPtr = m.ptr(i); 81 | for(int j = 0; j < cols; ++j){ 82 | fscanf(fp, "%f,", mPtr+j); 83 | } 84 | } 85 | 86 | fclose(fp); 87 | } 88 | -------------------------------------------------------------------------------- /frontalUtil.h: -------------------------------------------------------------------------------- 1 | #ifndef FRONTAL_UTIL_H 2 | #define FRONTAL_UTIL_H 3 | 4 | #include 5 | #include 6 | 7 | #include "base.h" 8 | 9 | void read3DModelFromYML(const std::string & fileName, 10 | model3D& model); 11 | 12 | //just for test image 13 | void readFacialFeaturePointFromYML(const std::string& fileName, 14 | Mat_& facialFeaturePoint); 15 | void readCameraMatrixFromYML(const std::string& fileName, 16 | Mat_& cameraMatrix); 17 | 18 | void readMatFromFile(const string& fileName, Mat_& m); 19 | 20 | 21 | template 22 | bool equal(const T& x, const T& y) 23 | { 24 | if(x-y < 1e-5 && x-y > 1e-5) 25 | return true; 26 | else 27 | return false; 28 | } 29 | 30 | #endif // FRONTAL_UTIL_H 31 | -------------------------------------------------------------------------------- /frontalization.cpp: -------------------------------------------------------------------------------- 1 | #include "frontalization.h" 2 | 3 | void frontalizeWithoutSymmetry(const Mat &image, 4 | const Mat_ &cameraMatrix, 5 | const Mat_ &refU, 6 | const Size &refSize, 7 | Mat &frontalImage, 8 | Mat_ &projection_, 9 | vector &indexFrontal_) 10 | { 11 | CV_Assert((image.depth() == CV_8U) && (refU.cols == 3)); 12 | 13 | // search the invalid 3d point 14 | vector bgind; 15 | const Mat_ absRefU = cv::abs(refU); 16 | int nPoint = refU.rows; 17 | for(int i = 0; i < nPoint; ++i){ 18 | if(equal(static_cast(cv::sum(absRefU.row(i))[0]), static_cast(0.0))){ 19 | bgind.push_back(i); 20 | } 21 | } 22 | 23 | // calculate the projection of refU on query image 24 | Mat_ threeDee(nPoint, 4, 1); 25 | const float* refUPtr; 26 | float *threeDeePtr; 27 | for(int i = 0; i < nPoint; ++i){ 28 | refUPtr =refU.ptr(i); 29 | threeDeePtr = threeDee.ptr(i); 30 | 31 | threeDeePtr[0] = refUPtr[0]; 32 | threeDeePtr[1] = refUPtr[1]; 33 | threeDeePtr[2] = refUPtr[2]; 34 | } 35 | 36 | Mat_ tmpProjection = threeDee * cameraMatrix.t(); 37 | Mat_ tmpProjection2( nPoint, 2 ); 38 | 39 | set badPoint; 40 | int queryWidth = image.cols, queryHeight = image.rows; 41 | const float* tmpProjectionPtr; 42 | float* tmpProjectionPtr2; 43 | for(int i = 0; i < nPoint; ++i){ 44 | tmpProjectionPtr = tmpProjection.ptr(i); 45 | tmpProjectionPtr2 = tmpProjection2.ptr(i); 46 | 47 | tmpProjectionPtr2[0] = tmpProjectionPtr[0]/tmpProjectionPtr[2]; 48 | tmpProjectionPtr2[1] = tmpProjectionPtr[1]/tmpProjectionPtr[2]; 49 | 50 | // if the projection point is out of plane, record it 51 | if(std::min(tmpProjectionPtr2[0], tmpProjectionPtr2[1]) < 1.0 || 52 | tmpProjectionPtr2[0] > (queryWidth-1) || 53 | tmpProjectionPtr2[1] > (queryHeight-1)) 54 | badPoint.insert(i); 55 | } 56 | 57 | int sizeBgind = bgind.size(); 58 | for(int i = 0; i < sizeBgind; ++i) 59 | badPoint.insert(bgind[i]); 60 | 61 | // exculde invalid points from projection 62 | Mat_ projection(nPoint-int(badPoint.size()), 2); 63 | float* projectionPtr; 64 | int k = 0; 65 | for(int i = 0; i < nPoint; ++i){ 66 | if(badPoint.find(i) == badPoint.end()){ 67 | tmpProjectionPtr2 = tmpProjection2.ptr(i); 68 | projectionPtr = projection.ptr(k++); 69 | 70 | projectionPtr[0] = tmpProjectionPtr2[0]; 71 | projectionPtr[1] = tmpProjectionPtr2[1]; 72 | } 73 | } 74 | 75 | vector indexFrontalTmp(refSize.width*refSize.height); 76 | vector indexFrontal; 77 | int sizeIndexFrontalTmp = indexFrontalTmp.size(); 78 | for(int i = 0; i < sizeIndexFrontalTmp; ++i) 79 | indexFrontalTmp[i] = i; 80 | for(int i = 0; i < sizeIndexFrontalTmp; ++i){ 81 | if(badPoint.find(i) == badPoint.end()) 82 | indexFrontal.push_back(indexFrontalTmp[i]); 83 | } 84 | 85 | // calculate eachh pixel value of frontal image 86 | bilinearInterp(image, projection, indexFrontal, refSize, frontalImage); 87 | 88 | projection_ = projection; 89 | indexFrontal_ = indexFrontal; 90 | 91 | // rotate the frontal image 92 | // Mat rotMat = getRotationMatrix2D(Point2f(refSize.width/2.0, refSize.height/2.0), 270, 1.0); 93 | // warpAffine(frontalImage, frontalImage, rotMat, refSize); 94 | } 95 | 96 | ///////////////////////////////////////////////////////////////////////////// 97 | void bilinearInterp(const Mat& src, const Mat_& pos, const vector& indexFrontal, const Size dstSize, Mat &dst) 98 | { 99 | int dstHeight = dstSize.height, dstWidth = dstSize.width; 100 | 101 | CV_Assert(pos.cols == 2 && static_cast(pos.rows) ==static_cast( indexFrontal.size())); 102 | 103 | dst.create(dstHeight, dstWidth, src.type()); 104 | 105 | const float* posPtr; 106 | float dx, dy; 107 | int x, y, row, col; 108 | int nChannel = src.channels(); 109 | int sizeIndexFrontal = indexFrontal.size(); 110 | 111 | if(nChannel == 1){ 112 | for(int i = 0; i < sizeIndexFrontal; ++i){ 113 | posPtr = pos.ptr(i); 114 | 115 | x = static_cast(posPtr[1]); 116 | y = static_cast(posPtr[0]); 117 | dx = posPtr[1] - x; 118 | dy = posPtr[0] - y; 119 | 120 | row = indexFrontal[i]%dstHeight; 121 | col = indexFrontal[i]/dstHeight; 122 | 123 | dst.at(row, col) = saturate_cast((1-dx)*(1-dy)*src.at(x, y)+ 124 | (1-dx)*dy*src.at(x, y+1)+ 125 | dx*(1-dy)*src.at(x+1, y)+ 126 | dx*dy*src.at(x+1,y+1)); 127 | } 128 | } 129 | else if(nChannel == 3){ 130 | Vec3b tmp1, tmp2, tmp3, tmp4; 131 | for(int i = 0; i < sizeIndexFrontal; ++i){ 132 | posPtr = pos.ptr(i); 133 | 134 | x = static_cast(posPtr[1]); // here must be careful 135 | y = static_cast(posPtr[0]); 136 | dx = posPtr[1] - x; 137 | dy = posPtr[0] - y; 138 | 139 | tmp1 = src.at(x, y); 140 | tmp2 = src.at(x, y+1); 141 | tmp3 = src.at(x+1, y); 142 | tmp4 = src.at(x+1, y+1); 143 | 144 | row = indexFrontal[i]%dstHeight; 145 | col = indexFrontal[i]/dstHeight; 146 | 147 | dst.at(row, col)[0] = saturate_cast((1-dx)*(1-dy)*tmp1[0] + 148 | (1-dx)*dy*tmp2[0] + dx*(1-dy)*tmp3[0] + dx*dy*tmp4[0]); 149 | dst.at(row, col)[1] = saturate_cast((1-dx)*(1-dy)*tmp1[1] + 150 | (1-dx)*dy*tmp2[1] + dx*(1-dy)*tmp3[1] + dx*dy*tmp4[1]); 151 | dst.at(row, col)[2] = saturate_cast((1-dx)*(1-dy)*tmp1[2] + 152 | (1-dx)*dy*tmp2[2] + dx*(1-dy)*tmp3[2] + dx*dy*tmp4[2]); 153 | } 154 | } 155 | else{ 156 | DEBUGMSG("Unsupport channel number"); 157 | return; 158 | } 159 | } 160 | 161 | 162 | void doCameraCalibration(const Mat_& points3D, 163 | const Mat_& points2D, 164 | const Size imgSize, 165 | const Mat_& intrinsicMatrix, 166 | Mat_& cameraMatrix) 167 | { 168 | 169 | int nPoint = points3D.rows; 170 | CV_Assert(nPoint == points2D.rows && 171 | points3D.cols == 3 && 172 | points2D.cols == 2); 173 | 174 | double* objPoints = new double [nPoint*3]; 175 | double* imgPoints = new double [nPoint*2]; 176 | double AIn[9]; 177 | 178 | const float* point3DPtr, *point2DPtr, *intrMatPtr; 179 | 180 | for(int i = 0; i < nPoint; ++i){ 181 | point3DPtr = points3D.ptr(i); 182 | point2DPtr = points2D.ptr(i); 183 | 184 | objPoints[i*3] = static_cast(point3DPtr[0]); 185 | objPoints[i*3+1] = static_cast(point3DPtr[1]); 186 | objPoints[i*3+2] = static_cast(point3DPtr[2]); 187 | 188 | imgPoints[i*2] = static_cast(point2DPtr[0]); 189 | imgPoints[i*2+1] = static_cast(point2DPtr[1]); 190 | } 191 | 192 | for(int i = 0; i < 3; ++i){ 193 | intrMatPtr = intrinsicMatrix.ptr(i); 194 | 195 | AIn[i*3] = static_cast(intrMatPtr[0]); 196 | AIn[i*3+1] = static_cast(intrMatPtr[1]); 197 | AIn[i*3+2] = static_cast(intrMatPtr[2]); 198 | } 199 | 200 | bool usePOSIT = false; 201 | 202 | double* APtr = new double [9]; 203 | double* RPtr = new double [9]; 204 | double* TPtr = new double [3]; 205 | 206 | run(imgSize.width, imgSize.height, nPoint, imgPoints, objPoints, APtr, RPtr, TPtr, AIn, 0, usePOSIT, true, 0, NULL, NULL); 207 | 208 | Mat_ ROut(3, 3); 209 | Mat_ TOut(3, 1); 210 | 211 | float *ROutPtr, *TOutPtr; 212 | 213 | for(int i = 0; i < 3; ++i){ 214 | ROutPtr = ROut.ptr(i); 215 | TOutPtr = TOut.ptr(i); 216 | 217 | ROutPtr[0] = static_cast(RPtr[i*3]); 218 | ROutPtr[1] = static_cast(RPtr[i*3+1]); 219 | ROutPtr[2] = static_cast(RPtr[i*3+2]); 220 | 221 | TOutPtr[0] = static_cast(TPtr[i]); 222 | } 223 | 224 | cameraMatrix.create(3, 4); 225 | float* cameraMatrixPtr; 226 | 227 | ROut = ROut.t(); 228 | 229 | for(int i = 0; i < 3; ++i){ 230 | cameraMatrixPtr = cameraMatrix.ptr(i); 231 | ROutPtr = ROut.ptr(i); 232 | TOutPtr = TOut.ptr(i); 233 | 234 | cameraMatrixPtr[0] = ROutPtr[0]; 235 | cameraMatrixPtr[1] = ROutPtr[1]; 236 | cameraMatrixPtr[2] = ROutPtr[2]; 237 | cameraMatrixPtr[3] = TOutPtr[0]; 238 | } 239 | 240 | 241 | cameraMatrix = intrinsicMatrix*cameraMatrix; 242 | 243 | delete [] objPoints; 244 | delete [] imgPoints; 245 | delete [] APtr; 246 | delete [] RPtr; 247 | delete [] TPtr; 248 | } 249 | -------------------------------------------------------------------------------- /frontalization.h: -------------------------------------------------------------------------------- 1 | #ifndef FRONTALIZATION_H 2 | #define FRONTALIZATION_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include "base.h" 10 | #include "frontalUtil.h" 11 | #include "calib/calib.h" 12 | 13 | using namespace std; 14 | using namespace cv; 15 | 16 | #define ACC_CONST 800 17 | 18 | void frontalizeWithoutSymmetry(const Mat& image, 19 | const Mat_& cameraMatrix, 20 | const Mat_& refU, 21 | const Size &refSize, 22 | Mat &frontalImage, 23 | Mat_ &projection_ = *(new Mat_()), 24 | vector &indexFrontal_ = *(new vector())); 25 | 26 | void frontalizeWithSoftSymmetry(const Mat& image, 27 | const Mat_& cameraMatrix, 28 | const Mat_& refU, 29 | const Size &refSize, 30 | const Mat_& eyeMask, 31 | Mat& frontalImage); 32 | 33 | /////////////////////////////////////////////////// 34 | /** 35 | * @brief using bi-linear interploation to calculate the \a dst according to \a src and the correspondence position \a pos in src 36 | * @param pos the corresponding position for each points of dst in src (Nx2) 37 | * @param dstSize the size of dst (WxH) 38 | * @note the row of pos and the area of dstSize must be the same; src only has single channel. 39 | */ 40 | void bilinearInterp(const Mat& src, 41 | const Mat_& pos, 42 | const vector &indexFrontal, 43 | const Size dstSize, 44 | Mat& dst); 45 | 46 | /** 47 | * @brief calculate the camera's parameter 48 | * 49 | */ 50 | void doCameraCalibration(const Mat_& Points3D, 51 | const Mat_& Points2D, 52 | const Size imgSize, 53 | const Mat_ &intrinsicMatrix, Mat_ &cameraMatrix); 54 | 55 | #endif // FRONTALIZATION_H 56 | -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | #include "frontalization.h" 2 | 3 | #include 4 | 5 | 6 | int main(int argc, char* argv[]) 7 | { 8 | 9 | if(argc != 4){ 10 | cout << "Usage: " << argv[0] << " <3DModelFile>" << endl; 11 | return -1; 12 | } 13 | 14 | Mat image = imread(argv[1]); 15 | Mat frontalImage; 16 | Mat_ cameraMatrix, facialFeaturePoints; 17 | model3D model; 18 | 19 | readFacialFeaturePointFromYML(string(argv[2]), facialFeaturePoints); 20 | // readCameraMatrixFromYML(string(argv[3]), cameraMatrix); 21 | read3DModelFromYML(string(argv[3]), model); 22 | 23 | doCameraCalibration(model.threedee, facialFeaturePoints, model.sizeU, model.outA, cameraMatrix); 24 | 25 | frontalizeWithoutSymmetry(image, cameraMatrix, model.refU, model.sizeU, frontalImage); 26 | 27 | imshow("image", image); 28 | imshow("frontal", frontalImage); 29 | waitKey(); 30 | 31 | imwrite("image.png", image); 32 | imwrite("frontal.png", frontalImage); 33 | 34 | return 0; 35 | } 36 | 37 | -------------------------------------------------------------------------------- /pro/Makefile: -------------------------------------------------------------------------------- 1 | ############################################################################# 2 | # Makefile for building: frontalization 3 | # Generated by qmake (2.01a) (Qt 4.8.4) on: ?? 1? 6 22:56:43 2015 4 | # Project: frontalization.pro 5 | # Template: app 6 | # Command: /usr/lib/x86_64-linux-gnu/qt4/bin/qmake -o Makefile frontalization.pro 7 | ############################################################################# 8 | 9 | ####### Compiler, tools and options 10 | 11 | CC = gcc 12 | CXX = g++ 13 | DEFINES = 14 | CFLAGS = -m64 -pipe -O2 -Wall -W $(DEFINES) 15 | CXXFLAGS = -m64 -pipe -O2 -Wall -W $(DEFINES) 16 | INCPATH = -I/usr/share/qt4/mkspecs/linux-g++-64 -I. -I/usr/local/include/opencv -I-I/usr/local/include 17 | LINK = g++ 18 | LFLAGS = -m64 -Wl,-O1 19 | LIBS = $(SUBLIBS) -L/usr/local/lib -lopencv_calib3d -lopencv_contrib -lopencv_core -lopencv_features2d -lopencv_flann -lopencv_highgui -lopencv_imgproc -lopencv_legacy -lopencv_ml -lopencv_objdetect -lopencv_photo -lopencv_stitching -lopencv_superres -lopencv_ts -lopencv_video -lopencv_videostab 20 | AR = ar cqs 21 | RANLIB = 22 | QMAKE = /usr/lib/x86_64-linux-gnu/qt4/bin/qmake 23 | TAR = tar -cf 24 | COMPRESS = gzip -9f 25 | COPY = cp -f 26 | SED = sed 27 | COPY_FILE = $(COPY) 28 | COPY_DIR = $(COPY) -r 29 | STRIP = strip 30 | INSTALL_FILE = install -m 644 -p 31 | INSTALL_DIR = $(COPY_DIR) 32 | INSTALL_PROGRAM = install -m 755 -p 33 | DEL_FILE = rm -f 34 | SYMLINK = ln -f -s 35 | DEL_DIR = rmdir 36 | MOVE = mv -f 37 | CHK_DIR_EXISTS= test -d 38 | MKDIR = mkdir -p 39 | 40 | ####### Output directory 41 | 42 | OBJECTS_DIR = ./ 43 | 44 | ####### Files 45 | 46 | SOURCES = ../main.cpp \ 47 | ../frontalization.cpp \ 48 | ../calib/calib.cpp \ 49 | ../calib/POSIT.cpp \ 50 | ../calib/util/calib_util.cpp \ 51 | ../frontalUtil.cpp 52 | OBJECTS = main.o \ 53 | frontalization.o \ 54 | calib.o \ 55 | POSIT.o \ 56 | calib_util.o \ 57 | frontalUtil.o 58 | DIST = /usr/share/qt4/mkspecs/common/unix.conf \ 59 | /usr/share/qt4/mkspecs/common/linux.conf \ 60 | /usr/share/qt4/mkspecs/common/gcc-base.conf \ 61 | /usr/share/qt4/mkspecs/common/gcc-base-unix.conf \ 62 | /usr/share/qt4/mkspecs/common/g++-base.conf \ 63 | /usr/share/qt4/mkspecs/common/g++-unix.conf \ 64 | /usr/share/qt4/mkspecs/qconfig.pri \ 65 | /usr/share/qt4/mkspecs/features/qt_functions.prf \ 66 | /usr/share/qt4/mkspecs/features/qt_config.prf \ 67 | /usr/share/qt4/mkspecs/features/exclusive_builds.prf \ 68 | /usr/share/qt4/mkspecs/features/default_pre.prf \ 69 | /usr/share/qt4/mkspecs/features/release.prf \ 70 | /usr/share/qt4/mkspecs/features/default_post.prf \ 71 | /usr/share/qt4/mkspecs/features/unix/gdb_dwarf_index.prf \ 72 | /usr/share/qt4/mkspecs/features/warn_on.prf \ 73 | /usr/share/qt4/mkspecs/features/resources.prf \ 74 | /usr/share/qt4/mkspecs/features/uic.prf \ 75 | /usr/share/qt4/mkspecs/features/yacc.prf \ 76 | /usr/share/qt4/mkspecs/features/lex.prf \ 77 | /usr/share/qt4/mkspecs/features/include_source_dir.prf \ 78 | frontalization.pro 79 | QMAKE_TARGET = frontalization 80 | DESTDIR = 81 | TARGET = frontalization 82 | 83 | first: all 84 | ####### Implicit rules 85 | 86 | .SUFFIXES: .o .c .cpp .cc .cxx .C 87 | 88 | .cpp.o: 89 | $(CXX) -c $(CXXFLAGS) $(INCPATH) -o "$@" "$<" 90 | 91 | .cc.o: 92 | $(CXX) -c $(CXXFLAGS) $(INCPATH) -o "$@" "$<" 93 | 94 | .cxx.o: 95 | $(CXX) -c $(CXXFLAGS) $(INCPATH) -o "$@" "$<" 96 | 97 | .C.o: 98 | $(CXX) -c $(CXXFLAGS) $(INCPATH) -o "$@" "$<" 99 | 100 | .c.o: 101 | $(CC) -c $(CFLAGS) $(INCPATH) -o "$@" "$<" 102 | 103 | ####### Build rules 104 | 105 | all: Makefile $(TARGET) 106 | 107 | $(TARGET): $(OBJECTS) 108 | $(LINK) $(LFLAGS) -o $(TARGET) $(OBJECTS) $(OBJCOMP) $(LIBS) 109 | 110 | Makefile: frontalization.pro /usr/share/qt4/mkspecs/linux-g++-64/qmake.conf /usr/share/qt4/mkspecs/common/unix.conf \ 111 | /usr/share/qt4/mkspecs/common/linux.conf \ 112 | /usr/share/qt4/mkspecs/common/gcc-base.conf \ 113 | /usr/share/qt4/mkspecs/common/gcc-base-unix.conf \ 114 | /usr/share/qt4/mkspecs/common/g++-base.conf \ 115 | /usr/share/qt4/mkspecs/common/g++-unix.conf \ 116 | /usr/share/qt4/mkspecs/qconfig.pri \ 117 | /usr/share/qt4/mkspecs/features/qt_functions.prf \ 118 | /usr/share/qt4/mkspecs/features/qt_config.prf \ 119 | /usr/share/qt4/mkspecs/features/exclusive_builds.prf \ 120 | /usr/share/qt4/mkspecs/features/default_pre.prf \ 121 | /usr/share/qt4/mkspecs/features/release.prf \ 122 | /usr/share/qt4/mkspecs/features/default_post.prf \ 123 | /usr/share/qt4/mkspecs/features/unix/gdb_dwarf_index.prf \ 124 | /usr/share/qt4/mkspecs/features/warn_on.prf \ 125 | /usr/share/qt4/mkspecs/features/resources.prf \ 126 | /usr/share/qt4/mkspecs/features/uic.prf \ 127 | /usr/share/qt4/mkspecs/features/yacc.prf \ 128 | /usr/share/qt4/mkspecs/features/lex.prf \ 129 | /usr/share/qt4/mkspecs/features/include_source_dir.prf 130 | $(QMAKE) -o Makefile frontalization.pro 131 | /usr/share/qt4/mkspecs/common/unix.conf: 132 | /usr/share/qt4/mkspecs/common/linux.conf: 133 | /usr/share/qt4/mkspecs/common/gcc-base.conf: 134 | /usr/share/qt4/mkspecs/common/gcc-base-unix.conf: 135 | /usr/share/qt4/mkspecs/common/g++-base.conf: 136 | /usr/share/qt4/mkspecs/common/g++-unix.conf: 137 | /usr/share/qt4/mkspecs/qconfig.pri: 138 | /usr/share/qt4/mkspecs/features/qt_functions.prf: 139 | /usr/share/qt4/mkspecs/features/qt_config.prf: 140 | /usr/share/qt4/mkspecs/features/exclusive_builds.prf: 141 | /usr/share/qt4/mkspecs/features/default_pre.prf: 142 | /usr/share/qt4/mkspecs/features/release.prf: 143 | /usr/share/qt4/mkspecs/features/default_post.prf: 144 | /usr/share/qt4/mkspecs/features/unix/gdb_dwarf_index.prf: 145 | /usr/share/qt4/mkspecs/features/warn_on.prf: 146 | /usr/share/qt4/mkspecs/features/resources.prf: 147 | /usr/share/qt4/mkspecs/features/uic.prf: 148 | /usr/share/qt4/mkspecs/features/yacc.prf: 149 | /usr/share/qt4/mkspecs/features/lex.prf: 150 | /usr/share/qt4/mkspecs/features/include_source_dir.prf: 151 | qmake: FORCE 152 | @$(QMAKE) -o Makefile frontalization.pro 153 | 154 | dist: 155 | @$(CHK_DIR_EXISTS) .tmp/frontalization1.0.0 || $(MKDIR) .tmp/frontalization1.0.0 156 | $(COPY_FILE) --parents $(SOURCES) $(DIST) .tmp/frontalization1.0.0/ && (cd `dirname .tmp/frontalization1.0.0` && $(TAR) frontalization1.0.0.tar frontalization1.0.0 && $(COMPRESS) frontalization1.0.0.tar) && $(MOVE) `dirname .tmp/frontalization1.0.0`/frontalization1.0.0.tar.gz . && $(DEL_FILE) -r .tmp/frontalization1.0.0 157 | 158 | 159 | clean:compiler_clean 160 | -$(DEL_FILE) $(OBJECTS) 161 | -$(DEL_FILE) *~ core *.core 162 | 163 | 164 | ####### Sub-libraries 165 | 166 | distclean: clean 167 | -$(DEL_FILE) $(TARGET) 168 | -$(DEL_FILE) Makefile 169 | 170 | 171 | check: first 172 | 173 | compiler_rcc_make_all: 174 | compiler_rcc_clean: 175 | compiler_uic_make_all: 176 | compiler_uic_clean: 177 | compiler_image_collection_make_all: qmake_image_collection.cpp 178 | compiler_image_collection_clean: 179 | -$(DEL_FILE) qmake_image_collection.cpp 180 | compiler_yacc_decl_make_all: 181 | compiler_yacc_decl_clean: 182 | compiler_yacc_impl_make_all: 183 | compiler_yacc_impl_clean: 184 | compiler_lex_make_all: 185 | compiler_lex_clean: 186 | compiler_clean: 187 | 188 | ####### Compile 189 | 190 | main.o: ../main.cpp ../frontalization.h \ 191 | ../base.h \ 192 | ../frontalUtil.h \ 193 | ../calib/calib.h \ 194 | ../calib/util/calib_util.h \ 195 | ../calib/POSIT.h 196 | $(CXX) -c $(CXXFLAGS) $(INCPATH) -o main.o ../main.cpp 197 | 198 | frontalization.o: ../frontalization.cpp ../frontalization.h \ 199 | ../base.h \ 200 | ../frontalUtil.h \ 201 | ../calib/calib.h \ 202 | ../calib/util/calib_util.h \ 203 | ../calib/POSIT.h 204 | $(CXX) -c $(CXXFLAGS) $(INCPATH) -o frontalization.o ../frontalization.cpp 205 | 206 | calib.o: ../calib/calib.cpp ../calib/calib.h \ 207 | ../calib/util/calib_util.h \ 208 | ../calib/POSIT.h 209 | $(CXX) -c $(CXXFLAGS) $(INCPATH) -o calib.o ../calib/calib.cpp 210 | 211 | POSIT.o: ../calib/POSIT.cpp ../calib/POSIT.h 212 | $(CXX) -c $(CXXFLAGS) $(INCPATH) -o POSIT.o ../calib/POSIT.cpp 213 | 214 | calib_util.o: ../calib/util/calib_util.cpp ../calib/util/calib_util.h 215 | $(CXX) -c $(CXXFLAGS) $(INCPATH) -o calib_util.o ../calib/util/calib_util.cpp 216 | 217 | frontalUtil.o: ../frontalUtil.cpp ../frontalUtil.h \ 218 | ../base.h 219 | $(CXX) -c $(CXXFLAGS) $(INCPATH) -o frontalUtil.o ../frontalUtil.cpp 220 | 221 | ####### Install 222 | 223 | install: FORCE 224 | 225 | uninstall: FORCE 226 | 227 | FORCE: 228 | 229 | -------------------------------------------------------------------------------- /pro/frontal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shaoguangcheng/faceFrontalization/c5ba11e8a1ebcb47c43e8cb70e7e69b105de8ed7/pro/frontal.png -------------------------------------------------------------------------------- /pro/frontalization: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shaoguangcheng/faceFrontalization/c5ba11e8a1ebcb47c43e8cb70e7e69b105de8ed7/pro/frontalization -------------------------------------------------------------------------------- /pro/frontalization.pro: -------------------------------------------------------------------------------- 1 | TEMPLATE = app 2 | CONFIG += console 3 | CONFIG -= qt 4 | 5 | SOURCES += ../main.cpp \ 6 | ../frontalization.cpp \ 7 | ../calib/calib.cpp \ 8 | ../calib/POSIT.cpp \ 9 | ../calib/util/calib_util.cpp \ 10 | ../frontalUtil.cpp 11 | 12 | 13 | INCLUDEPATH += /usr/local/include/opencv -I/usr/local/include 14 | 15 | LIBS += -L/usr/local/lib -lopencv_calib3d -lopencv_contrib -lopencv_core -lopencv_features2d\ 16 | -lopencv_flann -lopencv_highgui -lopencv_imgproc -lopencv_legacy -lopencv_ml \ 17 | -lopencv_objdetect -lopencv_photo -lopencv_stitching -lopencv_superres -lopencv_ts -lopencv_video -lopencv_videostab 18 | 19 | HEADERS += \ 20 | ../base.h \ 21 | ../frontalization.h \ 22 | ../calib/POSIT.h \ 23 | ../calib/util/calib_util.h \ 24 | ../calib/calib.h \ 25 | ../frontalUtil.h 26 | -------------------------------------------------------------------------------- /pro/image.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shaoguangcheng/faceFrontalization/c5ba11e8a1ebcb47c43e8cb70e7e69b105de8ed7/pro/image.png --------------------------------------------------------------------------------