facemarkAAM.cpp 41.7 KB
Newer Older
1
/*
2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31
By downloading, copying, installing or using the software you agree to this
license. If you do not agree to this license, do not download, install,
copy or use the software.
                          License Agreement
               For Open Source Computer Vision Library
                       (3-clause BSD License)
Copyright (C) 2013, OpenCV Foundation, all rights reserved.
Third party copyrights are property of their respective owners.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
  * Redistributions of source code must retain the above copyright notice,
    this list of conditions and the following disclaimer.
  * Redistributions in binary form must reproduce the above copyright notice,
    this list of conditions and the following disclaimer in the documentation
    and/or other materials provided with the distribution.
  * Neither the names of the copyright holders nor the names of the contributors
    may be used to endorse or promote products derived from this software
    without specific prior written permission.
This software is provided by the copyright holders and contributors "as is" and
any express or implied warranties, including, but not limited to, the implied
warranties of merchantability and fitness for a particular purpose are
disclaimed. In no event shall copyright holders or contributors be liable for
any direct, indirect, incidental, special, exemplary, or consequential damages
(including, but not limited to, procurement of substitute goods or services;
loss of use, data, or profits; or business interruption) however caused
and on any theory of liability, whether in contract, strict liability,
or tort (including negligence or otherwise) arising in any way out of
the use of this software, even if advised of the possibility of such damage.

This file was part of GSoC Project: Facemark API for OpenCV
32 33 34 35 36 37
Final report: https://gist.github.com/kurnianggoro/74de9121e122ad0bd825176751d47ecc
Student: Laksono Kurnianggoro
Mentor: Delia Passalacqua
*/

#include "precomp.hpp"
Alexander Alekhin's avatar
Alexander Alekhin committed
38
#include "opencv2/face.hpp"
39 40 41
namespace cv {
namespace face {

Alexander Alekhin's avatar
Alexander Alekhin committed
42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94
/*
* Parameters
*/
FacemarkAAM::Params::Params(){
    model_filename = "";
    m = 200;
    n = 10;
    n_iter = 50;
    verbose = true;
    save_model = true;
    scales.push_back(1.0);
    max_m = 550;
    max_n = 136;
    texture_max_m = 145;
}

FacemarkAAM::Config::Config(Mat rot, Point2f trans, float scaling,int scale_id){
    R = rot.clone();
    t = trans;
    scale = scaling;
    model_scale_idx = scale_id;
}

void FacemarkAAM::Params::read( const cv::FileNode& fn ){
    *this = FacemarkAAM::Params();

    if (!fn["model_filename"].empty()) fn["model_filename"] >> model_filename;

    if (!fn["m"].empty()) fn["m"] >> m;
    if (!fn["n"].empty()) fn["n"] >> m;
    if (!fn["n_iter"].empty()) fn["n_iter"] >> m;
    if (!fn["verbose"].empty()) fn["verbose"] >> m;
    if (!fn["max_m"].empty()) fn["max_m"] >> m;
    if (!fn["max_n"].empty()) fn["max_n"] >> m;
    if (!fn["texture_max_m"].empty()) fn["texture_max_m"] >> m;
    if (!fn["scales"].empty()) fn["scales"] >> m;
}

void FacemarkAAM::Params::write( cv::FileStorage& fs ) const{
    fs << "model_filename" << model_filename;
    fs << "m" << m;
    fs << "n" << n;
    fs << "n_iter" << n_iter;
    fs << "verbose" << verbose;
    fs << "max_m" << verbose;
    fs << "max_n" << verbose;
    fs << "texture_max_m" << verbose;
    fs << "scales" << verbose;
}

class FacemarkAAMImpl : public FacemarkAAM {
public:
    FacemarkAAMImpl( const FacemarkAAM::Params &parameters = FacemarkAAM::Params() );
95 96
    void read( const FileNode& /*fn*/ ) CV_OVERRIDE;
    void write( FileStorage& /*fs*/ ) const CV_OVERRIDE;
Alexander Alekhin's avatar
Alexander Alekhin committed
97 98

    void saveModel(String fs);
99
    void loadModel(String fs) CV_OVERRIDE;
Alexander Alekhin's avatar
Alexander Alekhin committed
100

101 102
    bool setFaceDetector(bool(*f)(InputArray , OutputArray, void * ), void* userData) CV_OVERRIDE;
    bool getFaces(InputArray image, OutputArray faces) CV_OVERRIDE;
Alexander Alekhin's avatar
Alexander Alekhin committed
103

104
    bool getData(void * items) CV_OVERRIDE;
Alexander Alekhin's avatar
Alexander Alekhin committed
105

106
    bool fitConfig( InputArray image, InputArray roi, OutputArrayOfArrays _landmarks, const std::vector<Config> &runtime_params ) CV_OVERRIDE;
107

Alexander Alekhin's avatar
Alexander Alekhin committed
108 109
protected:

110
    bool fit( InputArray image, InputArray faces, OutputArrayOfArrays landmarks ) CV_OVERRIDE;
Alexander Alekhin's avatar
Alexander Alekhin committed
111 112
    bool fitImpl( const Mat image, std::vector<Point2f>& landmarks,const  Mat R,const  Point2f T,const  float scale, const int sclIdx=0 );

113 114
    bool addTrainingSample(InputArray image, InputArray landmarks) CV_OVERRIDE;
    void training(void* parameters) CV_OVERRIDE;
Alexander Alekhin's avatar
Alexander Alekhin committed
115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143

    Mat procrustes(std::vector<Point2f> , std::vector<Point2f> , Mat & , Scalar & , float & );
    void calcMeanShape(std::vector<std::vector<Point2f> > ,std::vector<Point2f> & );
    void procrustesAnalysis(std::vector<std::vector<Point2f> > , std::vector<std::vector<Point2f> > & , std::vector<Point2f> & );

    inline Mat linearize(Mat );
    inline Mat linearize(std::vector<Point2f> );
    void getProjection(const Mat , Mat &, int );
    void calcSimilarityEig(std::vector<Point2f> ,Mat , Mat & , Mat & );
    Mat orthonormal(Mat );
    void delaunay(std::vector<Point2f> , std::vector<Vec3i> & );
    Mat createMask(std::vector<Point2f> , Rect );
    Mat createTextureBase(std::vector<Point2f> , std::vector<Vec3i> , Rect , std::vector<std::vector<Point> > & );
    Mat warpImage(const Mat ,const  std::vector<Point2f> ,const  std::vector<Point2f> ,
                  const std::vector<Vec3i> , const Rect , const  std::vector<std::vector<Point> > );
    template <class T>
    Mat getFeature(const Mat , std::vector<int> map);
    void createMaskMapping(const Mat mask, const Mat mask2,  std::vector<int> & , std::vector<int> &, std::vector<int> &);

    void warpUpdate(std::vector<Point2f> & shape, Mat delta, std::vector<Point2f> s0, Mat S, Mat Q, std::vector<Vec3i> triangles,std::vector<std::vector<int> > Tp);
    Mat computeWarpParts(std::vector<Point2f> curr_shape,std::vector<Point2f> s0, Mat ds0, std::vector<Vec3i> triangles,std::vector<std::vector<int> > Tp);
    void image_jacobian(const Mat gx, const Mat gy, const Mat Jx, const Mat Jy, Mat & G);
    void gradient(const Mat M, Mat & gx, Mat & gy);
    void createWarpJacobian(Mat S, Mat Q,  std::vector<Vec3i> , Model::Texture & T, Mat & Wx_dp, Mat & Wy_dp, std::vector<std::vector<int> > & Tp);

    std::vector<Mat> images;
    std::vector<std::vector<Point2f> > facePoints;
    FacemarkAAM::Params params;
    FacemarkAAM::Model AAM;
Alexander Alekhin's avatar
Alexander Alekhin committed
144 145
    FN_FaceDetector faceDetector;
    void* faceDetectorData;
Alexander Alekhin's avatar
Alexander Alekhin committed
146 147 148 149

private:
    bool isModelTrained;
};
150

Alexander Alekhin's avatar
Alexander Alekhin committed
151 152 153 154 155 156
/*
* Constructor
*/
Ptr<FacemarkAAM> FacemarkAAM::create(const FacemarkAAM::Params &parameters){
    return Ptr<FacemarkAAMImpl>(new FacemarkAAMImpl(parameters));
}
157

158 159 160 161 162 163 164 165
/*
* Constructor
*/
Ptr<Facemark> createFacemarkAAM(){
    FacemarkAAM::Params parameters;
    return Ptr<FacemarkAAMImpl>(new FacemarkAAMImpl(parameters));
}

Alexander Alekhin's avatar
Alexander Alekhin committed
166
FacemarkAAMImpl::FacemarkAAMImpl( const FacemarkAAM::Params &parameters ) :
Alexander Alekhin's avatar
Alexander Alekhin committed
167 168
    params( parameters ),
    faceDetector(NULL), faceDetectorData(NULL)
Alexander Alekhin's avatar
Alexander Alekhin committed
169 170 171
{
    isModelTrained = false;
}
172

Alexander Alekhin's avatar
Alexander Alekhin committed
173 174 175
void FacemarkAAMImpl::read( const cv::FileNode& fn ){
    params.read( fn );
}
176

Alexander Alekhin's avatar
Alexander Alekhin committed
177 178 179
void FacemarkAAMImpl::write( cv::FileStorage& fs ) const {
    params.write( fs );
}
180

Alexander Alekhin's avatar
Alexander Alekhin committed
181
bool FacemarkAAMImpl::setFaceDetector(bool(*f)(InputArray , OutputArray, void *), void* userData){
Alexander Alekhin's avatar
Alexander Alekhin committed
182
    faceDetector = f;
Alexander Alekhin's avatar
Alexander Alekhin committed
183
    faceDetectorData = userData;
Alexander Alekhin's avatar
Alexander Alekhin committed
184 185
    return true;
}
186 187


Alexander Alekhin's avatar
Alexander Alekhin committed
188 189 190
bool FacemarkAAMImpl::getFaces(InputArray image, OutputArray faces)
{
    if (!faceDetector)
Alexander Alekhin's avatar
Alexander Alekhin committed
191
        return false;
Alexander Alekhin's avatar
Alexander Alekhin committed
192
    return faceDetector(image, faces, faceDetectorData);
Alexander Alekhin's avatar
Alexander Alekhin committed
193
}
194

Alexander Alekhin's avatar
Alexander Alekhin committed
195
bool FacemarkAAMImpl::getData(void * items){
Alexander Alekhin's avatar
Alexander Alekhin committed
196 197 198 199 200
    CV_Assert(items);

    Data* data = (Data*)items;
    data->s0 = AAM.s0;
    return true;
Alexander Alekhin's avatar
Alexander Alekhin committed
201
}
202

Alexander Alekhin's avatar
Alexander Alekhin committed
203
bool FacemarkAAMImpl::addTrainingSample(InputArray image, InputArray landmarks){
Alexander Alekhin's avatar
Alexander Alekhin committed
204
    // FIXIT
Alexander Alekhin's avatar
Alexander Alekhin committed
205
    std::vector<Point2f> & _landmarks = *(std::vector<Point2f>*)landmarks.getObj();
206

Alexander Alekhin's avatar
Alexander Alekhin committed
207 208
    images.push_back(image.getMat());
    facePoints.push_back(_landmarks);
209

Alexander Alekhin's avatar
Alexander Alekhin committed
210 211
    return true;
}
212

Alexander Alekhin's avatar
Alexander Alekhin committed
213 214 215
void FacemarkAAMImpl::training(void* parameters){
    if(parameters!=0){/*do nothing*/}
    if (images.size()<1) {
Alexander Alekhin's avatar
Alexander Alekhin committed
216
        CV_Error(Error::StsBadArg, "Training data is not provided. Consider to add using addTrainingSample() function!");
217 218
    }

Alexander Alekhin's avatar
Alexander Alekhin committed
219
    if(strcmp(params.model_filename.c_str(),"")==0 && params.save_model){
Alexander Alekhin's avatar
Alexander Alekhin committed
220
        CV_Error(Error::StsBadArg, "The model_filename parameter should be set!");
221 222
    }

Alexander Alekhin's avatar
Alexander Alekhin committed
223 224 225
    std::vector<std::vector<Point2f> > normalized;
    Mat erode_kernel = getStructuringElement(MORPH_RECT, Size(3,3), Point(1,1));
    Mat image;
226

Alexander Alekhin's avatar
Alexander Alekhin committed
227 228
    int param_max_m = params.max_m;//550;
    int param_max_n = params.max_n;//136;
229

Alexander Alekhin's avatar
Alexander Alekhin committed
230 231
    AAM.scales = params.scales;
    AAM.textures.resize(AAM.scales.size());
232

Alexander Alekhin's avatar
Alexander Alekhin committed
233 234
    /*-------------- A. Load the training data---------*/
    procrustesAnalysis(facePoints, normalized,AAM.s0);
235

Alexander Alekhin's avatar
Alexander Alekhin committed
236 237 238 239 240 241 242
    /*-------------- B. Create the shape model---------*/
    Mat s0_lin = linearize(AAM.s0).t() ;
    // linearize all shapes  data, all x and then all y for each shape
    Mat M;
    for(unsigned i=0;i<normalized.size();i++){
        M.push_back(linearize(normalized[i]).t()-s0_lin);
    }
243

Alexander Alekhin's avatar
Alexander Alekhin committed
244 245 246 247 248 249
    /* get PCA Projection vectors */
    Mat S;
    getProjection(M.t(),S,param_max_n);
    /* Create similarity eig*/
    Mat shape_S,shape_Q;
    calcSimilarityEig(AAM.s0,S,AAM.Q,AAM.S);
250

Alexander Alekhin's avatar
Alexander Alekhin committed
251 252
    /* ----------C. Create the coordinate frame ------------*/
    delaunay(AAM.s0,AAM.triangles);
253

Alexander Alekhin's avatar
Alexander Alekhin committed
254 255 256 257 258
    for(size_t scale=0; scale<AAM.scales.size();scale++){
        AAM.textures[scale].max_m = params.texture_max_m;//145;
        if(params.verbose) printf("Training for scale %f ...\n", AAM.scales[scale]);
        Mat s0_scaled_m = Mat(AAM.s0)/AAM.scales[scale]; // scale the shape
        std::vector<Point2f> s0_scaled = s0_scaled_m.reshape(2); //convert to points
259

Alexander Alekhin's avatar
Alexander Alekhin committed
260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290
        /*get the min and max of x and y coordinate*/
        double min_x, max_x, min_y, max_y;
        s0_scaled_m = s0_scaled_m.reshape(1);
        Mat s0_scaled_x = s0_scaled_m.col(0);
        Mat s0_scaled_y = s0_scaled_m.col(1);
        minMaxIdx(s0_scaled_x, &min_x, &max_x);
        minMaxIdx(s0_scaled_y, &min_y, &max_y);

        std::vector<Point2f> base_shape = Mat(Mat(s0_scaled)-Scalar(min_x-2.0,min_y-2.0)).reshape(2);
        AAM.textures[scale].base_shape = base_shape;
        AAM.textures[scale].resolution = Rect(0,0,(int)ceil(max_x-min_x+3),(int)ceil(max_y-min_y+3));

        Mat base_texture = createTextureBase(base_shape, AAM.triangles, AAM.textures[scale].resolution, AAM.textures[scale].textureIdx);

        Mat mask1 = base_texture>0;
        Mat mask2;
        erode(mask1, mask1, erode_kernel);
        erode(mask1, mask2, erode_kernel);

        Mat warped;
        std::vector<int> fe_map;
        createMaskMapping(mask1,mask2, AAM.textures[scale].ind1, AAM.textures[scale].ind2,fe_map);//ok

        /* ------------ Part D. Get textures -------------*/
        Mat texture_feats, feat;
        if(params.verbose) printf("(1/4) Feature extraction ...\n");
        for(size_t i=0; i<images.size();i++){
            if(params.verbose) printf("extract features from image #%i/%i\n", (int)(i+1), (int)images.size());
            warped = warpImage(images[i],base_shape, facePoints[i], AAM.triangles, AAM.textures[scale].resolution,AAM.textures[scale].textureIdx);
            feat = getFeature<uchar>(warped, AAM.textures[scale].ind1);
            texture_feats.push_back(feat.t());
291
        }
Alexander Alekhin's avatar
Alexander Alekhin committed
292
        Mat T= texture_feats.t();
293

Alexander Alekhin's avatar
Alexander Alekhin committed
294
        /* -------------- E. Create the texture model -----------------*/
Alexander Alekhin's avatar
Alexander Alekhin committed
295
        reduce(T,AAM.textures[scale].A0,1, REDUCE_AVG);
296

Alexander Alekhin's avatar
Alexander Alekhin committed
297 298 299
        if(params.verbose) printf("(2/4) Compute the feature average ...\n");
        Mat A0_mtx = repeat(AAM.textures[scale].A0,1,T.cols);
        Mat textures_normalized = T - A0_mtx;
300

Alexander Alekhin's avatar
Alexander Alekhin committed
301 302 303
        if(params.verbose) printf("(3/4) Projecting the features ...\n");
        getProjection(textures_normalized, AAM.textures[scale].A ,param_max_m);
        AAM.textures[scale].AA0 = getFeature<float>(AAM.textures[scale].A0, fe_map);
304

Alexander Alekhin's avatar
Alexander Alekhin committed
305 306 307 308 309 310
        if(params.verbose) printf("(4/4) Extraction of the eroded face features ...\n");
        Mat U_data, ud;
        for(int i =0;i<AAM.textures[scale].A.cols;i++){
            Mat c = AAM.textures[scale].A.col(i);
            ud = getFeature<float>(c,fe_map);
            U_data.push_back(ud.t());
311
        }
Alexander Alekhin's avatar
Alexander Alekhin committed
312 313 314 315 316 317 318 319
        Mat U = U_data.t();
        AAM.textures[scale].AA = orthonormal(U);
    } // scale

    images.clear();
    if(params.save_model){
        if(params.verbose) printf("Saving the model\n");
        saveModel(params.model_filename);
320
    }
Alexander Alekhin's avatar
Alexander Alekhin committed
321 322 323
    isModelTrained = true;
    if(params.verbose) printf("Training is completed\n");
}
324

325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360
/**
 * @brief Copy the contents of a corners vector to an OutputArray, settings its size.
 */
static void _copyVector2Output(std::vector< std::vector< Point2f > > &vec, OutputArrayOfArrays out)
{
    out.create((int)vec.size(), 1, CV_32FC2);

    if (out.isMatVector()) {
        for (unsigned int i = 0; i < vec.size(); i++) {
            out.create(68, 1, CV_32FC2, i);
            Mat &m = out.getMatRef(i);
            Mat(Mat(vec[i]).t()).copyTo(m);
        }
    }
    else if (out.isUMatVector()) {
        for (unsigned int i = 0; i < vec.size(); i++) {
            out.create(68, 1, CV_32FC2, i);
            UMat &m = out.getUMatRef(i);
            Mat(Mat(vec[i]).t()).copyTo(m);
        }
    }
    else if (out.kind() == _OutputArray::STD_VECTOR_VECTOR) {
        for (unsigned int i = 0; i < vec.size(); i++) {
            out.create(68, 1, CV_32FC2, i);
            Mat m = out.getMat(i);
            Mat(Mat(vec[i]).t()).copyTo(m);
        }
    }
    else {
        CV_Error(cv::Error::StsNotImplemented,
            "Only Mat vector, UMat vector, and vector<vector> OutputArrays are currently supported.");
    }
}


bool FacemarkAAMImpl::fit( InputArray image, InputArray roi, OutputArrayOfArrays _landmarks )
361 362 363 364 365
{
    std::vector<Config> config; // empty
    return fitConfig(image, roi, _landmarks, config);
}

366
bool FacemarkAAMImpl::fitConfig( InputArray image, InputArray roi, OutputArrayOfArrays _landmarks, const std::vector<Config> &configs )
Alexander Alekhin's avatar
Alexander Alekhin committed
367
{
368 369
    Mat roimat = roi.getMat();
    std::vector<Rect> faces = roimat.reshape(4, roimat.rows);
Alexander Alekhin's avatar
Alexander Alekhin committed
370
    if(faces.size()<1) return false;
371

372
    std::vector<std::vector<Point2f> > landmarks;
Alexander Alekhin's avatar
Alexander Alekhin committed
373
    landmarks.resize(faces.size());
374

Alexander Alekhin's avatar
Alexander Alekhin committed
375
    Mat img = image.getMat();
376
    if (! configs.empty()){
377

378
        if (configs.size()!=faces.size()) {
Alexander Alekhin's avatar
Alexander Alekhin committed
379
            CV_Error(Error::StsBadArg, "Number of faces and extra_parameters are different!");
380
        }
381 382
        for(size_t i=0; i<configs.size();i++){
            fitImpl(img, landmarks[i], configs[i].R,configs[i].t, configs[i].scale, configs[i].model_scale_idx);
Alexander Alekhin's avatar
Alexander Alekhin committed
383 384 385 386 387
        }
    }else{
        Mat R =  Mat::eye(2, 2, CV_32F);
        Point2f t = Point2f((float)(img.cols/2.0),(float)(img.rows/2.0));
        float scale = 1.0;
388

Alexander Alekhin's avatar
Alexander Alekhin committed
389 390 391
        for(unsigned i=0; i<faces.size();i++){
            fitImpl(img, landmarks[i], R,t, scale);
        }
392
    }
393
    _copyVector2Output(landmarks, _landmarks);
394

Alexander Alekhin's avatar
Alexander Alekhin committed
395 396
    return true;
}
397

Alexander Alekhin's avatar
Alexander Alekhin committed
398 399 400
bool FacemarkAAMImpl::fitImpl( const Mat image, std::vector<Point2f>& landmarks, const Mat R, const Point2f T, const  float scale, int _scl){
    if (landmarks.size()>0)
        landmarks.clear();
401

Alexander Alekhin's avatar
Alexander Alekhin committed
402
    CV_Assert(isModelTrained);
403

Alexander Alekhin's avatar
Alexander Alekhin committed
404 405
    int param_n = params.n, param_m = params.m;
    int scl = _scl<(int)AAM.scales.size()?_scl:(int)AAM.scales.size();
406

Alexander Alekhin's avatar
Alexander Alekhin committed
407 408
    /*variables*/
    std::vector<Point2f> s0 = Mat(Mat(AAM.s0)/AAM.scales[scl]).reshape(2);
409

Alexander Alekhin's avatar
Alexander Alekhin committed
410 411 412 413 414
    /*pre-computation*/
    Mat S = Mat(AAM.S, Range::all(), Range(0,param_n>AAM.S.cols?AAM.S.cols:param_n)).clone(); // chop the shape data
    std::vector<std::vector<int> > Tp;
    Mat Wx_dp, Wy_dp;
    createWarpJacobian(S, AAM.Q, AAM.triangles, AAM.textures[scl],Wx_dp, Wy_dp, Tp);
415

Alexander Alekhin's avatar
Alexander Alekhin committed
416 417 418
    std::vector<Point2f> s0_init = Mat(Mat(R*scale*AAM.scales[scl]*Mat(Mat(s0).reshape(1)).t()).t()).reshape(2);
    std::vector<Point2f> curr_shape =  Mat(Mat(s0_init)+Scalar(T.x,T.y));
    curr_shape = Mat(1.0/scale*Mat(curr_shape)).reshape(2);
419

Alexander Alekhin's avatar
Alexander Alekhin committed
420 421 422
    Mat imgray;
    Mat img;
    if(image.channels()>1){
Alexander Alekhin's avatar
Alexander Alekhin committed
423
        cvtColor(image,imgray,COLOR_BGR2GRAY);
Alexander Alekhin's avatar
Alexander Alekhin committed
424 425 426
    }else{
        imgray = image;
    }
427

428
    resize(imgray,img,Size(int(image.cols/scale),int(image.rows/scale)), 0, 0, INTER_LINEAR_EXACT);// matlab use bicubic interpolation, the result is float numbers
429

Alexander Alekhin's avatar
Alexander Alekhin committed
430 431 432 433
    /*chop the textures model*/
    int maxCol = param_m;
    if(AAM.textures[scl].A.cols<param_m)maxCol = AAM.textures[scl].A.cols;
    if(AAM.textures[scl].AA.cols<maxCol)maxCol = AAM.textures[scl].AA.cols;
434

Alexander Alekhin's avatar
Alexander Alekhin committed
435 436
    Mat A = Mat(AAM.textures[scl].A,Range(0,AAM.textures[scl].A.rows), Range(0,maxCol)).clone();
    Mat AA = Mat(AAM.textures[scl].AA,Range(0,AAM.textures[scl].AA.rows), Range(0,maxCol)).clone();
437

Alexander Alekhin's avatar
Alexander Alekhin committed
438 439 440 441 442 443 444 445
    /*iteratively update the fitting*/
    Mat I, II, warped, c, gx, gy, Irec, Irec_feat, dc;
    Mat refI, refII, refWarped, ref_c, ref_gx, ref_gy, refIrec, refIrec_feat, ref_dc ;
    for(int t=0;t<params.n_iter;t++){
        warped = warpImage(img,AAM.textures[scl].base_shape, curr_shape,
                           AAM.triangles,
                           AAM.textures[scl].resolution ,
                           AAM.textures[scl].textureIdx);
446

Alexander Alekhin's avatar
Alexander Alekhin committed
447 448
        I = getFeature<uchar>(warped, AAM.textures[scl].ind1);
        II = getFeature<uchar>(warped, AAM.textures[scl].ind2);
449

Alexander Alekhin's avatar
Alexander Alekhin committed
450 451 452 453 454
        if(t==0){
            c = A.t()*(I-AAM.textures[scl].A0); //little bit different to matlab, probably due to datatype
        }else{
            c = c+dc;
        }
455

Alexander Alekhin's avatar
Alexander Alekhin committed
456 457
        Irec_feat = (AAM.textures[scl].A0+A*c);
        Irec = Mat::zeros(AAM.textures[scl].resolution.width, AAM.textures[scl].resolution.height, CV_32FC1);
458

Alexander Alekhin's avatar
Alexander Alekhin committed
459 460 461 462
        for(int j=0;j<(int)AAM.textures[scl].ind1.size();j++){
            Irec.at<float>(AAM.textures[scl].ind1[j]) = Irec_feat.at<float>(j);
        }
        Mat irec = Irec.t();
463

Alexander Alekhin's avatar
Alexander Alekhin committed
464
        gradient(irec, gx, gy);
465

Alexander Alekhin's avatar
Alexander Alekhin committed
466 467
        Mat Jc;
        image_jacobian(Mat(gx.t()).reshape(0,1).t(),Mat(gy.t()).reshape(0,1).t(),Wx_dp, Wy_dp,Jc);
468

Alexander Alekhin's avatar
Alexander Alekhin committed
469 470 471 472 473
        Mat J;
        std::vector<float> Irec_vec;
        for(size_t j=0;j<AAM.textures[scl].ind2.size();j++){
            J.push_back(Jc.row(AAM.textures[scl].ind2[j]));
            Irec_vec.push_back(Irec.at<float>(AAM.textures[scl].ind2[j]));
474
        }
Alexander Alekhin's avatar
Alexander Alekhin committed
475 476 477 478 479 480 481 482 483 484 485

        /*compute Jfsic and Hfsic*/
        Mat Jfsic = J - AA*(AA.t()*J);
        Mat Hfsic = Jfsic.t()*Jfsic;
        Mat iHfsic;
        invert(Hfsic, iHfsic);

        /*compute dp dq and dc*/
        Mat dqp = iHfsic*Jfsic.t()*(II-AAM.textures[scl].AA0);
        dc = AA.t()*(II-Mat(Irec_vec)-J*dqp);
        warpUpdate(curr_shape, dqp, s0,S, AAM.Q, AAM.triangles,Tp);
486
    }
Alexander Alekhin's avatar
Alexander Alekhin committed
487 488 489
    landmarks = Mat(scale*Mat(curr_shape)).reshape(2);
    return true;
}
490

Alexander Alekhin's avatar
Alexander Alekhin committed
491 492 493 494 495 496 497
void FacemarkAAMImpl::saveModel(String s){
    FileStorage fs(s.c_str(),FileStorage::WRITE_BASE64);
    fs << "AAM_tri" << AAM.triangles;
    fs << "scales" << AAM.scales;
    fs << "s0" << AAM.s0;
    fs << "S" << AAM.S;
    fs << "Q" << AAM.Q;
498

Alexander Alekhin's avatar
Alexander Alekhin committed
499 500 501 502
    String x;
    for(int i=0;i< (int)AAM.scales.size();i++){
        x = cv::format("scale%i_max_m",i);
        fs << x << AAM.textures[i].max_m;
503

Alexander Alekhin's avatar
Alexander Alekhin committed
504 505
        x = cv::format("scale%i_resolution",i);
        fs << x << AAM.textures[i].resolution;
506

Alexander Alekhin's avatar
Alexander Alekhin committed
507 508
        x = cv::format("scale%i_textureIdx",i);
        fs << x << AAM.textures[i].textureIdx;
509

Alexander Alekhin's avatar
Alexander Alekhin committed
510 511
        x = cv::format("scale%i_base_shape",i);
        fs << x << AAM.textures[i].base_shape;
512

Alexander Alekhin's avatar
Alexander Alekhin committed
513 514
        x = cv::format("scale%i_A",i);
        fs << x << AAM.textures[i].A;
515

Alexander Alekhin's avatar
Alexander Alekhin committed
516 517
        x = cv::format("scale%i_A0",i);
        fs << x << AAM.textures[i].A0;
518

Alexander Alekhin's avatar
Alexander Alekhin committed
519 520
        x = cv::format("scale%i_AA",i);
        fs << x << AAM.textures[i].AA;
521

Alexander Alekhin's avatar
Alexander Alekhin committed
522 523
        x = cv::format("scale%i_AA0",i);
        fs << x << AAM.textures[i].AA0;
524

Alexander Alekhin's avatar
Alexander Alekhin committed
525 526
        x = cv::format("scale%i_ind1",i);
        fs << x << AAM.textures[i].ind1;
527

Alexander Alekhin's avatar
Alexander Alekhin committed
528 529
        x = cv::format("scale%i_ind2",i);
        fs << x << AAM.textures[i].ind2;
530 531

    }
Alexander Alekhin's avatar
Alexander Alekhin committed
532 533 534
    fs.release();
    if(params.verbose) printf("The model is successfully saved! \n");
}
535

Alexander Alekhin's avatar
Alexander Alekhin committed
536 537 538 539 540 541 542 543
void FacemarkAAMImpl::loadModel(String s){
    FileStorage fs(s.c_str(),FileStorage::READ);
    String x;
    fs["AAM_tri"] >> AAM.triangles;
    fs["scales"] >> AAM.scales;
    fs["s0"] >> AAM.s0;
    fs["S"] >> AAM.S;
    fs["Q"] >> AAM.Q;
544 545


Alexander Alekhin's avatar
Alexander Alekhin committed
546 547 548 549
    AAM.textures.resize(AAM.scales.size());
    for(int i=0;i< (int)AAM.scales.size();i++){
        x = cv::format("scale%i_max_m",i);
        fs[x] >> AAM.textures[i].max_m;
550

Alexander Alekhin's avatar
Alexander Alekhin committed
551 552
        x = cv::format("scale%i_resolution",i);
        fs[x] >> AAM.textures[i].resolution;
553

Alexander Alekhin's avatar
Alexander Alekhin committed
554 555
        x = cv::format("scale%i_textureIdx",i);
        fs[x] >> AAM.textures[i].textureIdx;
556

Alexander Alekhin's avatar
Alexander Alekhin committed
557 558
        x = cv::format("scale%i_base_shape",i);
        fs[x] >> AAM.textures[i].base_shape;
559

Alexander Alekhin's avatar
Alexander Alekhin committed
560 561
        x = cv::format("scale%i_A",i);
        fs[x] >> AAM.textures[i].A;
562

Alexander Alekhin's avatar
Alexander Alekhin committed
563 564
        x = cv::format("scale%i_A0",i);
        fs[x] >> AAM.textures[i].A0;
565

Alexander Alekhin's avatar
Alexander Alekhin committed
566 567
        x = cv::format("scale%i_AA",i);
        fs[x] >> AAM.textures[i].AA;
568

Alexander Alekhin's avatar
Alexander Alekhin committed
569 570
        x = cv::format("scale%i_AA0",i);
        fs[x] >> AAM.textures[i].AA0;
571

Alexander Alekhin's avatar
Alexander Alekhin committed
572 573
        x = cv::format("scale%i_ind1",i);
        fs[x] >> AAM.textures[i].ind1;
574

Alexander Alekhin's avatar
Alexander Alekhin committed
575 576
        x = cv::format("scale%i_ind2",i);
        fs[x] >> AAM.textures[i].ind2;
577 578
    }

Alexander Alekhin's avatar
Alexander Alekhin committed
579 580 581 582
    fs.release();
    isModelTrained = true;
    if(params.verbose) printf("the model has been loaded\n");
}
583

Alexander Alekhin's avatar
Alexander Alekhin committed
584
Mat FacemarkAAMImpl::procrustes(std::vector<Point2f> P, std::vector<Point2f> Q, Mat & rot, Scalar & trans, float & scale){
585

Alexander Alekhin's avatar
Alexander Alekhin committed
586 587 588
    // calculate average
    Scalar mx = mean(P);
    Scalar my = mean(Q);
589

Alexander Alekhin's avatar
Alexander Alekhin committed
590 591 592
    // zero centered data
    Mat X0 = Mat(P) - mx;
    Mat Y0 = Mat(Q) - my;
593

Alexander Alekhin's avatar
Alexander Alekhin committed
594 595 596 597
    // calculate magnitude
    Mat Xs, Ys;
    multiply(X0,X0,Xs);
    multiply(Y0,Y0,Ys);
598

Alexander Alekhin's avatar
Alexander Alekhin committed
599 600
    // calculate the sum
    Mat sumXs, sumYs;
Alexander Alekhin's avatar
Alexander Alekhin committed
601 602
    reduce(Xs,sumXs, 0, REDUCE_SUM);
    reduce(Ys,sumYs, 0, REDUCE_SUM);
603

Alexander Alekhin's avatar
Alexander Alekhin committed
604 605 606
    //calculate the normrnd
    double normX = sqrt(Mat(sumXs.reshape(1)).at<float>(0)+Mat(sumXs.reshape(1)).at<float>(1));
    double normY = sqrt(Mat(sumYs.reshape(1)).at<float>(0)+Mat(sumYs.reshape(1)).at<float>(1));
607

Alexander Alekhin's avatar
Alexander Alekhin committed
608 609 610
    //normalization
    X0 = X0/normX;
    Y0 = Y0/normY;
611

Alexander Alekhin's avatar
Alexander Alekhin committed
612 613 614
    //reshape, convert to 2D Matrix
    Mat Xn=X0.reshape(1);
    Mat Yn=Y0.reshape(1);
615

Alexander Alekhin's avatar
Alexander Alekhin committed
616 617
    //calculate the covariance matrix
    Mat M = Xn.t()*Yn;
618

Alexander Alekhin's avatar
Alexander Alekhin committed
619 620 621
    // decompose
    Mat U,S,Vt;
    SVD::compute(M, S, U, Vt);
622

Alexander Alekhin's avatar
Alexander Alekhin committed
623 624 625
    // extract the transformations
    scale = (S.at<float>(0)+S.at<float>(1))*(float)normX/(float)normY;
    rot = Vt.t()*U.t();
626

Alexander Alekhin's avatar
Alexander Alekhin committed
627 628 629 630
    Mat muX(mx),mX; muX.pop_back();muX.pop_back();
    Mat muY(my),mY; muY.pop_back();muY.pop_back();
    muX.convertTo(mX,CV_32FC1);
    muY.convertTo(mY,CV_32FC1);
631

Alexander Alekhin's avatar
Alexander Alekhin committed
632 633 634
    Mat t = mX.t()-scale*mY.t()*rot;
    trans[0] = t.at<float>(0);
    trans[1] = t.at<float>(1);
635

Alexander Alekhin's avatar
Alexander Alekhin committed
636 637
    // calculate the recovered form
    Mat Qmat = Mat(Q).reshape(1);
638

Alexander Alekhin's avatar
Alexander Alekhin committed
639 640
    return Mat(scale*Qmat*rot+trans).clone();
}
641

Alexander Alekhin's avatar
Alexander Alekhin committed
642
void FacemarkAAMImpl::procrustesAnalysis(std::vector<std::vector<Point2f> > shapes, std::vector<std::vector<Point2f> > & normalized, std::vector<Point2f> & new_mean){
643

Alexander Alekhin's avatar
Alexander Alekhin committed
644 645
    std::vector<Scalar> mean_every_shape;
    mean_every_shape.resize(shapes.size());
646

Alexander Alekhin's avatar
Alexander Alekhin committed
647
    Point2f temp;
648

Alexander Alekhin's avatar
Alexander Alekhin committed
649 650 651
    // calculate the mean of every shape
    for(size_t i=0; i< shapes.size();i++){
        mean_every_shape[i] = mean(shapes[i]);
652 653
    }

Alexander Alekhin's avatar
Alexander Alekhin committed
654 655 656 657 658
    //normalize every shapes
    Mat tShape;
    normalized.clear();
    for(size_t i=0; i< shapes.size();i++){
        normalized.push_back((Mat)(Mat(shapes[i]) - mean_every_shape[i]));
659 660
    }

Alexander Alekhin's avatar
Alexander Alekhin committed
661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676
    // calculate the mean shape
    std::vector<Point2f> mean_shape;
    calcMeanShape(normalized, mean_shape);

    // update the mean shape and normalized shapes iteratively
    int maxIter = 100;
    Mat R;
    Scalar t;
    float s;
    Mat aligned;
    for(int i=0;i<maxIter;i++){
        // align
        for(unsigned k=0;k< normalized.size();k++){
            aligned=procrustes(mean_shape, normalized[k], R, t, s);
            aligned.reshape(2).copyTo(normalized[k]);
        }
677

Alexander Alekhin's avatar
Alexander Alekhin committed
678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695
        //calc new mean
        calcMeanShape(normalized, new_mean);
        // align the new mean
        aligned=procrustes(mean_shape, new_mean, R, t, s);
        // update
        aligned.reshape(2).copyTo(mean_shape);
    }
}

void FacemarkAAMImpl::calcMeanShape(std::vector<std::vector<Point2f> > shapes,std::vector<Point2f> & mean){
    mean.resize(shapes[0].size());
    Point2f tmp;
    for(unsigned i=0;i<shapes[0].size();i++){
        tmp.x=0;
        tmp.y=0;
        for(unsigned k=0;k< shapes.size();k++){
            tmp.x+= shapes[k][i].x;
            tmp.y+= shapes[k][i].y;
696
        }
Alexander Alekhin's avatar
Alexander Alekhin committed
697 698 699
        tmp.x/=shapes.size();
        tmp.y/=shapes.size();
        mean[i] = tmp;
700
    }
Alexander Alekhin's avatar
Alexander Alekhin committed
701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737
}

void FacemarkAAMImpl::getProjection(const Mat M, Mat & P,  int n){
    Mat U,S,Vt,S1, Ut;
    int k;
    if(M.rows < M.cols){
        // SVD::compute(M*M.t(), S, U, Vt);
        eigen(M*M.t(), S, Ut); U=Ut.t();

        // find the minimum between number of non-zero eigval,
        // compressed dim, row, and column
        // threshold(S,S1,0.00001,1,THRESH_BINARY);
        k= S.rows; //countNonZero(S1);
        if(k>n)k=n;
        if(k>M.rows)k=M.rows;
        if(k>M.cols)k=M.cols;

        // cut the column of eigen vector
        U.colRange(0,k).copyTo(P);
    }else{
        // SVD::compute(M.t()*M, S, U, Vt);
        eigen(M.t()*M, S, Ut);U=Ut.t();

        // threshold(S,S1,0.00001,1,THRESH_BINARY);
        k= S.rows; //countNonZero(S1);
        if(k>n)k=n;
        if(k>M.rows)k=M.rows;
        if(k>M.cols)k=M.cols;

        // cut the eigen values to k-amount
        Mat D = Mat::zeros(k,k,CV_32FC1);
        Mat diag = D.diag();
        Mat s; pow(S,-0.5,s);
        s(Range(0,k), Range::all()).copyTo(diag);

        // cut the eigen vector to k-column,
        P = Mat(M*U.colRange(0,k)*D).clone();
738

Alexander Alekhin's avatar
Alexander Alekhin committed
739 740
    }
}
741

Alexander Alekhin's avatar
Alexander Alekhin committed
742 743 744
Mat FacemarkAAMImpl::orthonormal(Mat Mo){
    Mat M;
    Mo.convertTo(M,CV_32FC1);
745

Alexander Alekhin's avatar
Alexander Alekhin committed
746 747
    // TODO: float precission is only 1e-7, but MATLAB version use thresh=2.2204e-16
    float thresh = (float)2.2204e-6;
748

Alexander Alekhin's avatar
Alexander Alekhin committed
749
    Mat O = Mat::zeros(M.rows, M.cols, CV_32FC1);
750

Alexander Alekhin's avatar
Alexander Alekhin committed
751
    int k = 0; //storing index
752

Alexander Alekhin's avatar
Alexander Alekhin committed
753 754 755 756
    Mat w,nv;
    float n;
    for(int i=0;i<M.cols;i++){
        Mat v = M.col(i); // processed column to orthogonalize
757

Alexander Alekhin's avatar
Alexander Alekhin committed
758 759 760 761 762
        // subtract projection over previous vectors
        for(int j=0;j<k;j++){
            Mat o=O.col(j);
            w = v-o*(o.t()*v);
            w.copyTo(v);
763 764
        }

Alexander Alekhin's avatar
Alexander Alekhin committed
765 766 767 768 769 770 771 772 773
        // only keep non zero vector
        n = (float)norm(v);
        if(n>thresh){
            Mat ok=O.col(k);
            // nv=v/n;
            normalize(v,nv);
            nv.copyTo(ok);
            k+=1;
        }
774 775 776

    }

Alexander Alekhin's avatar
Alexander Alekhin committed
777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838
    return O.colRange(0,k).clone();
}

void FacemarkAAMImpl::calcSimilarityEig(std::vector<Point2f> s0,Mat S, Mat & Q_orth, Mat & S_orth){
    int npts = (int)s0.size();

    Mat Q = Mat::zeros(2*npts,4,CV_32FC1);
    Mat c0 = Q.col(0);
    Mat c1 = Q.col(1);
    Mat c2 = Q.col(2);
    Mat c3 = Q.col(3);

    /*c0 = s0(:)*/
    Mat w = linearize(s0);
    // w.convertTo(w, CV_64FC1);
    w.copyTo(c0);

    /*c1 = [-s0(npts:2*npts); s0(0:npts-1)]*/
    Mat s0_mat = Mat(s0).reshape(1);
    // s0_mat.convertTo(s0_mat, CV_64FC1);
    Mat swapper = Mat::zeros(2,npts,CV_32FC1);
    Mat s00 = s0_mat.col(0);
    Mat s01 = s0_mat.col(1);
    Mat sw0 = swapper.row(0);
    Mat sw1 = swapper.row(1);
    Mat(s00.t()).copyTo(sw1);
    s01 = -s01;
    Mat(s01.t()).copyTo(sw0);

    Mat(swapper.reshape(1,2*npts)).copyTo(c1);

    /*c2 - [ones(npts); zeros(npts)]*/
    Mat ones = Mat::ones(1,npts,CV_32FC1);
    Mat c2_mat = Mat::zeros(2,npts,CV_32FC1);
    Mat c20 = c2_mat.row(0);
    ones.copyTo(c20);
    Mat(c2_mat.reshape(1,2*npts)).copyTo(c2);

    /*c3 - [zeros(npts); ones(npts)]*/
    Mat c3_mat = Mat::zeros(2,npts,CV_32FC1);
    Mat c31 = c3_mat.row(1);
    ones.copyTo(c31);
    Mat(c3_mat.reshape(1,2*npts)).copyTo(c3);

    Mat Qo = orthonormal(Q);

    Mat all = Qo.t();
    all.push_back(S.t());

    Mat allOrth = orthonormal(all.t());
    Q_orth =  allOrth.colRange(0,4).clone();
    S_orth =  allOrth.colRange(4,allOrth.cols).clone();

}

inline Mat FacemarkAAMImpl::linearize(Mat s){ // all x values and then all y values
    return Mat(s.reshape(1).t()).reshape(1,2*s.rows);
}
inline Mat FacemarkAAMImpl::linearize(std::vector<Point2f> s){ // all x values and then all y values
    return linearize(Mat(s));
}

839 840
void FacemarkAAMImpl::delaunay(std::vector<Point2f> s, std::vector<Vec3i> & triangles)
{
Alexander Alekhin's avatar
Alexander Alekhin committed
841 842 843 844 845 846 847 848 849 850 851
    triangles.clear();

    std::vector<Vec6f> tp;

    double min_x, max_x, min_y, max_y;
    Mat S = Mat(s).reshape(1);
    Mat s_x = S.col(0);
    Mat s_y = S.col(1);
    minMaxIdx(s_x, &min_x, &max_x);
    minMaxIdx(s_y, &min_y, &max_y);

852 853 854 855 856 857 858 859 860 861 862 863
    // TODO FIXIT Some triangles are lost
    //Subdiv2D subdiv(Rect(cvFloor(min_x), cvFloor(min_y), cvCeil(max_x) - cvFloor(min_x), cvCeil(max_y) - cvFloor(min_y)));
    Subdiv2D subdiv(Rect(cvFloor(min_x) - 10, cvFloor(min_y) - 10, cvCeil(max_x) - cvFloor(min_x) + 20, cvCeil(max_y) - cvFloor(min_y) + 20));

    // map subdiv_verter -> original point (or the first alias)
    std::vector<int> idx(s.size() + 4);
    for (size_t i = 0; i < s.size(); ++i)
    {
        int vertex = subdiv.insert(s[i]);
        if (idx.size() <= (size_t)vertex)
            idx.resize(vertex + 1);
        idx[vertex] = (int)i;
864 865
    }

Alexander Alekhin's avatar
Alexander Alekhin committed
866 867
    subdiv.getTriangleList(tp);

868 869 870
    for (size_t i = 0; i < tp.size(); i++)
    {
        const Vec6f& t = tp[i];
Alexander Alekhin's avatar
Alexander Alekhin committed
871 872

        //accept only vertex point
873 874 875 876 877 878 879 880 881 882 883
        CV_Assert(
            t[0]>=min_x && t[0]<=max_x && t[1]>=min_y && t[1]<=max_y &&
            t[2]>=min_x && t[2]<=max_x && t[3]>=min_y && t[3]<=max_y &&
            t[4]>=min_x && t[4]<=max_x && t[5]>=min_y && t[5]<=max_y
        );

        int tmp = 0, v1 = 0, v2 = 0, v3 = 0;
        subdiv.locate(Point2f(t[0], t[1]), tmp, v1);
        subdiv.locate(Point2f(t[2], t[3]), tmp, v2);
        subdiv.locate(Point2f(t[4], t[5]), tmp, v3);
        triangles.push_back(Vec3i(idx[v1], idx[v2], idx[v3]));
Alexander Alekhin's avatar
Alexander Alekhin committed
884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906
    } // for
}

Mat FacemarkAAMImpl::createMask(std::vector<Point2f> base_shape,  Rect res){
    Mat mask = Mat::zeros(res.height, res.width, CV_8U);
    std::vector<Point> hull;
    std::vector<Point> shape;
    Mat(base_shape).convertTo(shape, CV_32S);
    convexHull(shape,hull);
    fillConvexPoly(mask, &hull[0], (int)hull.size(), 255, 8 ,0);
    return mask.clone();
}

Mat FacemarkAAMImpl::createTextureBase(std::vector<Point2f> shape, std::vector<Vec3i> triangles, Rect res, std::vector<std::vector<Point> > & textureIdx){
    // max supported amount of triangles only 255
    Mat mask = Mat::zeros(res.height, res.width, CV_8U);

    std::vector<Point2f> p(3);
    textureIdx.clear();
    for(size_t i=0;i<triangles.size();i++){
        p[0] = shape[triangles[i][0]];
        p[1] = shape[triangles[i][1]];
        p[2] = shape[triangles[i][2]];
907 908


Alexander Alekhin's avatar
Alexander Alekhin committed
909 910 911 912 913 914 915 916 917 918 919
        std::vector<Point> polygon;
        approxPolyDP(p,polygon, 1.0, true);
        fillConvexPoly(mask, &polygon[0], (int)polygon.size(), (double)i+1,8,0 );

        std::vector<Point> list;
        for(int y=0;y<res.height;y++){
            for(int x=0;x<res.width;x++){
                if(mask.at<uchar>(y,x)==(uchar)(i+1)){
                    list.push_back(Point(x,y));
                }
            }
920
        }
Alexander Alekhin's avatar
Alexander Alekhin committed
921
        textureIdx.push_back(list);
922 923 924

    }

Alexander Alekhin's avatar
Alexander Alekhin committed
925 926 927 928 929 930 931 932 933 934 935 936 937 938
    return mask.clone();
}

Mat FacemarkAAMImpl::warpImage(
    const Mat img, const std::vector<Point2f> target_shape,
    const std::vector<Point2f> curr_shape, const std::vector<Vec3i> triangles,
    const Rect res, const std::vector<std::vector<Point> > textureIdx)
{
    // TODO: this part can be optimized, collect tranformation pair form all triangles first, then do one time remapping
    Mat warped = Mat::zeros(res.height, res.width, CV_8U);
    Mat warped2 = Mat::zeros(res.height, res.width, CV_8U);
    Mat image,part, warped_part;

    if(img.channels()>1){
Alexander Alekhin's avatar
Alexander Alekhin committed
939
        cvtColor(img,image,COLOR_BGR2GRAY);
Alexander Alekhin's avatar
Alexander Alekhin committed
940 941
    }else{
        image = img;
942 943
    }

Alexander Alekhin's avatar
Alexander Alekhin committed
944 945 946 947 948 949 950 951 952 953 954 955 956 957 958 959 960 961 962 963 964 965 966 967 968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985 986 987 988 989 990 991 992 993 994 995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006 1007
    Mat A,R,t;
    A = Mat::zeros(2,3,CV_64F);
    std::vector<Point2f> target(3),source(3);
    std::vector<Point> polygon;
    for(size_t i=0;i<triangles.size();i++){
        target[0] = target_shape[triangles[i][0]];
        target[1] = target_shape[triangles[i][1]];
        target[2] = target_shape[triangles[i][2]];

        source[0] = curr_shape[triangles[i][0]];
        source[1] = curr_shape[triangles[i][1]];
        source[2] = curr_shape[triangles[i][2]];

        Mat target_mtx = Mat(target).reshape(1)-1.0;
        Mat source_mtx = Mat(source).reshape(1)-1.0;
        Mat U = target_mtx.col(0);
        Mat V = target_mtx.col(1);
        Mat X = source_mtx.col(0);
        Mat Y = source_mtx.col(1);

        double denominator = (target[1].x-target[0].x)*(target[2].y-target[0].y)-
                            (target[1].y-target[0].y)*(target[2].x-target[0].x);
        // denominator = 1.0/denominator;

        A.at<double>(0) = ((target[2].y-target[0].y)*(source[1].x-source[0].x)-
                         (target[1].y-target[0].y)*(source[2].x-source[0].x))/denominator;
        A.at<double>(1) = ((target[1].x-target[0].x)*(source[2].x-source[0].x)-
                         (target[2].x-target[0].x)*(source[1].x-source[0].x))/denominator;
        A.at<double>(2) =X.at<float>(0) + ((V.at<float>(0) * (U.at<float>(2) - U.at<float>(0)) - U.at<float>(0)*(V.at<float>(2) - V.at<float>(0))) * (X.at<float>(1) - X.at<float>(0)) + (U.at<float>(0) * (V.at<float>(1) - V.at<float>(0)) - V.at<float>(0)*(U.at<float>(1) - U.at<float>(0))) * (X.at<float>(2) - X.at<float>(0))) / denominator;
        A.at<double>(3) =((V.at<float>(2) - V.at<float>(0)) * (Y.at<float>(1) - Y.at<float>(0)) - (V.at<float>(1) - V.at<float>(0)) * (Y.at<float>(2) - Y.at<float>(0))) / denominator;
        A.at<double>(4) = ((U.at<float>(1) - U.at<float>(0)) * (Y.at<float>(2) - Y.at<float>(0)) - (U.at<float>(2) - U.at<float>(0)) * (Y.at<float>(1) - Y.at<float>(0))) / denominator;
        A.at<double>(5) = Y.at<float>(0) + ((V.at<float>(0) * (U.at<float>(2) - U.at<float>(0)) - U.at<float>(0) * (V.at<float>(2) - V.at<float>(0))) * (Y.at<float>(1) - Y.at<float>(0)) + (U.at<float>(0) * (V.at<float>(1) - V.at<float>(0)) - V.at<float>(0)*(U.at<float>(1) - U.at<float>(0))) * (Y.at<float>(2) - Y.at<float>(0))) / denominator;

        // A = getAffineTransform(target,source);

        R=A.colRange(0,2);
        t=A.colRange(2,3);

        Mat pts_ori = Mat(textureIdx[i]).reshape(1);
        Mat pts = pts_ori.t(); //matlab
        Mat bx = pts_ori.col(0);
        Mat by = pts_ori.col(1);

        Mat base_ind = (by-1)*res.width+bx;

        Mat pts_f;
        pts.convertTo(pts_f,CV_64FC1);
        pts_f.push_back(Mat::ones(1,(int)textureIdx[i].size(),CV_64FC1));

        Mat trans = (A*pts_f).t();

        Mat T; trans.convertTo(T, CV_32S); // this rounding make the result a little bit different to matlab
        Mat mx = T.col(0);
        Mat my = T.col(1);

        Mat ind = (my-1)*image.cols+mx;
        int maxIdx = image.rows*image.cols;
        int idx;

        for(int k=0;k<ind.rows;k++){
            idx=ind.at<int>(k);
            if(idx>=0 && idx<maxIdx){
                warped.at<uchar>(base_ind.at<int>(k)) = (uchar)(image.at<uchar>(idx));
            }
1008

Alexander Alekhin's avatar
Alexander Alekhin committed
1009 1010 1011
        }
        warped.copyTo(warped2);
    }
1012

Alexander Alekhin's avatar
Alexander Alekhin committed
1013 1014
    return warped2.clone();
}
1015

Alexander Alekhin's avatar
Alexander Alekhin committed
1016 1017 1018 1019 1020 1021 1022 1023 1024
template <class T>
Mat FacemarkAAMImpl::getFeature(const Mat m, std::vector<int> map){
    std::vector<float> feat;
    Mat M = m.t();//matlab
    for(size_t i=0;i<map.size();i++){
        feat.push_back((float)M.at<T>(map[i]));
    }
    return Mat(feat).clone();
}
1025

Alexander Alekhin's avatar
Alexander Alekhin committed
1026
void FacemarkAAMImpl::createMaskMapping(const Mat m1, const Mat m2, std::vector<int> & ind1, std::vector<int> & ind2, std::vector<int> & ind3){
1027

Alexander Alekhin's avatar
Alexander Alekhin committed
1028
    int cnt = 0, idx=0;
1029

Alexander Alekhin's avatar
Alexander Alekhin committed
1030 1031 1032
    ind1.clear();
    ind2.clear();
    ind3.clear();
1033

Alexander Alekhin's avatar
Alexander Alekhin committed
1034 1035
    Mat mask = m1.t();//matlab
    Mat mask2 = m2.t();//matlab
1036

Alexander Alekhin's avatar
Alexander Alekhin committed
1037 1038 1039 1040 1041 1042
    for(int i=0;i<mask.rows;i++){
        for(int j=0;j<mask.cols;j++){
            if(mask.at<uchar>(i,j)>0){
                if(mask2.at<uchar>(i,j)>0){
                    ind2.push_back(idx);
                    ind3.push_back(cnt);
1043 1044
                }

Alexander Alekhin's avatar
Alexander Alekhin committed
1045 1046 1047
                ind1.push_back(idx);

                cnt +=1;
1048
            }
Alexander Alekhin's avatar
Alexander Alekhin committed
1049 1050 1051
            idx+=1;
        } // j
    } // i
1052

Alexander Alekhin's avatar
Alexander Alekhin committed
1053
}
1054

Alexander Alekhin's avatar
Alexander Alekhin committed
1055
void FacemarkAAMImpl::image_jacobian(const Mat gx, const Mat gy, const Mat Jx, const Mat Jy, Mat & G){
1056

Alexander Alekhin's avatar
Alexander Alekhin committed
1057 1058
    Mat Gx = repeat(gx,1,Jx.cols);
    Mat Gy = repeat(gy,1,Jx.cols);
1059

Alexander Alekhin's avatar
Alexander Alekhin committed
1060 1061 1062
    Mat G1,G2;
    multiply(Gx,Jx,G1);
    multiply(Gy,Jy,G2);
1063

Alexander Alekhin's avatar
Alexander Alekhin committed
1064 1065
    G=G1+G2;
}
1066

Alexander Alekhin's avatar
Alexander Alekhin committed
1067 1068 1069
void FacemarkAAMImpl::warpUpdate(std::vector<Point2f> & shape, Mat delta, std::vector<Point2f> s0, Mat S, Mat Q, std::vector<Vec3i> triangles,std::vector<std::vector<int> > Tp){
    std::vector<Point2f> new_shape;
    int nSimEig = 4;
1070

Alexander Alekhin's avatar
Alexander Alekhin committed
1071 1072 1073
    /*get dr, dp and compute ds0*/
    Mat dr = -Mat(delta, Range(0,nSimEig));
    Mat dp = -Mat(delta, Range(nSimEig, delta.rows));
1074 1075


Alexander Alekhin's avatar
Alexander Alekhin committed
1076 1077 1078 1079 1080 1081
    Mat ds0 = S*dp + Q*dr;
    Mat ds0_mat = Mat::zeros((int)s0.size(),2, CV_32FC1);
    Mat c0 = ds0_mat.col(0);
    Mat c1 = ds0_mat.col(1);
    Mat(ds0, Range(0,(int)s0.size())).copyTo(c0);
    Mat(ds0, Range((int)s0.size(),(int)s0.size()*2)).copyTo(c1);
1082

Alexander Alekhin's avatar
Alexander Alekhin committed
1083
    Mat s_new = computeWarpParts(shape,s0,ds0_mat, triangles, Tp);
1084

Alexander Alekhin's avatar
Alexander Alekhin committed
1085
    Mat diff =linearize(Mat(s_new - Mat(s0).reshape(1)));
1086

Alexander Alekhin's avatar
Alexander Alekhin committed
1087 1088
    Mat r = Q.t()*diff;
    Mat p = S.t()*diff;
1089

Alexander Alekhin's avatar
Alexander Alekhin committed
1090 1091 1092
    Mat s = linearize(s0)  +S*p + Q*r;
    Mat(Mat(s.t()).reshape(0,2).t()).reshape(2).copyTo(shape);
}
1093

Alexander Alekhin's avatar
Alexander Alekhin committed
1094
Mat FacemarkAAMImpl::computeWarpParts(std::vector<Point2f> curr_shape,std::vector<Point2f> s0, Mat ds0, std::vector<Vec3i> triangles,std::vector<std::vector<int> > Tp){
1095

Alexander Alekhin's avatar
Alexander Alekhin committed
1096 1097
    std::vector<Point2f> new_shape;
    std::vector<Point2f> ds = ds0.reshape(2);
1098

Alexander Alekhin's avatar
Alexander Alekhin committed
1099 1100 1101 1102 1103 1104 1105 1106
    float mx,my;
    Mat A;
    std::vector<Point2f> target(3),source(3);
    std::vector<double> p(3);
    p[2] = 1;
    for(size_t i=0;i<s0.size();i++){
        p[0] = s0[i].x + ds[i].x;
        p[1] = s0[i].y + ds[i].y;
1107

Alexander Alekhin's avatar
Alexander Alekhin committed
1108 1109 1110 1111 1112 1113 1114
        std::vector<Point2f> v;
        std::vector<float>vx, vy;
        for(size_t j=0;j<Tp[i].size();j++){
            int idx = Tp[i][j];
            target[0] = s0[triangles[idx][0]];
            target[1] = s0[triangles[idx][1]];
            target[2] = s0[triangles[idx][2]];
1115

Alexander Alekhin's avatar
Alexander Alekhin committed
1116 1117 1118
            source[0] = curr_shape[triangles[idx][0]];
            source[1] = curr_shape[triangles[idx][1]];
            source[2] = curr_shape[triangles[idx][2]];
1119

Alexander Alekhin's avatar
Alexander Alekhin committed
1120
            A = getAffineTransform(target,source);
1121

Alexander Alekhin's avatar
Alexander Alekhin committed
1122 1123 1124 1125
            Mat(A*Mat(p)).reshape(2).copyTo(v);
            vx.push_back(v[0].x);
            vy.push_back(v[0].y);
        }// j
1126

Alexander Alekhin's avatar
Alexander Alekhin committed
1127 1128 1129 1130 1131 1132
        /*find the median*/
        size_t n = vx.size()/2;
        nth_element(vx.begin(), vx.begin()+n, vx.end());
        mx = vx[n];
        nth_element(vy.begin(), vy.begin()+n, vy.end());
        my = vy[n];
1133

Alexander Alekhin's avatar
Alexander Alekhin committed
1134 1135
        new_shape.push_back(Point2f(mx,my));
    } // s0.size()
1136

Alexander Alekhin's avatar
Alexander Alekhin committed
1137 1138
    return Mat(new_shape).reshape(1).clone();
}
1139

Alexander Alekhin's avatar
Alexander Alekhin committed
1140 1141 1142
void FacemarkAAMImpl::gradient(const Mat M, Mat & gx, Mat & gy){
    gx = Mat::zeros(M.size(),CV_32FC1);
    gy = Mat::zeros(M.size(),CV_32FC1);
1143

Alexander Alekhin's avatar
Alexander Alekhin committed
1144 1145 1146 1147 1148 1149 1150 1151 1152
    /*gx*/
    for(int i=0;i<M.rows;i++){
        for(int j=0;j<M.cols;j++){
            if(j>0 && j<M.cols-1){
                gx.at<float>(i,j) = ((float)0.5)*(M.at<float>(i,j+1)-M.at<float>(i,j-1));
            }else if (j==0){
                gx.at<float>(i,j) = M.at<float>(i,j+1)-M.at<float>(i,j);
            }else{
                gx.at<float>(i,j) = M.at<float>(i,j)-M.at<float>(i,j-1);
1153 1154
            }

Alexander Alekhin's avatar
Alexander Alekhin committed
1155 1156
        }
    }
1157

Alexander Alekhin's avatar
Alexander Alekhin committed
1158 1159 1160 1161 1162 1163 1164 1165 1166
    /*gy*/
    for(int i=0;i<M.rows;i++){
        for(int j=0;j<M.cols;j++){
            if(i>0 && i<M.rows-1){
                gy.at<float>(i,j) = ((float)0.5)*(M.at<float>(i+1,j)-M.at<float>(i-1,j));
            }else if (i==0){
                gy.at<float>(i,j) = M.at<float>(i+1,j)-M.at<float>(i,j);
            }else{
                gy.at<float>(i,j) = M.at<float>(i,j)-M.at<float>(i-1,j);
1167 1168
            }

Alexander Alekhin's avatar
Alexander Alekhin committed
1169
        }
1170 1171
    }

Alexander Alekhin's avatar
Alexander Alekhin committed
1172
}
1173

Alexander Alekhin's avatar
Alexander Alekhin committed
1174
void FacemarkAAMImpl::createWarpJacobian(Mat S, Mat Q, std::vector<Vec3i> triangles, Model::Texture & T, Mat & Wx_dp, Mat & Wy_dp, std::vector<std::vector<int> > & Tp){
1175

Alexander Alekhin's avatar
Alexander Alekhin committed
1176 1177
    std::vector<Point2f> base_shape = T.base_shape;
    Rect resolution = T.resolution;
1178

Alexander Alekhin's avatar
Alexander Alekhin committed
1179
    std::vector<std::vector<int> >triangles_on_a_point;
1180

Alexander Alekhin's avatar
Alexander Alekhin committed
1181
    int npts = (int)base_shape.size();
1182

Alexander Alekhin's avatar
Alexander Alekhin committed
1183 1184 1185 1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196 1197 1198 1199 1200 1201 1202 1203 1204 1205 1206 1207 1208 1209 1210
    Mat dW_dxdyt ;
    /*get triangles for each point*/
    std::vector<int> trianglesIdx;
    triangles_on_a_point.resize(npts);
    for(int i=0;i<(int)triangles.size();i++){
        triangles_on_a_point[triangles[i][0]].push_back(i);
        triangles_on_a_point[triangles[i][1]].push_back(i);
        triangles_on_a_point[triangles[i][2]].push_back(i);
    }
    Tp = triangles_on_a_point;

    /*calculate dW_dxdy*/
    float v0x,v0y,v1x,v1y,v2x,v2y, denominator;
    for(int k=0;k<npts;k++){
        Mat acc = Mat::zeros(resolution.height, resolution.width, CV_32F);

        /*for each triangle on k-th point*/
        for(size_t i=0;i<triangles_on_a_point[k].size();i++){
            int tId = triangles_on_a_point[k][i];

            Vec3i v;
            if(triangles[tId][0]==k ){
                v=Vec3i(triangles[tId][0],triangles[tId][1],triangles[tId][2]);
            }else if(triangles[tId][1]==k){
                v=Vec3i(triangles[tId][1],triangles[tId][0],triangles[tId][2]);
            }else{
                v=Vec3i(triangles[tId][2],triangles[tId][0],triangles[tId][1]);
            }
1211

Alexander Alekhin's avatar
Alexander Alekhin committed
1212 1213 1214 1215 1216 1217
            v0x = base_shape[v[0]].x;
            v0y = base_shape[v[0]].y;
            v1x = base_shape[v[1]].x;
            v1y = base_shape[v[1]].y;
            v2x = base_shape[v[2]].x;
            v2y = base_shape[v[2]].y;
1218

Alexander Alekhin's avatar
Alexander Alekhin committed
1219
            denominator = (v1x-v0x)*(v2y-v0y)-(v1y-v0y)*(v2x-v0x);
1220

Alexander Alekhin's avatar
Alexander Alekhin committed
1221 1222
            Mat pixels = Mat(T.textureIdx[tId]).reshape(1); // same, just different order
            Mat p;
1223

Alexander Alekhin's avatar
Alexander Alekhin committed
1224 1225 1226
            pixels.convertTo(p,CV_32F, 1.0,1.0); //matlab use offset
            Mat x = p.col(0);
            Mat y = p.col(1);
1227

Alexander Alekhin's avatar
Alexander Alekhin committed
1228 1229
            Mat alpha = (x-v0x)*(v2y-v0y)-(y-v0y)*(v2x-v0x);
            Mat beta = (v1x-v0x)*(y-v0y)-(v1y-v0y)*(x-v0x);
1230

Alexander Alekhin's avatar
Alexander Alekhin committed
1231
            Mat res = 1.0 - alpha/denominator - beta/denominator; // same just different order
1232

Alexander Alekhin's avatar
Alexander Alekhin committed
1233 1234 1235 1236 1237 1238 1239 1240
            /*remap to image form*/
            Mat dx = Mat::zeros(resolution.height, resolution.width, CV_32F);
            for(int j=0;j<res.rows;j++){
                dx.at<float>((int)(y.at<float>(j)-1.0), (int)(x.at<float>(j)-1.0)) = res.at<float>(j); // matlab use offset
            };

            acc = acc+dx;
        }
1241

Alexander Alekhin's avatar
Alexander Alekhin committed
1242 1243
        Mat vectorized = Mat(acc.t()).reshape(0,1);
        dW_dxdyt.push_back(vectorized.clone());
1244

Alexander Alekhin's avatar
Alexander Alekhin committed
1245
    }// k
1246

Alexander Alekhin's avatar
Alexander Alekhin committed
1247 1248
    Mat dx_dp;
    hconcat(Q, S, dx_dp);
1249

Alexander Alekhin's avatar
Alexander Alekhin committed
1250 1251 1252
    Mat dW_dxdy = dW_dxdyt.t();
    Wx_dp = dW_dxdy* Mat(dx_dp,Range(0,npts));
    Wy_dp = dW_dxdy* Mat(dx_dp,Range(npts,2*npts));
1253

Alexander Alekhin's avatar
Alexander Alekhin committed
1254
} //createWarpJacobian
1255 1256 1257

} /* namespace face */
} /* namespace cv */