facemarkLBF.cpp 48.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 42 43 44 45 46 47
#include <fstream>
#include <cmath>
#include <ctime>
#include <cstdio>
#include <cstdarg>

namespace cv {
namespace face {

Alexander Alekhin's avatar
Alexander Alekhin committed
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
#define TIMER_BEGIN { double __time__ = (double)getTickCount();
#define TIMER_NOW   ((getTickCount() - __time__) / getTickFrequency())
#define TIMER_END   }

#define SIMILARITY_TRANSFORM(x, y, scale, rotate) do {            \
    double x_tmp = scale * (rotate(0, 0)*x + rotate(0, 1)*y); \
    double y_tmp = scale * (rotate(1, 0)*x + rotate(1, 1)*y); \
    x = x_tmp; y = y_tmp;                                     \
} while(0)

FacemarkLBF::Params::Params(){

    cascade_face = "";
    shape_offset = 0.0;
    n_landmarks = 68;
    initShape_n = 10;
    stages_n=5;
    tree_n=6;
    tree_depth=5;
    bagging_overlap = 0.4;
    model_filename = "";
    save_model = true;
    verbose = true;
    seed = 0;

    int _pupils[][6] = { { 36, 37, 38, 39, 40, 41 }, { 42, 43, 44, 45, 46, 47 } };
    for (int i = 0; i < 6; i++) {
        pupils[0].push_back(_pupils[0][i]);
        pupils[1].push_back(_pupils[1][i]);
    }
78

Alexander Alekhin's avatar
Alexander Alekhin committed
79 80 81 82 83 84
    int _feats_m[] = { 500, 500, 500, 300, 300, 300, 200, 200, 200, 100 };
    double _radius_m[] = { 0.3, 0.2, 0.15, 0.12, 0.10, 0.10, 0.08, 0.06, 0.06, 0.05 };
    for (int i = 0; i < 10; i++) {
        feats_m.push_back(_feats_m[i]);
        radius_m.push_back(_radius_m[i]);
    }
85

Alexander Alekhin's avatar
Alexander Alekhin committed
86 87
    detectROI = Rect(-1,-1,-1,-1);
}
88

Alexander Alekhin's avatar
Alexander Alekhin committed
89 90
void FacemarkLBF::Params::read( const cv::FileNode& fn ){
    *this = FacemarkLBF::Params();
91

Alexander Alekhin's avatar
Alexander Alekhin committed
92 93
    if (!fn["verbose"].empty())
        fn["verbose"] >> verbose;
94

Alexander Alekhin's avatar
Alexander Alekhin committed
95
}
96

Alexander Alekhin's avatar
Alexander Alekhin committed
97 98 99
void FacemarkLBF::Params::write( cv::FileStorage& fs ) const{
    fs << "verbose" << verbose;
}
100

Alexander Alekhin's avatar
Alexander Alekhin committed
101 102 103
class FacemarkLBFImpl : public FacemarkLBF {
public:
    FacemarkLBFImpl( const FacemarkLBF::Params &parameters = FacemarkLBF::Params() );
104

105 106
    void read( const FileNode& /*fn*/ ) CV_OVERRIDE;
    void write( FileStorage& /*fs*/ ) const CV_OVERRIDE;
107

108
    void loadModel(String fs) CV_OVERRIDE;
109

110 111 112
    bool setFaceDetector(bool(*f)(InputArray , OutputArray, void * extra_params ), void* userData) CV_OVERRIDE;
    bool getFaces(InputArray image, OutputArray faces) CV_OVERRIDE;
    bool getData(void * items) CV_OVERRIDE;
113

Alexander Alekhin's avatar
Alexander Alekhin committed
114
    Params params;
115

Alexander Alekhin's avatar
Alexander Alekhin committed
116
protected:
117

Dmitry Kurtaev's avatar
Dmitry Kurtaev committed
118
    bool fit( InputArray image, InputArray faces, OutputArrayOfArrays landmarks ) CV_OVERRIDE;//!< from many ROIs
Alexander Alekhin's avatar
Alexander Alekhin committed
119
    bool fitImpl( const Mat image, std::vector<Point2f> & landmarks );//!< from a face
120

121 122
    bool addTrainingSample(InputArray image, InputArray landmarks) CV_OVERRIDE;
    void training(void* parameters) CV_OVERRIDE;
123

Alexander Alekhin's avatar
Alexander Alekhin committed
124 125 126 127 128
    Rect getBBox(Mat &img, const Mat_<double> shape);
    void prepareTrainingData(Mat img, std::vector<Point2f> facePoints,
        std::vector<Mat> & cropped, std::vector<Mat> & shapes, std::vector<BBox> &boxes);
    void data_augmentation(std::vector<Mat> &imgs, std::vector<Mat> &gt_shapes, std::vector<BBox> &bboxes);
    Mat getMeanShape(std::vector<Mat> &gt_shapes, std::vector<BBox> &bboxes);
129

Alexander Alekhin's avatar
Alexander Alekhin committed
130
    bool defaultFaceDetector(const Mat& image, std::vector<Rect>& faces);
131

Alexander Alekhin's avatar
Alexander Alekhin committed
132
    CascadeClassifier face_cascade;
Alexander Alekhin's avatar
Alexander Alekhin committed
133 134
    FN_FaceDetector faceDetector;
    void* faceDetectorData;
135

Alexander Alekhin's avatar
Alexander Alekhin committed
136 137 138 139 140
    /*training data*/
    std::vector<std::vector<Point2f> > data_facemarks; //original position
    std::vector<Mat> data_faces; //face ROI
    std::vector<BBox> data_boxes;
    std::vector<Mat> data_shapes; //position in the face ROI
141

Alexander Alekhin's avatar
Alexander Alekhin committed
142 143
private:
    bool isModelTrained;
144

Alexander Alekhin's avatar
Alexander Alekhin committed
145 146 147 148 149 150 151 152 153
    /*---------------LBF Class---------------------*/
    class LBF {
    public:
        void calcSimilarityTransform(const Mat &shape1, const Mat &shape2, double &scale, Mat &rotate);
        std::vector<Mat> getDeltaShapes(std::vector<Mat> &gt_shapes, std::vector<Mat> &current_shapes,
                                   std::vector<BBox> &bboxes, Mat &mean_shape);
        double calcVariance(const Mat &vec);
        double calcVariance(const std::vector<double> &vec);
        double calcMeanError(std::vector<Mat> &gt_shapes, std::vector<Mat> &current_shapes, int landmark_n , std::vector<int> &left, std::vector<int> &right );
154

Alexander Alekhin's avatar
Alexander Alekhin committed
155
    };
156

Alexander Alekhin's avatar
Alexander Alekhin committed
157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209
    /*---------------RandomTree Class---------------------*/
    class RandomTree : public LBF {
    public:
        RandomTree(){};
        ~RandomTree(){};

        void initTree(int landmark_id, int depth, std::vector<int>, std::vector<double>);
        void train(std::vector<Mat> &imgs, std::vector<Mat> &current_shapes, std::vector<BBox> &bboxes,
                   std::vector<Mat> &delta_shapes, Mat &mean_shape, std::vector<int> &index, int stage);
        void splitNode(std::vector<cv::Mat> &imgs, std::vector<cv::Mat> &current_shapes, std::vector<BBox> &bboxes,
                      cv::Mat &delta_shapes, cv::Mat &mean_shape, std::vector<int> &root, int idx, int stage);

        void write(FileStorage fs, int forestId, int i, int j);
        void read(FileStorage fs, int forestId, int i, int j);

        int depth;
        int nodes_n;
        int landmark_id;
        cv::Mat_<double> feats;
        std::vector<int> thresholds;

        std::vector<int> params_feats_m;
        std::vector<double> params_radius_m;
    };
    /*---------------RandomForest Class---------------------*/
    class RandomForest : public LBF {
    public:
        RandomForest(){};
        ~RandomForest(){};

        void initForest(int landmark_n, int trees_n, int tree_depth, double ,  std::vector<int>, std::vector<double>, bool);
        void train(std::vector<cv::Mat> &imgs, std::vector<cv::Mat> &current_shapes, \
                   std::vector<BBox> &bboxes, std::vector<cv::Mat> &delta_shapes, cv::Mat &mean_shape, int stage);
        Mat generateLBF(Mat &img, Mat &current_shape, BBox &bbox, Mat &mean_shape);

        void write(FileStorage fs, int forestId);
        void read(FileStorage fs, int forestId);

        bool verbose;
        int landmark_n;
        int trees_n, tree_depth;
        double overlap_ratio;
        std::vector<std::vector<RandomTree> > random_trees;

        std::vector<int> feats_m;
        std::vector<double> radius_m;
    };
    /*---------------Regressor Class---------------------*/
    class Regressor  : public LBF {
    protected:
        struct feature_node{
            int index;
            double value;
210
        };
Alexander Alekhin's avatar
Alexander Alekhin committed
211 212 213
    public:
        Regressor(){};
        ~Regressor(){};
214

Alexander Alekhin's avatar
Alexander Alekhin committed
215 216 217 218 219 220
        void initRegressor(Params);
        void trainRegressor(std::vector<cv::Mat> &imgs, std::vector<cv::Mat> &gt_shapes, \
                   std::vector<cv::Mat> &current_shapes, std::vector<BBox> &bboxes, \
                   cv::Mat &mean_shape, int start_from, Params );
        Mat globalRegressionPredict(const Mat &lbf, int stage);
        Mat predict(Mat &img, BBox &bbox);
221

Alexander Alekhin's avatar
Alexander Alekhin committed
222 223
        void write(FileStorage fs, Params config);
        void read(FileStorage fs, Params & config);
224

Alexander Alekhin's avatar
Alexander Alekhin committed
225 226 227 228
        void globalRegressionTrain(
            std::vector<Mat> &lbfs, std::vector<Mat> &delta_shapes,
            int stage, Params config
        );
229

Alexander Alekhin's avatar
Alexander Alekhin committed
230 231 232
        Mat supportVectorRegression(
            feature_node **x, double *y, int nsamples, int feat_size, bool verbose=0
        );
233

Alexander Alekhin's avatar
Alexander Alekhin committed
234 235 236 237 238
        int stages_n;
        int landmark_n;
        cv::Mat mean_shape;
        std::vector<RandomForest> random_forests;
        std::vector<cv::Mat> gl_regression_weights;
239

Alexander Alekhin's avatar
Alexander Alekhin committed
240
    }; // LBF
241

Alexander Alekhin's avatar
Alexander Alekhin committed
242 243
    Regressor regressor;
}; // class
244

Alexander Alekhin's avatar
Alexander Alekhin committed
245 246 247 248 249 250
/*
* Constructor
*/
Ptr<FacemarkLBF> FacemarkLBF::create(const FacemarkLBF::Params &parameters){
    return Ptr<FacemarkLBFImpl>(new FacemarkLBFImpl(parameters));
}
251 252 253 254 255 256 257
/*
* Constructor
*/
Ptr<Facemark> createFacemarkLBF(){
    const FacemarkLBF::Params parameters;
    return Ptr<FacemarkLBFImpl>(new FacemarkLBFImpl(parameters));
}
Alexander Alekhin's avatar
Alexander Alekhin committed
258

Alexander Alekhin's avatar
Alexander Alekhin committed
259 260
FacemarkLBFImpl::FacemarkLBFImpl( const FacemarkLBF::Params &parameters ) :
    faceDetector(NULL), faceDetectorData(NULL)
Alexander Alekhin's avatar
Alexander Alekhin committed
261 262 263 264 265
{
    isModelTrained = false;
    params = parameters;
}

Alexander Alekhin's avatar
Alexander Alekhin committed
266
bool FacemarkLBFImpl::setFaceDetector(bool(*f)(InputArray , OutputArray, void * extra_params ), void* userData){
Alexander Alekhin's avatar
Alexander Alekhin committed
267
    faceDetector = f;
Alexander Alekhin's avatar
Alexander Alekhin committed
268
    faceDetectorData = userData;
Alexander Alekhin's avatar
Alexander Alekhin committed
269 270
    return true;
}
271

Alexander Alekhin's avatar
Alexander Alekhin committed
272 273 274 275 276 277 278 279
bool FacemarkLBFImpl::getFaces(InputArray image, OutputArray faces_)
{
    if (!faceDetector)
    {
        std::vector<Rect> faces;
        defaultFaceDetector(image.getMat(), faces);
        Mat(faces).copyTo(faces_);
        return true;
280
    }
Alexander Alekhin's avatar
Alexander Alekhin committed
281
    return faceDetector(image, faces_, faceDetectorData);
Alexander Alekhin's avatar
Alexander Alekhin committed
282
}
283

Alexander Alekhin's avatar
Alexander Alekhin committed
284
bool FacemarkLBFImpl::defaultFaceDetector(const Mat& image, std::vector<Rect>& faces){
Alexander Alekhin's avatar
Alexander Alekhin committed
285 286 287
    Mat gray;

    faces.clear();
288

Alexander Alekhin's avatar
Alexander Alekhin committed
289 290 291 292 293 294
    if (image.channels() > 1)
    {
        cvtColor(image, gray, COLOR_BGR2GRAY);
    }
    else
    {
Alexander Alekhin's avatar
Alexander Alekhin committed
295
        gray = image;
296 297
    }

Alexander Alekhin's avatar
Alexander Alekhin committed
298
    equalizeHist(gray, gray);
299

Alexander Alekhin's avatar
Alexander Alekhin committed
300 301 302 303 304 305 306 307 308 309 310 311
    if (face_cascade.empty())
    {
        { /* check the cascade classifier file */
            std::ifstream infile;
            infile.open(params.cascade_face.c_str(), std::ios::in);
            if (!infile)
                CV_Error_(Error::StsBadArg, ("The cascade classifier model is not found: %s", params.cascade_face.c_str()));
        }
        face_cascade.load(params.cascade_face.c_str());
        CV_Assert(!face_cascade.empty());
    }
    face_cascade.detectMultiScale(gray, faces, 1.05, 2, CASCADE_SCALE_IMAGE, Size(30, 30) );
Alexander Alekhin's avatar
Alexander Alekhin committed
312 313
    return true;
}
314

Alexander Alekhin's avatar
Alexander Alekhin committed
315
bool FacemarkLBFImpl::getData(void * items){
Alexander Alekhin's avatar
Alexander Alekhin committed
316 317
    CV_UNUSED(items);
    return false;
Alexander Alekhin's avatar
Alexander Alekhin committed
318 319 320
}

bool FacemarkLBFImpl::addTrainingSample(InputArray image, InputArray landmarks){
Alexander Alekhin's avatar
Alexander Alekhin committed
321
    // FIXIT
Alexander Alekhin's avatar
Alexander Alekhin committed
322 323 324 325 326 327
    std::vector<Point2f> & _landmarks = *(std::vector<Point2f>*)landmarks.getObj();
    prepareTrainingData(image.getMat(), _landmarks, data_faces, data_shapes, data_boxes);
    return true;
}

void FacemarkLBFImpl::training(void* parameters){
Alexander Alekhin's avatar
Alexander Alekhin committed
328 329 330 331 332
    CV_UNUSED(parameters);

    if (data_faces.empty())
    {
        CV_Error(Error::StsBadArg, "Training data is not provided. Consider to add using addTrainingSample() function!");
Alexander Alekhin's avatar
Alexander Alekhin committed
333
    }
334

Alexander Alekhin's avatar
Alexander Alekhin committed
335 336 337
    if (params.cascade_face.empty() || (params.model_filename.empty() && params.save_model))
    {
        CV_Error(Error::StsBadArg, "The parameter cascade_face and model_filename should be set!");
Alexander Alekhin's avatar
Alexander Alekhin committed
338
    }
339

Alexander Alekhin's avatar
Alexander Alekhin committed
340 341 342 343 344 345 346 347 348 349 350 351 352
    // flip the image and swap the landmark position
    data_augmentation(data_faces, data_shapes, data_boxes);

    Mat mean_shape = getMeanShape(data_shapes, data_boxes);

    int N = (int)data_faces.size();
    int L = N*params.initShape_n;
    std::vector<Mat> imgs(L), gt_shapes(L), current_shapes(L);
    std::vector<BBox> bboxes(L);
    RNG rng(params.seed);
    for (int i = 0; i < N; i++) {
        for (int j = 0; j < params.initShape_n; j++) {
            int idx = i*params.initShape_n + j;
Alexander Alekhin's avatar
Alexander Alekhin committed
353 354
            int k = rng.uniform(0, N - 1);
            k = (k >= i) ? k + 1 : k; // require k != i
Alexander Alekhin's avatar
Alexander Alekhin committed
355 356 357 358
            imgs[idx] = data_faces[i];
            gt_shapes[idx] = data_shapes[i];
            bboxes[idx] = data_boxes[i];
            current_shapes[idx] = data_boxes[i].reproject(data_boxes[k].project(data_shapes[k]));
359
        }
Alexander Alekhin's avatar
Alexander Alekhin committed
360
    }
361

Alexander Alekhin's avatar
Alexander Alekhin committed
362 363
    regressor.initRegressor(params);
    regressor.trainRegressor(imgs, gt_shapes, current_shapes, bboxes, mean_shape, 0, params);
364

Alexander Alekhin's avatar
Alexander Alekhin committed
365 366 367 368
    if(params.save_model){
        FileStorage fs(params.model_filename.c_str(),FileStorage::WRITE_BASE64);
        regressor.write(fs, params);
    }
369

Alexander Alekhin's avatar
Alexander Alekhin committed
370 371 372
    isModelTrained = true;
}

373
bool FacemarkLBFImpl::fit( InputArray image, InputArray roi, OutputArrayOfArrays  _landmarks )
Alexander Alekhin's avatar
Alexander Alekhin committed
374
{
Alexander Alekhin's avatar
Alexander Alekhin committed
375
    // FIXIT
Alexander Alekhin's avatar
Alexander Alekhin committed
376
    std::vector<Rect> & faces = *(std::vector<Rect> *)roi.getObj();
Alexander Alekhin's avatar
Alexander Alekhin committed
377
    if (faces.empty()) return false;
378

Alexander Alekhin's avatar
Alexander Alekhin committed
379 380
    std::vector<std::vector<Point2f> > & landmarks =
        *(std::vector<std::vector<Point2f> >*) _landmarks.getObj();
381

Alexander Alekhin's avatar
Alexander Alekhin committed
382
    landmarks.resize(faces.size());
383

Alexander Alekhin's avatar
Alexander Alekhin committed
384 385 386 387
    for(unsigned i=0; i<faces.size();i++){
        params.detectROI = faces[i];
        fitImpl(image.getMat(), landmarks[i]);
    }
388

Alexander Alekhin's avatar
Alexander Alekhin committed
389 390
    return true;
}
391

Alexander Alekhin's avatar
Alexander Alekhin committed
392 393 394 395 396
bool FacemarkLBFImpl::fitImpl( const Mat image, std::vector<Point2f>& landmarks){
    if (landmarks.size()>0)
        landmarks.clear();

    if (!isModelTrained) {
Alexander Alekhin's avatar
Alexander Alekhin committed
397
        CV_Error(Error::StsBadArg, "The LBF model is not trained yet. Please provide a trained model.");
398 399
    }

Alexander Alekhin's avatar
Alexander Alekhin committed
400 401
    Mat img;
    if(image.channels()>1){
Alexander Alekhin's avatar
Alexander Alekhin committed
402
        cvtColor(image,img,COLOR_BGR2GRAY);
Alexander Alekhin's avatar
Alexander Alekhin committed
403 404 405
    }else{
        img = image;
    }
406

Alexander Alekhin's avatar
Alexander Alekhin committed
407 408 409 410 411
    Rect box;
    if (params.detectROI.width>0){
        box = params.detectROI;
    }else{
        std::vector<Rect> rects;
412

Alexander Alekhin's avatar
Alexander Alekhin committed
413 414
        if (!getFaces(img, rects)) return 0;
        if (rects.empty())  return 0; //failed to get face
Alexander Alekhin's avatar
Alexander Alekhin committed
415 416
        box = rects[0];
    }
417

Alexander Alekhin's avatar
Alexander Alekhin committed
418 419 420 421 422
    double min_x, min_y, max_x, max_y;
    min_x = std::max(0., (double)box.x - box.width / 2);
    max_x = std::min(img.cols - 1., (double)box.x+box.width + box.width / 2);
    min_y = std::max(0., (double)box.y - box.height / 2);
    max_y = std::min(img.rows - 1., (double)box.y + box.height + box.height / 2);
423

Alexander Alekhin's avatar
Alexander Alekhin committed
424 425
    double w = max_x - min_x;
    double h = max_y - min_y;
426

Alexander Alekhin's avatar
Alexander Alekhin committed
427 428 429
    BBox bbox(box.x - min_x, box.y - min_y, box.width, box.height);
    Mat crop = img(Rect((int)min_x, (int)min_y, (int)w, (int)h)).clone();
    Mat shape = regressor.predict(crop, bbox);
430

Alexander Alekhin's avatar
Alexander Alekhin committed
431 432 433 434 435 436
    if(params.detectROI.width>0){
        landmarks = Mat(shape.reshape(2)+Scalar(min_x, min_y));
        params.detectROI.width = -1;
    }else{
        landmarks = Mat(shape.reshape(2)+Scalar(min_x, min_y));
    }
437

Alexander Alekhin's avatar
Alexander Alekhin committed
438 439
    return 1;
}
440

Alexander Alekhin's avatar
Alexander Alekhin committed
441 442 443
void FacemarkLBFImpl::read( const cv::FileNode& fn ){
    params.read( fn );
}
444

Alexander Alekhin's avatar
Alexander Alekhin committed
445 446 447
void FacemarkLBFImpl::write( cv::FileStorage& fs ) const {
    params.write( fs );
}
448

Alexander Alekhin's avatar
Alexander Alekhin committed
449 450 451 452 453
void FacemarkLBFImpl::loadModel(String s){
    if(params.verbose) printf("loading data from : %s\n", s.c_str());
    std::ifstream infile;
    infile.open(s.c_str(), std::ios::in);
    if (!infile) {
Alexander Alekhin's avatar
Alexander Alekhin committed
454
        CV_Error(Error::StsBadArg, "No valid input file was given, please check the given filename.");
455 456
    }

Alexander Alekhin's avatar
Alexander Alekhin committed
457 458
    FileStorage fs(s.c_str(),FileStorage::READ);
    regressor.read(fs, params);
459

Alexander Alekhin's avatar
Alexander Alekhin committed
460 461
    isModelTrained = true;
}
462

Alexander Alekhin's avatar
Alexander Alekhin committed
463 464
Rect FacemarkLBFImpl::getBBox(Mat &img, const Mat_<double> shape) {
    std::vector<Rect> rects;
465

Alexander Alekhin's avatar
Alexander Alekhin committed
466
    if(!faceDetector){
Alexander Alekhin's avatar
Alexander Alekhin committed
467 468
        defaultFaceDetector(img, rects);
    }else{
469
        faceDetector(img, rects, faceDetectorData);
470 471
    }

Alexander Alekhin's avatar
Alexander Alekhin committed
472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505
    if (rects.size() == 0) return Rect(-1, -1, -1, -1);
    double center_x=0, center_y=0, min_x, max_x, min_y, max_y;

    min_x = shape(0, 0);
    max_x = shape(0, 0);
    min_y = shape(0, 1);
    max_y = shape(0, 1);

    for (int i = 0; i < shape.rows; i++) {
        center_x += shape(i, 0);
        center_y += shape(i, 1);
        min_x = std::min(min_x, shape(i, 0));
        max_x = std::max(max_x, shape(i, 0));
        min_y = std::min(min_y, shape(i, 1));
        max_y = std::max(max_y, shape(i, 1));
    }
    center_x /= shape.rows;
    center_y /= shape.rows;

    for (int i = 0; i < (int)rects.size(); i++) {
        Rect r = rects[i];
        if (max_x - min_x > r.width*1.5) continue;
        if (max_y - min_y > r.height*1.5) continue;
        if (abs(center_x - (r.x + r.width / 2)) > r.width / 2) continue;
        if (abs(center_y - (r.y + r.height / 2)) > r.height / 2) continue;
        return r;
    }
    return Rect(-1, -1, -1, -1);
}

void FacemarkLBFImpl::prepareTrainingData(Mat img, std::vector<Point2f> facePoints,
    std::vector<Mat> & cropped, std::vector<Mat> & shapes, std::vector<BBox> &boxes)
{
    if(img.channels()>1){
Alexander Alekhin's avatar
Alexander Alekhin committed
506
        cvtColor(img,img,COLOR_BGR2GRAY);
507 508
    }

Alexander Alekhin's avatar
Alexander Alekhin committed
509 510 511 512 513 514 515 516 517 518 519 520 521 522 523
    Mat shape;
    Mat _shape = Mat(facePoints).reshape(1);
    Rect box = getBBox(img, _shape);
    if(box.x != -1){
        _shape.convertTo(shape, CV_64FC1);
        Mat sx = shape.col(0);
        Mat sy = shape.col(1);
        double min_x, max_x, min_y, max_y;
        minMaxIdx(sx, &min_x, &max_x);
        minMaxIdx(sy, &min_y, &max_y);

        min_x = std::max(0., min_x - (double)box.width / 2.);
        max_x = std::min(img.cols - 1., max_x + (double)box.width / 2.);
        min_y = std::max(0., min_y - (double)box.height / 2.);
        max_y = std::min(img.rows - 1., max_y + (double)box.height / 2.);
524

Alexander Alekhin's avatar
Alexander Alekhin committed
525 526
        double w = max_x - min_x;
        double h = max_y - min_y;
527

Alexander Alekhin's avatar
Alexander Alekhin committed
528
        shape = Mat(shape.reshape(2)-Scalar(min_x, min_y)).reshape(1);
529

Alexander Alekhin's avatar
Alexander Alekhin committed
530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551
        boxes.push_back(BBox(box.x - min_x, box.y - min_y, box.width, box.height));
        Mat crop = img(Rect((int)min_x, (int)min_y, (int)w, (int)h)).clone();
        cropped.push_back(crop);
        shapes.push_back(shape);

    } // if box is valid
} // prepareTrainingData

void FacemarkLBFImpl::data_augmentation(std::vector<Mat> &imgs, std::vector<Mat> &gt_shapes, std::vector<BBox> &bboxes) {
    int N = (int)imgs.size();
    imgs.reserve(2 * N);
    gt_shapes.reserve(2 * N);
    bboxes.reserve(2 * N);
    for (int i = 0; i < N; i++) {
        Mat img_flipped;
        Mat_<double> gt_shape_flipped(gt_shapes[i].size());
        flip(imgs[i], img_flipped, 1);
        int w = img_flipped.cols - 1;
        // int h = img_flipped.rows - 1;
        for (int k = 0; k < gt_shapes[i].rows; k++) {
            gt_shape_flipped(k, 0) = w - gt_shapes[i].at<double>(k, 0);
            gt_shape_flipped(k, 1) = gt_shapes[i].at<double>(k, 1);
552
        }
Alexander Alekhin's avatar
Alexander Alekhin committed
553 554 555 556 557 558
        int x_b, y_b, w_b, h_b;
        x_b = w - (int)bboxes[i].x - (int)bboxes[i].width;
        y_b = (int)bboxes[i].y;
        w_b = (int)bboxes[i].width;
        h_b = (int)bboxes[i].height;
        BBox bbox_flipped(x_b, y_b, w_b, h_b);
559

Alexander Alekhin's avatar
Alexander Alekhin committed
560 561 562
        imgs.push_back(img_flipped);
        gt_shapes.push_back(gt_shape_flipped);
        bboxes.push_back(bbox_flipped);
563 564

    }
Alexander Alekhin's avatar
Alexander Alekhin committed
565 566 567 568 569 570 571 572
#define SWAP(shape, i, j) do { \
        double tmp = shape.at<double>(i-1, 0); \
        shape.at<double>(i-1, 0) = shape.at<double>(j-1, 0); \
        shape.at<double>(j-1, 0) = tmp; \
        tmp =  shape.at<double>(i-1, 1); \
        shape.at<double>(i-1, 1) = shape.at<double>(j-1, 1); \
        shape.at<double>(j-1, 1) = tmp; \
    } while(0)
573

Alexander Alekhin's avatar
Alexander Alekhin committed
574 575 576 577 578 579 580 581 582 583 584 585 586 587
    if (params.n_landmarks == 29) {
        for (int i = N; i < (int)gt_shapes.size(); i++) {
            SWAP(gt_shapes[i], 1, 2);
            SWAP(gt_shapes[i], 3, 4);
            SWAP(gt_shapes[i], 5, 7);
            SWAP(gt_shapes[i], 6, 8);
            SWAP(gt_shapes[i], 13, 15);
            SWAP(gt_shapes[i], 9, 10);
            SWAP(gt_shapes[i], 11, 12);
            SWAP(gt_shapes[i], 17, 18);
            SWAP(gt_shapes[i], 14, 16);
            SWAP(gt_shapes[i], 19, 20);
            SWAP(gt_shapes[i], 23, 24);
        }
588
    }
Alexander Alekhin's avatar
Alexander Alekhin committed
589 590 591 592 593 594 595 596 597 598 599 600 601 602 603
    else if (params.n_landmarks == 68) {
        for (int i = N; i < (int)gt_shapes.size(); i++) {
            for (int k = 1; k <= 8; k++) SWAP(gt_shapes[i], k, 18 - k);
            for (int k = 18; k <= 22; k++) SWAP(gt_shapes[i], k, 45 - k);
            for (int k = 37; k <= 40; k++) SWAP(gt_shapes[i], k, 83 - k);
            SWAP(gt_shapes[i], 42, 47);
            SWAP(gt_shapes[i], 41, 48);
            SWAP(gt_shapes[i], 32, 36);
            SWAP(gt_shapes[i], 33, 35);
            for (int k = 49; k <= 51; k++) SWAP(gt_shapes[i], k, 104 - k);
            SWAP(gt_shapes[i], 60, 56);
            SWAP(gt_shapes[i], 59, 57);
            SWAP(gt_shapes[i], 61, 65);
            SWAP(gt_shapes[i], 62, 64);
            SWAP(gt_shapes[i], 68, 66);
604
        }
Alexander Alekhin's avatar
Alexander Alekhin committed
605 606 607
    }
    else {
        printf("Wrong n_landmarks, currently only 29 and 68 landmark points are supported");
608 609
    }

Alexander Alekhin's avatar
Alexander Alekhin committed
610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645
#undef SWAP

}

FacemarkLBFImpl::BBox::BBox() {}
FacemarkLBFImpl::BBox::~BBox() {}

FacemarkLBFImpl::BBox::BBox(double _x, double _y, double w, double h) {
    x = _x;
    y = _y;
    width = w;
    height = h;
    x_center = x + w / 2.;
    y_center = y + h / 2.;
    x_scale = w / 2.;
    y_scale = h / 2.;
}

// Project absolute shape to relative shape binding to this bbox
Mat FacemarkLBFImpl::BBox::project(const Mat &shape) const {
    Mat_<double> res(shape.rows, shape.cols);
    const Mat_<double> &shape_ = (Mat_<double>)shape;
    for (int i = 0; i < shape.rows; i++) {
        res(i, 0) = (shape_(i, 0) - x_center) / x_scale;
        res(i, 1) = (shape_(i, 1) - y_center) / y_scale;
    }
    return res;
}

// Project relative shape to absolute shape binding to this bbox
Mat FacemarkLBFImpl::BBox::reproject(const Mat &shape) const {
    Mat_<double> res(shape.rows, shape.cols);
    const Mat_<double> &shape_ = (Mat_<double>)shape;
    for (int i = 0; i < shape.rows; i++) {
        res(i, 0) = shape_(i, 0)*x_scale + x_center;
        res(i, 1) = shape_(i, 1)*y_scale + y_center;
646
    }
Alexander Alekhin's avatar
Alexander Alekhin committed
647 648
    return res;
}
649

Alexander Alekhin's avatar
Alexander Alekhin committed
650
Mat FacemarkLBFImpl::getMeanShape(std::vector<Mat> &gt_shapes, std::vector<BBox> &bboxes) {
651

Alexander Alekhin's avatar
Alexander Alekhin committed
652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679
    int N = (int)gt_shapes.size();
    Mat mean_shape = Mat::zeros(gt_shapes[0].rows, 2, CV_64FC1);
    for (int i = 0; i < N; i++) {
        mean_shape += bboxes[i].project(gt_shapes[i]);
    }
    mean_shape /= N;
    return mean_shape;
}

// Similarity Transform, project shape2 to shape1
// p1 ~= scale * rotate * p2, p1 and p2 are vector in math
void FacemarkLBFImpl::LBF::calcSimilarityTransform(const Mat &shape1, const Mat &shape2, double &scale, Mat &rotate) {
    Mat_<double> rotate_(2, 2);
    double x1_center, y1_center, x2_center, y2_center;
    x1_center = cv::mean(shape1.col(0))[0];
    y1_center = cv::mean(shape1.col(1))[0];
    x2_center = cv::mean(shape2.col(0))[0];
    y2_center = cv::mean(shape2.col(1))[0];

    Mat temp1(shape1.rows, shape1.cols, CV_64FC1);
    Mat temp2(shape2.rows, shape2.cols, CV_64FC1);
    temp1.col(0) = shape1.col(0) - x1_center;
    temp1.col(1) = shape1.col(1) - y1_center;
    temp2.col(0) = shape2.col(0) - x2_center;
    temp2.col(1) = shape2.col(1) - y2_center;

    Mat_<double> covar1, covar2;
    Mat_<double> mean1, mean2;
Alexander Alekhin's avatar
Alexander Alekhin committed
680 681
    calcCovarMatrix(temp1, covar1, mean1, COVAR_COLS);
    calcCovarMatrix(temp2, covar2, mean2, COVAR_COLS);
Alexander Alekhin's avatar
Alexander Alekhin committed
682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 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 738 739 740 741 742 743 744 745 746 747

    double s1 = sqrt(cv::norm(covar1));
    double s2 = sqrt(cv::norm(covar2));
    scale = s1 / s2;
    temp1 /= s1;
    temp2 /= s2;

    double num = temp1.col(1).dot(temp2.col(0)) - temp1.col(0).dot(temp2.col(1));
    double den = temp1.col(0).dot(temp2.col(0)) + temp1.col(1).dot(temp2.col(1));
    double normed = sqrt(num*num + den*den);
    double sin_theta = num / normed;
    double cos_theta = den / normed;
    rotate_(0, 0) = cos_theta; rotate_(0, 1) = -sin_theta;
    rotate_(1, 0) = sin_theta; rotate_(1, 1) = cos_theta;
    rotate = rotate_;
}

// Get relative delta_shapes for predicting target
std::vector<Mat> FacemarkLBFImpl::LBF::getDeltaShapes(std::vector<Mat> &gt_shapes, std::vector<Mat> &current_shapes,
                           std::vector<BBox> &bboxes, Mat &mean_shape) {
    std::vector<Mat> delta_shapes;
    int N = (int)gt_shapes.size();
    delta_shapes.resize(N);
    double scale;
    Mat_<double> rotate;
    for (int i = 0; i < N; i++) {
        delta_shapes[i] = bboxes[i].project(gt_shapes[i]) - bboxes[i].project(current_shapes[i]);
        calcSimilarityTransform(mean_shape, bboxes[i].project(current_shapes[i]), scale, rotate);
        // delta_shapes[i] = scale * delta_shapes[i] * rotate.t(); // the result is better without this part
    }
    return delta_shapes;
}

double FacemarkLBFImpl::LBF::calcVariance(const Mat &vec) {
    double m1 = cv::mean(vec)[0];
    double m2 = cv::mean(vec.mul(vec))[0];
    double variance = m2 - m1*m1;
    return variance;
}

double FacemarkLBFImpl::LBF::calcVariance(const std::vector<double> &vec) {
    if (vec.size() == 0) return 0.;
    Mat_<double> vec_(vec);
    double m1 = cv::mean(vec_)[0];
    double m2 = cv::mean(vec_.mul(vec_))[0];
    double variance = m2 - m1*m1;
    return variance;
}

double FacemarkLBFImpl::LBF::calcMeanError(std::vector<Mat> &gt_shapes, std::vector<Mat> &current_shapes, int landmark_n , std::vector<int> &left, std::vector<int> &right ) {
    int N = (int)gt_shapes.size();

    double e = 0;
    // every train data
    for (int i = 0; i < N; i++) {
        const Mat_<double> &gt_shape = (Mat_<double>)gt_shapes[i];
        const Mat_<double> &current_shape = (Mat_<double>)current_shapes[i];
        double x1, y1, x2, y2;
        x1 = x2 = y1 = y2 = 0;
        for (int j = 0; j < (int)left.size(); j++) {
            x1 += gt_shape(left[j], 0);
            y1 += gt_shape(left[j], 1);
        }
        for (int j = 0; j < (int)right.size(); j++) {
            x2 += gt_shape(right[j], 0);
            y2 += gt_shape(right[j], 1);
748
        }
Alexander Alekhin's avatar
Alexander Alekhin committed
749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778 779 780
        x1 /= left.size(); y1 /= left.size();
        x2 /= right.size(); y2 /= right.size();
        double pupils_distance = sqrt((x2 - x1)*(x2 - x1) + (y2 - y1)*(y2 - y1));
        // every landmark
        double e_ = 0;
        for (int j = 0; j < landmark_n; j++) {
            e_ += norm(gt_shape.row(j) - current_shape.row(j));
        }
        e += e_ / pupils_distance;
    }
    e /= N*landmark_n;
    return e;
}

/*---------------RandomTree Implementation---------------------*/
void FacemarkLBFImpl::RandomTree::initTree(int _landmark_id, int _depth, std::vector<int> feats_m, std::vector<double> radius_m) {
    landmark_id = _landmark_id;
    depth = _depth;
    nodes_n = 1 << depth;
    feats = Mat::zeros(nodes_n, 4, CV_64FC1);
    thresholds.resize(nodes_n);

    params_feats_m = feats_m;
    params_radius_m = radius_m;
}

void FacemarkLBFImpl::RandomTree::train(std::vector<Mat> &imgs, std::vector<Mat> &current_shapes, std::vector<BBox> &bboxes,
                       std::vector<Mat> &delta_shapes, Mat &mean_shape, std::vector<int> &index, int stage) {
    Mat_<double> delta_shapes_((int)delta_shapes.size(), 2);
    for (int i = 0; i < (int)delta_shapes.size(); i++) {
        delta_shapes_(i, 0) = delta_shapes[i].at<double>(landmark_id, 0);
        delta_shapes_(i, 1) = delta_shapes[i].at<double>(landmark_id, 1);
781
    }
Alexander Alekhin's avatar
Alexander Alekhin committed
782 783 784 785 786
    splitNode(imgs, current_shapes, bboxes, delta_shapes_, mean_shape, index, 1, stage);
}

void FacemarkLBFImpl::RandomTree::splitNode(std::vector<Mat> &imgs, std::vector<Mat> &current_shapes, std::vector<BBox> &bboxes,
                           Mat &delta_shapes, Mat &mean_shape, std::vector<int> &root, int idx, int stage) {
787

Alexander Alekhin's avatar
Alexander Alekhin committed
788 789 790 791 792 793 794 795 796 797 798
    int N = (int)root.size();
    if (N == 0) {
        thresholds[idx] = 0;
        feats.row(idx).setTo(0);
        std::vector<int> left, right;
        // split left and right child in DFS
        if (2 * idx < feats.rows / 2)
            splitNode(imgs, current_shapes, bboxes, delta_shapes, mean_shape, left, 2 * idx, stage);
        if (2 * idx + 1 < feats.rows / 2)
            splitNode(imgs, current_shapes, bboxes, delta_shapes, mean_shape, right, 2 * idx + 1, stage);
        return;
799 800
    }

Alexander Alekhin's avatar
Alexander Alekhin committed
801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821
    int feats_m = params_feats_m[stage];
    double radius_m = params_radius_m[stage];
    Mat_<double> candidate_feats(feats_m, 4);
    RNG rng(getTickCount());
    // generate feature pool
    for (int i = 0; i < feats_m; i++) {
        double x1, y1, x2, y2;
        x1 = rng.uniform(-1., 1.); y1 = rng.uniform(-1., 1.);
        x2 = rng.uniform(-1., 1.); y2 = rng.uniform(-1., 1.);
        if (x1*x1 + y1*y1 > 1. || x2*x2 + y2*y2 > 1.) {
            i--;
            continue;
        }
        candidate_feats[i][0] = x1 * radius_m;
        candidate_feats[i][1] = y1 * radius_m;
        candidate_feats[i][2] = x2 * radius_m;
        candidate_feats[i][3] = y2 * radius_m;
    }
    // calc features
    Mat_<int> densities(feats_m, N);
    for (int i = 0; i < N; i++) {
822 823
        double scale;
        Mat_<double> rotate;
Alexander Alekhin's avatar
Alexander Alekhin committed
824 825 826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842
        const Mat_<double> &current_shape = (Mat_<double>)current_shapes[root[i]];
        BBox &bbox = bboxes[root[i]];
        Mat &img = imgs[root[i]];
        calcSimilarityTransform(bbox.project(current_shape), mean_shape, scale, rotate);
        for (int j = 0; j < feats_m; j++) {
            double x1 = candidate_feats(j, 0);
            double y1 = candidate_feats(j, 1);
            double x2 = candidate_feats(j, 2);
            double y2 = candidate_feats(j, 3);
            SIMILARITY_TRANSFORM(x1, y1, scale, rotate);
            SIMILARITY_TRANSFORM(x2, y2, scale, rotate);

            x1 = x1*bbox.x_scale + current_shape(landmark_id, 0);
            y1 = y1*bbox.y_scale + current_shape(landmark_id, 1);
            x2 = x2*bbox.x_scale + current_shape(landmark_id, 0);
            y2 = y2*bbox.y_scale + current_shape(landmark_id, 1);
            x1 = max(0., min(img.cols - 1., x1)); y1 = max(0., min(img.rows - 1., y1));
            x2 = max(0., min(img.cols - 1., x2)); y2 = max(0., min(img.rows - 1., y2));
            densities(j, i) = (int)img.at<uchar>(int(y1), int(x1)) - (int)img.at<uchar>(int(y2), int(x2));
843 844
        }
    }
Alexander Alekhin's avatar
Alexander Alekhin committed
845 846 847 848 849 850 851 852 853 854 855 856 857 858
    Mat_<int> densities_sorted;
    cv::sort(densities, densities_sorted, SORT_EVERY_ROW + SORT_ASCENDING);
    //select a feat which reduces maximum variance
    double variance_all = (calcVariance(delta_shapes.col(0)) + calcVariance(delta_shapes.col(1)))*N;
    double variance_reduce_max = 0;
    int threshold = 0;
    int feat_id = 0;
    std::vector<double> left_x, left_y, right_x, right_y;
    left_x.reserve(N); left_y.reserve(N);
    right_x.reserve(N); right_y.reserve(N);
    for (int j = 0; j < feats_m; j++) {
        left_x.clear(); left_y.clear();
        right_x.clear(); right_y.clear();
        int threshold_ = densities_sorted(j, (int)(N*rng.uniform(0.05, 0.95)));
859
        for (int i = 0; i < N; i++) {
Alexander Alekhin's avatar
Alexander Alekhin committed
860 861 862
            if (densities(j, i) < threshold_) {
                left_x.push_back(delta_shapes.at<double>(root[i], 0));
                left_y.push_back(delta_shapes.at<double>(root[i], 1));
863
            }
Alexander Alekhin's avatar
Alexander Alekhin committed
864 865 866
            else {
                right_x.push_back(delta_shapes.at<double>(root[i], 0));
                right_y.push_back(delta_shapes.at<double>(root[i], 1));
867 868
            }
        }
Alexander Alekhin's avatar
Alexander Alekhin committed
869 870 871 872 873 874 875 876
        double variance_ = (calcVariance(left_x) + calcVariance(left_y))*left_x.size() + \
            (calcVariance(right_x) + calcVariance(right_y))*right_x.size();
        double variance_reduce = variance_all - variance_;
        if (variance_reduce > variance_reduce_max) {
            variance_reduce_max = variance_reduce;
            threshold = threshold_;
            feat_id = j;
        }
877
    }
Alexander Alekhin's avatar
Alexander Alekhin committed
878 879 880 881 882 883 884 885 886 887
    thresholds[idx] = threshold;
    feats(idx, 0) = candidate_feats(feat_id, 0); feats(idx, 1) = candidate_feats(feat_id, 1);
    feats(idx, 2) = candidate_feats(feat_id, 2); feats(idx, 3) = candidate_feats(feat_id, 3);
    // generate left and right child
    std::vector<int> left, right;
    left.reserve(N);
    right.reserve(N);
    for (int i = 0; i < N; i++) {
        if (densities(feat_id, i) < threshold) left.push_back(root[i]);
        else right.push_back(root[i]);
888
    }
Alexander Alekhin's avatar
Alexander Alekhin committed
889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907 908 909 910 911 912 913 914 915 916 917 918 919 920 921 922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937
    // split left and right child in DFS
    if (2 * idx < feats.rows / 2)
        splitNode(imgs, current_shapes, bboxes, delta_shapes, mean_shape, left, 2 * idx, stage);
    if (2 * idx + 1 < feats.rows / 2)
        splitNode(imgs, current_shapes, bboxes, delta_shapes, mean_shape, right, 2 * idx + 1, stage);
}

void FacemarkLBFImpl::RandomTree::write(FileStorage fs, int k, int i, int j) {

    String x;
    x = cv::format("tree_%i_%i_%i",k,i,j);
    fs << x << feats;
    x = cv::format("thresholds_%i_%i_%i",k,i,j);
    fs << x << thresholds;
}

void FacemarkLBFImpl::RandomTree::read(FileStorage fs, int k, int i, int j) {
    String x;
    x = cv::format("tree_%i_%i_%i",k,i,j);
    fs[x] >> feats;
    x = cv::format("thresholds_%i_%i_%i",k,i,j);
    fs[x] >> thresholds;
}


/*---------------RandomForest Implementation---------------------*/
void FacemarkLBFImpl::RandomForest::initForest(
    int _landmark_n,
    int _trees_n,
    int _tree_depth,
    double _overlap_ratio,
    std::vector<int>_feats_m,
    std::vector<double>_radius_m,
    bool verbose_mode
) {
    trees_n = _trees_n;
    landmark_n = _landmark_n;
    tree_depth = _tree_depth;
    overlap_ratio = _overlap_ratio;

    feats_m = _feats_m;
    radius_m = _radius_m;

    verbose = verbose_mode;

    random_trees.resize(landmark_n);
    for (int i = 0; i < landmark_n; i++) {
        random_trees[i].resize(trees_n);
        for (int j = 0; j < trees_n; j++) random_trees[i][j].initTree(i, tree_depth, feats_m, radius_m);
938
    }
Alexander Alekhin's avatar
Alexander Alekhin committed
939 940 941 942 943 944 945 946 947 948 949 950 951 952 953 954 955 956 957 958
}

void FacemarkLBFImpl::RandomForest::train(std::vector<Mat> &imgs, std::vector<Mat> &current_shapes, \
                         std::vector<BBox> &bboxes, std::vector<Mat> &delta_shapes, Mat &mean_shape, int stage) {
    int N = (int)imgs.size();
    int Q = int(N / ((1. - overlap_ratio) * trees_n));

    #ifdef _OPENMP
    #pragma omp parallel for
    #endif
    for (int i = 0; i < landmark_n; i++) {
    TIMER_BEGIN
        std::vector<int> root;
        for (int j = 0; j < trees_n; j++) {
            int start = max(0, int(floor(j*Q - j*Q*overlap_ratio)));
            int end = min(int(start + Q + 1), N);
            int L = end - start;
            root.resize(L);
            for (int k = 0; k < L; k++) root[k] = start + k;
            random_trees[i][j].train(imgs, current_shapes, bboxes, delta_shapes, mean_shape, root, stage);
959
        }
Alexander Alekhin's avatar
Alexander Alekhin committed
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
        if(verbose) printf("Train %2dth of %d landmark Done, it costs %.4lf s\n", i+1, landmark_n, TIMER_NOW);
    TIMER_END
    }
}

Mat FacemarkLBFImpl::RandomForest::generateLBF(Mat &img, Mat &current_shape, BBox &bbox, Mat &mean_shape) {
    Mat_<int> lbf_feat(1, landmark_n*trees_n);
    double scale;
    Mat_<double> rotate;
    calcSimilarityTransform(bbox.project(current_shape), mean_shape, scale, rotate);

    int base = 1 << (tree_depth - 1);

    #ifdef _OPENMP
    #pragma omp parallel for
    #endif
    for (int i = 0; i < landmark_n; i++) {
        for (int j = 0; j < trees_n; j++) {
            RandomTree &tree = random_trees[i][j];
            int code = 0;
            int idx = 1;
            for (int k = 1; k < tree.depth; k++) {
                double x1 = tree.feats(idx, 0);
                double y1 = tree.feats(idx, 1);
                double x2 = tree.feats(idx, 2);
                double y2 = tree.feats(idx, 3);
986 987 988
                SIMILARITY_TRANSFORM(x1, y1, scale, rotate);
                SIMILARITY_TRANSFORM(x2, y2, scale, rotate);

Alexander Alekhin's avatar
Alexander Alekhin committed
989 990 991 992
                x1 = x1*bbox.x_scale + current_shape.at<double>(i, 0);
                y1 = y1*bbox.y_scale + current_shape.at<double>(i, 1);
                x2 = x2*bbox.x_scale + current_shape.at<double>(i, 0);
                y2 = y2*bbox.y_scale + current_shape.at<double>(i, 1);
993 994
                x1 = max(0., min(img.cols - 1., x1)); y1 = max(0., min(img.rows - 1., y1));
                x2 = max(0., min(img.cols - 1., x2)); y2 = max(0., min(img.rows - 1., y2));
Alexander Alekhin's avatar
Alexander Alekhin committed
995 996 997 998
                int density = img.at<uchar>(int(y1), int(x1)) - img.at<uchar>(int(y2), int(x2));
                code <<= 1;
                if (density < tree.thresholds[idx]) {
                    idx = 2 * idx;
999 1000
                }
                else {
Alexander Alekhin's avatar
Alexander Alekhin committed
1001 1002
                    code += 1;
                    idx = 2 * idx + 1;
1003 1004
                }
            }
Alexander Alekhin's avatar
Alexander Alekhin committed
1005
            lbf_feat(i*trees_n + j) = (i*trees_n + j)*base + code;
1006 1007
        }
    }
Alexander Alekhin's avatar
Alexander Alekhin committed
1008 1009
    return lbf_feat;
}
1010

Alexander Alekhin's avatar
Alexander Alekhin committed
1011 1012 1013 1014 1015
void FacemarkLBFImpl::RandomForest::write(FileStorage fs, int k) {
    for (int i = 0; i < landmark_n; i++) {
        for (int j = 0; j < trees_n; j++) {
            random_trees[i][j].write(fs,k,i,j);
        }
1016
    }
Alexander Alekhin's avatar
Alexander Alekhin committed
1017 1018 1019 1020 1021 1022 1023 1024
}

void FacemarkLBFImpl::RandomForest::read(FileStorage fs,int k)
{
    for (int i = 0; i < landmark_n; i++) {
        for (int j = 0; j < trees_n; j++) {
            random_trees[i][j].initTree(i, tree_depth, feats_m, radius_m);
            random_trees[i][j].read(fs,k,i,j);
1025 1026
        }
    }
Alexander Alekhin's avatar
Alexander Alekhin committed
1027 1028 1029 1030 1031 1032 1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045 1046 1047 1048 1049 1050 1051 1052 1053 1054 1055 1056 1057
}

/*---------------Regressor Implementation---------------------*/
void FacemarkLBFImpl::Regressor::initRegressor(Params config) {
    stages_n = config.stages_n;
    landmark_n = config.n_landmarks;

    random_forests.resize(stages_n);
    for (int i = 0; i < stages_n; i++)
        random_forests[i].initForest(
            config.n_landmarks,
            config.tree_n,
            config.tree_depth,
            config.bagging_overlap,
            config.feats_m,
            config.radius_m,
            config.verbose
        );

    mean_shape.create(config.n_landmarks, 2, CV_64FC1);

    gl_regression_weights.resize(stages_n);
    int F = config.n_landmarks * config.tree_n * (1 << (config.tree_depth - 1));

    for (int i = 0; i < stages_n; i++) {
        gl_regression_weights[i].create(2 * config.n_landmarks, F, CV_64FC1);
    }
}

void FacemarkLBFImpl::Regressor::trainRegressor(std::vector<Mat> &imgs, std::vector<Mat> &gt_shapes, std::vector<Mat> &current_shapes,
                        std::vector<BBox> &bboxes, Mat &mean_shape_, int start_from, Params config) {
Alexander Alekhin's avatar
Alexander Alekhin committed
1058
    CV_Assert(start_from >= 0 && start_from < stages_n);
Alexander Alekhin's avatar
Alexander Alekhin committed
1059 1060
    mean_shape = mean_shape_;
    int N = (int)imgs.size();
1061

Alexander Alekhin's avatar
Alexander Alekhin committed
1062 1063
    for (int k = start_from; k < stages_n; k++) {
        std::vector<Mat> delta_shapes = getDeltaShapes(gt_shapes, current_shapes, bboxes, mean_shape);
1064

Alexander Alekhin's avatar
Alexander Alekhin committed
1065 1066
        // train random forest
        if(config.verbose) printf("training random forest %dth of %d stages, ",k+1, stages_n);
1067
        TIMER_BEGIN
Alexander Alekhin's avatar
Alexander Alekhin committed
1068 1069
            random_forests[k].train(imgs, current_shapes, bboxes, delta_shapes, mean_shape, k);
            if(config.verbose) printf("costs %.4lf s\n",  TIMER_NOW);
1070 1071
        TIMER_END

Alexander Alekhin's avatar
Alexander Alekhin committed
1072 1073 1074 1075 1076
        // generate lbf of every train data
        std::vector<Mat> lbfs;
        lbfs.resize(N);
        for (int i = 0; i < N; i++) {
            lbfs[i] = random_forests[k].generateLBF(imgs[i], current_shapes[i], bboxes[i], mean_shape);
1077 1078
        }

Alexander Alekhin's avatar
Alexander Alekhin committed
1079 1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092
        // global regression
        if(config.verbose) printf("start train global regression of %dth stage\n", k);
        TIMER_BEGIN
            globalRegressionTrain(lbfs, delta_shapes, k, config);
            if(config.verbose) printf("end of train global regression of %dth stage, costs %.4lf s\n", k, TIMER_NOW);
        TIMER_END

        // update current_shapes
        double scale;
        Mat rotate;
        for (int i = 0; i < N; i++) {
            Mat delta_shape = globalRegressionPredict(lbfs[i], k);
            calcSimilarityTransform(bboxes[i].project(current_shapes[i]), mean_shape, scale, rotate);
            current_shapes[i] = bboxes[i].reproject(bboxes[i].project(current_shapes[i]) + scale * delta_shape * rotate.t());
1093 1094
        }

Alexander Alekhin's avatar
Alexander Alekhin committed
1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117
        // calc mean error
        double e = calcMeanError(gt_shapes, current_shapes, config.n_landmarks, config.pupils[0],config.pupils[1]);
        if(config.verbose) printf("Train %dth stage Done with Error = %lf\n", k, e);

    } // for int k
}//Regressor::training

void FacemarkLBFImpl::Regressor::globalRegressionTrain(
    std::vector<Mat> &lbfs, std::vector<Mat> &delta_shapes,
    int stage, Params config
) {

    int N = (int)lbfs.size();
    int M = lbfs[0].cols;
    int F = config.n_landmarks*config.tree_n*(1 << (config.tree_depth - 1));
    int landmark_n_ = delta_shapes[0].rows;
    feature_node **X = (feature_node **)malloc(N * sizeof(feature_node *));
    double **Y = (double **)malloc(landmark_n_ * 2 * sizeof(double *));
    for (int i = 0; i < N; i++) {
        X[i] = (feature_node *)malloc((M + 1) * sizeof(feature_node));
        for (int j = 0; j < M; j++) {
            X[i][j].index = lbfs[i].at<int>(0, j) + 1; // index starts from 1
            X[i][j].value = 1;
1118
        }
Alexander Alekhin's avatar
Alexander Alekhin committed
1119 1120
        X[i][M].index = -1;
        X[i][M].value = -1;
1121
    }
Alexander Alekhin's avatar
Alexander Alekhin committed
1122 1123 1124 1125 1126 1127
    for (int i = 0; i < landmark_n_; i++) {
        Y[2 * i] = (double *)malloc(N*sizeof(double));
        Y[2 * i + 1] = (double *)malloc(N*sizeof(double));
        for (int j = 0; j < N; j++) {
            Y[2 * i][j] = delta_shapes[j].at<double>(i, 0);
            Y[2 * i + 1][j] = delta_shapes[j].at<double>(i, 1);
1128 1129 1130
        }
    }

Alexander Alekhin's avatar
Alexander Alekhin committed
1131 1132 1133 1134 1135 1136
    double *y;
    Mat weights;
    for(int i=0; i< landmark_n_; i++){
        y =  Y[2 * i];
        Mat wx = supportVectorRegression(X,y,N,F,config.verbose);
        weights.push_back(wx);
1137

Alexander Alekhin's avatar
Alexander Alekhin committed
1138 1139 1140 1141
        y = Y[2 * i + 1];
        Mat wy = supportVectorRegression(X,y,N,F,config.verbose);
        weights.push_back(wy);
    }
1142

Alexander Alekhin's avatar
Alexander Alekhin committed
1143
    gl_regression_weights[stage] = weights;
1144

Alexander Alekhin's avatar
Alexander Alekhin committed
1145 1146 1147 1148 1149 1150
    // free
    for (int i = 0; i < N; i++) free(X[i]);
    for (int i = 0; i < 2 * landmark_n_; i++) free(Y[i]);
    free(X);
    free(Y);
} // Regressor:globalRegressionTrain
1151

Alexander Alekhin's avatar
Alexander Alekhin committed
1152 1153 1154 1155 1156 1157 1158 1159 1160 1161 1162 1163 1164 1165 1166 1167 1168 1169 1170 1171 1172 1173 1174 1175 1176 1177 1178 1179 1180 1181 1182 1183 1184 1185 1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196 1197 1198 1199 1200 1201
/*adapted from the liblinear library*/
/* TODO: change feature_node to MAT
* as the index in feature_node is only used for "counter"
*/
Mat FacemarkLBFImpl::Regressor::supportVectorRegression(
    feature_node **x, double *y, int nsamples, int feat_size, bool verbose
){
    #define GETI(i) ((int) y[i])

    std::vector<double> w;
    w.resize(feat_size);

    RNG rng(0);
    int l = nsamples; // n-samples
    double C = 1./(double)nsamples;
    double p = 0;
    int w_size = feat_size; //feat size
    double eps =  0.00001;
    int i, s, iter = 0;
    int max_iter = 1000;
    int active_size = l;
    std::vector<int> index(l);

    double d, G, H;
    double Gmax_old = HUGE_VAL;
    double Gmax_new, Gnorm1_new;
    double Gnorm1_init = -1.0; // Gnorm1_init is initialized at the first iteration
    std::vector<double> beta(l);
    std::vector<double> QD(l);

    double lambda[1], upper_bound[1];
    lambda[0] = 0.5/C;
    upper_bound[0] = HUGE_VAL;

    // Initial beta can be set here. Note that
    // -upper_bound <= beta[i] <= upper_bound
    for(i=0; i<l; i++)
        beta[i] = 0;

    for(i=0; i<w_size; i++)
        w[i] = 0;

    for(i=0; i<l; i++){
        QD[i] = 0;
        feature_node *xi = x[i];
        while(xi->index != -1){
            double val = xi->value;
            QD[i] += val*val;
            w[xi->index-1] += beta[i]*val;
            xi++;
1202
        }
Alexander Alekhin's avatar
Alexander Alekhin committed
1203 1204
        index[i] = i;
    }
1205

Alexander Alekhin's avatar
Alexander Alekhin committed
1206 1207 1208
    while(iter < max_iter){
        Gmax_new = 0;
        Gnorm1_new = 0;
1209

Alexander Alekhin's avatar
Alexander Alekhin committed
1210 1211 1212
        for(i=0; i<active_size; i++){
            int j = i+rng.uniform(0,RAND_MAX)%(active_size-i);
            swap(index[i], index[j]);
1213 1214
        }

Alexander Alekhin's avatar
Alexander Alekhin committed
1215 1216 1217 1218 1219
        for(s=0; s<active_size; s++){
            i = index[s];
            G = -y[i] + lambda[GETI(i)]*beta[i];
            H = QD[i] + lambda[GETI(i)];

1220 1221
            feature_node *xi = x[i];
            while(xi->index != -1){
Alexander Alekhin's avatar
Alexander Alekhin committed
1222
                int ind = xi->index-1;
1223
                double val = xi->value;
Alexander Alekhin's avatar
Alexander Alekhin committed
1224
                G += val*w[ind];
1225 1226 1227
                xi++;
            }

Alexander Alekhin's avatar
Alexander Alekhin committed
1228 1229 1230 1231 1232 1233 1234 1235 1236 1237 1238 1239 1240
            double Gp = G+p;
            double Gn = G-p;
            double violation = 0;
            if(beta[i] == 0){
                if(Gp < 0)
                    violation = -Gp;
                else if(Gn > 0)
                    violation = Gn;
                else if(Gp>Gmax_old && Gn<-Gmax_old){
                    active_size--;
                    swap(index[s], index[active_size]);
                    s--;
                    continue;
1241
                }
Alexander Alekhin's avatar
Alexander Alekhin committed
1242 1243 1244 1245 1246 1247 1248
            }else if(beta[i] >= upper_bound[GETI(i)]){
                if(Gp > 0)
                    violation = Gp;
                else if(Gp < -Gmax_old){
                    active_size--;
                    swap(index[s], index[active_size]);
                    s--;
1249 1250
                    continue;
                }
Alexander Alekhin's avatar
Alexander Alekhin committed
1251 1252 1253 1254 1255 1256 1257
            }else if(beta[i] <= -upper_bound[GETI(i)]){
                if(Gn < 0)
                    violation = -Gn;
                else if(Gn > Gmax_old){
                    active_size--;
                    swap(index[s], index[active_size]);
                    s--;
1258 1259
                    continue;
                }
Alexander Alekhin's avatar
Alexander Alekhin committed
1260 1261 1262 1263 1264 1265 1266 1267 1268 1269 1270 1271 1272 1273 1274 1275 1276 1277 1278 1279 1280 1281
            }else if(beta[i] > 0)
                violation = fabs(Gp);
            else
                violation = fabs(Gn);

            Gmax_new = max(Gmax_new, violation);
            Gnorm1_new += violation;

            // obtain Newton direction d
            if(Gp < H*beta[i])
                d = -Gp/H;
            else if(Gn > H*beta[i])
                d = -Gn/H;
            else
                d = -beta[i];

            if(fabs(d) < 1.0e-12)
                continue;

            double beta_old = beta[i];
            beta[i] = min(max(beta[i]+d, -upper_bound[GETI(i)]), upper_bound[GETI(i)]);
            d = beta[i]-beta_old;
1282

Alexander Alekhin's avatar
Alexander Alekhin committed
1283 1284 1285 1286 1287 1288 1289 1290 1291 1292 1293 1294 1295 1296 1297 1298 1299 1300 1301 1302 1303
            if(d != 0){
                xi = x[i];
                while(xi->index != -1){
                    w[xi->index-1] += d*xi->value;
                    xi++;
                }
            }
        }// for s<active_size

        if(iter == 0)
            Gnorm1_init = Gnorm1_new;
        iter++;

        if(Gnorm1_new <= eps*Gnorm1_init){
            if(active_size == l)
                break;
            else{
                active_size = l;
                Gmax_old = HUGE_VAL;
                continue;
            }
1304 1305
        }

Alexander Alekhin's avatar
Alexander Alekhin committed
1306 1307 1308 1309 1310 1311 1312 1313 1314 1315 1316 1317 1318 1319 1320 1321 1322 1323
        Gmax_old = Gmax_new;
    } //while <max_iter

    if(verbose) printf("optimization finished, #iter = %d\n", iter);
    if(iter >= max_iter && verbose)
        printf("WARNING: reaching max number of iterations\n");

    // calculate objective value
    double v = 0;
    int nSV = 0;
    for(i=0; i<w_size; i++)
        v += w[i]*w[i];
    v = 0.5*v;
    for(i=0; i<l; i++){
        v += p*fabs(beta[i]) - y[i]*beta[i] + 0.5*lambda[GETI(i)]*beta[i]*beta[i];
        if(beta[i] != 0)
            nSV++;
    }
1324

Alexander Alekhin's avatar
Alexander Alekhin committed
1325 1326
    if(verbose) printf("Objective value = %lf\n", v);
    if(verbose) printf("nSV = %d\n",nSV);
1327

Alexander Alekhin's avatar
Alexander Alekhin committed
1328
    return Mat(Mat(w).t()).clone();
1329

Alexander Alekhin's avatar
Alexander Alekhin committed
1330
}//end
1331

Alexander Alekhin's avatar
Alexander Alekhin committed
1332 1333 1334 1335 1336
Mat FacemarkLBFImpl::Regressor::globalRegressionPredict(const Mat &lbf, int stage) {
    const Mat_<double> &weight = (Mat_<double>)gl_regression_weights[stage];
    Mat_<double> delta_shape(weight.rows / 2, 2);
    const double *w_ptr = NULL;
    const int *lbf_ptr = lbf.ptr<int>(0);
1337

Alexander Alekhin's avatar
Alexander Alekhin committed
1338 1339 1340 1341 1342 1343
    //#pragma omp parallel for num_threads(2) private(w_ptr)
    for (int i = 0; i < delta_shape.rows; i++) {
        w_ptr = weight.ptr<double>(2 * i);
        double y = 0;
        for (int j = 0; j < lbf.cols; j++) y += w_ptr[lbf_ptr[j]];
        delta_shape(i, 0) = y;
1344

Alexander Alekhin's avatar
Alexander Alekhin committed
1345 1346 1347 1348 1349 1350 1351 1352 1353 1354 1355 1356 1357 1358 1359 1360 1361 1362 1363 1364 1365 1366 1367 1368
        w_ptr = weight.ptr<double>(2 * i + 1);
        y = 0;
        for (int j = 0; j < lbf.cols; j++) y += w_ptr[lbf_ptr[j]];
        delta_shape(i, 1) = y;
    }
    return delta_shape;
} // Regressor::globalRegressionPredict

Mat FacemarkLBFImpl::Regressor::predict(Mat &img, BBox &bbox) {
    Mat current_shape = bbox.reproject(mean_shape);
    double scale;
    Mat rotate;
    Mat lbf_feat;
    for (int k = 0; k < stages_n; k++) {
        // generate lbf
        lbf_feat = random_forests[k].generateLBF(img, current_shape, bbox, mean_shape);
        // update current_shapes
        Mat delta_shape = globalRegressionPredict(lbf_feat, k);
        delta_shape = delta_shape.reshape(0, landmark_n);
        calcSimilarityTransform(bbox.project(current_shape), mean_shape, scale, rotate);
        current_shape = bbox.reproject(bbox.project(current_shape) + scale * delta_shape * rotate.t());
    }
    return current_shape;
} // Regressor::predict
1369

Alexander Alekhin's avatar
Alexander Alekhin committed
1370
void FacemarkLBFImpl::Regressor::write(FileStorage fs, Params config) {
1371

Alexander Alekhin's avatar
Alexander Alekhin committed
1372 1373 1374 1375
    fs << "stages_n" << config.stages_n;
    fs << "tree_n" << config.tree_n;
    fs << "tree_depth" << config.tree_depth;
    fs << "n_landmarks" << config.n_landmarks;
1376

Alexander Alekhin's avatar
Alexander Alekhin committed
1377
    fs << "regressor_meanshape" <<mean_shape;
1378

Alexander Alekhin's avatar
Alexander Alekhin committed
1379 1380 1381 1382 1383 1384 1385
    // every stages
    String x;
    for (int k = 0; k < config.stages_n; k++) {
        if(config.verbose) printf("Write %dth stage\n", k);
        random_forests[k].write(fs,k);
        x = cv::format("weights_%i",k);
        fs << x << gl_regression_weights[k];
1386
    }
Alexander Alekhin's avatar
Alexander Alekhin committed
1387 1388 1389 1390 1391 1392 1393 1394 1395 1396 1397 1398 1399 1400 1401 1402 1403 1404 1405 1406 1407 1408 1409 1410 1411 1412 1413 1414 1415 1416 1417
}

void FacemarkLBFImpl::Regressor::read(FileStorage fs, Params & config){
    fs["stages_n"] >>  config.stages_n;
    fs["tree_n"] >>  config.tree_n;
    fs["tree_depth"] >>  config.tree_depth;
    fs["n_landmarks"] >>  config.n_landmarks;

    stages_n = config.stages_n;
    landmark_n = config.n_landmarks;

    initRegressor(config);

    fs["regressor_meanshape"] >>  mean_shape;

    // every stages
    String x;
    for (int k = 0; k < stages_n; k++) {
        random_forests[k].initForest(
            config.n_landmarks,
            config.tree_n,
            config.tree_depth,
            config.bagging_overlap,
            config.feats_m,
            config.radius_m,
            config.verbose
        );
        random_forests[k].read(fs,k);

        x = cv::format("weights_%i",k);
        fs[x] >> gl_regression_weights[k];
1418
    }
Alexander Alekhin's avatar
Alexander Alekhin committed
1419
}
1420

Alexander Alekhin's avatar
Alexander Alekhin committed
1421 1422 1423 1424
#undef TIMER_BEGIN
#undef TIMER_NOW
#undef TIMER_END
#undef SIMILARITY_TRANSFORM
1425 1426
} /* namespace face */
} /* namespace cv */