caffe-face特征提取中五点对齐优化

Caffe-Face 特征提取中五点对齐优化

align.cpp：

#include <iostream>
#include <cmath>
#include <cstdio>
#include <memory.h>

using namespace std;

const double DST_5POINTS[10] = {30.2946, 65.5318, 48.0252, 33.5493, 62.7299, 51.6963, 51.5014, 71.7366, 92.3655, 92.2041};
double src[10] = {105.8306, 147.9323, 121.3533, 106.1169, 144.3622, 109.8005, 112.5533, 139.1172, 155.6359, 156.3451};

void printMat(double* M, int row, int col=0)
{
    if (col == 0) {
        for (int j = 0; j < row; ++j) {
            printf("%10.4f ", M[j]);
        }
        printf("\n[ Vector with size %d ]\n\n", row);       
    }
    else {
        for (int i = 0; i < row; ++i) {
            for (int j = 0; j < col; ++j) {
                printf("%10.4f ", M[i*col+j]);
            }
            printf("\n");
        }
        printf("[ Mat with size %dx%d ]\n\n", row, col);
    }


}

// src: source points: size=10 vector with shape (2, 5) 
// dst: dest points: size=10 vector with shape (2, 5)
// M: rotation matrix with shape (2, 3)
void getAffineMatrix(double* src_5pts, const double* dst_5pts, double* M)
{
    double src[10], dst[10];
    memcpy(src, src_5pts, sizeof(double)*10);
    memcpy(dst, dst_5pts, sizeof(double)*10);
    // initialization
    double ptmp[2];
    ptmp[0] = ptmp[1] = 0;
    for (int i = 0; i < 5; ++i) {
        ptmp[0] += src[i];
        ptmp[1] += src[5+i];
    }
    ptmp[0] /= 5;
    ptmp[1] /= 5;
    for (int i = 0; i < 5; ++i) {
        src[i] -= ptmp[0];
        src[5+i] -= ptmp[1];
        dst[i] -= ptmp[0];
        dst[5+i] -= ptmp[1];
    }
    // determine init angle
    double dst_x = (dst[3]+dst[4]-dst[0]-dst[1])/2, dst_y = (dst[8]+dst[9]-dst[5]-dst[6])/2;
    double src_x = (src[3]+src[4]-src[0]-src[1])/2, src_y = (src[8]+src[9]-src[5]-src[6])/2;
    double theta = atan2(dst_x, dst_y) - atan2(src_x, src_y);
    // determine init scale
    double scale = sqrt(pow(dst_x, 2) + pow(dst_y, 2)) / sqrt(pow(src_x, 2) + pow(src_y, 2));
    double pts1[10];
    double pts0[2];
    double _a = sin(theta), _b = cos(theta);
    pts0[0] = pts0[1] = 0;
    for (int i = 0; i < 5; ++i) {
        pts1[i] = scale*(src[i]*_b + src[i+5]*_a);
        pts1[i+5] = scale*(-src[i]*_a + src[i+5]*_b);
        pts0[0] += (dst[i] - pts1[i]);
        pts0[1] += (dst[i+5] - pts1[i+5]);
    }
    pts0[0] /= 5;
    pts0[1] /= 5;

    double sqloss = 0;
    for (int i = 0; i < 5; ++i) {
        sqloss += ((pts0[0]+pts1[i]-dst[i])*(pts0[0]+pts1[i]-dst[i])
                + (pts0[1]+pts1[i+5]-dst[i+5])*(pts0[1]+pts1[i+5]-dst[i+5]));
    }
    // optimization
    double square_sum = 0;
    for (int i = 0; i < 10; ++i) {
        square_sum += src[i]*src[i];
    }
    for (int t = 0; t < 200; ++t) {
        //cout << sqloss << endl;
        // determine angle
        _a = 0;
        _b = 0;
        for (int i = 0; i < 5; ++i) {
            _a += ((pts0[0]-dst[i])*src[i+5] - (pts0[1]-dst[i+5])*src[i]);
            _b += ((pts0[0]-dst[i])*src[i] + (pts0[1]-dst[i+5])*src[i+5]);
        }
        if (_b < 0) {
            _b = -_b;
            _a = -_a;
        }
        double _s = sqrt(_a*_a + _b*_b);
        _b /= _s;
        _a /= _s;

        for (int i = 0; i < 5; ++i) {
            pts1[i] = scale*(src[i]*_b + src[i+5]*_a);
            pts1[i+5] = scale*(-src[i]*_a + src[i+5]*_b);
        }

        // determine scale
        double _scale = 0;
        for (int i = 0; i < 5; ++i) {
            _scale += ((dst[i]-pts0[0])*pts1[i] + (dst[i+5]-pts0[1])*pts1[i+5]);
        }
        _scale /= (square_sum*scale);
        for (int i = 0; i < 10; ++i) {
            pts1[i] *= (_scale / scale);
        }
        scale = _scale;

        // determine pts0
        pts0[0] = pts0[1] = 0;
        for (int i = 0; i < 5; ++i) {
            pts0[0] += (dst[i] - pts1[i]);
            pts0[1] += (dst[i+5] - pts1[i+5]);
        }
        pts0[0] /= 5;
        pts0[1] /= 5;

        double _sqloss = 0;
        for (int i = 0; i < 5; ++i) {
            _sqloss += ((pts0[0]+pts1[i]-dst[i])*(pts0[0]+pts1[i]-dst[i])
                    + (pts0[1]+pts1[i+5]-dst[i+5])*(pts0[1]+pts1[i+5]-dst[i+5]));
        }
        if (abs(_sqloss - sqloss) < 1e-2) {
            break;
        }
        sqloss = _sqloss;
    }
    // generate affine matrix
    for (int i = 0; i < 5; ++i) {
        pts1[i] += (pts0[0] + ptmp[0]);
        pts1[i+5] += (pts0[1] + ptmp[1]);
    }
    // printMat(pts1, 2, 5);
    M[0] = _b*scale;
    M[1] = _a*scale;
    M[3] = -_a*scale;
    M[4] = _b*scale;
    M[2] = pts0[0] + ptmp[0] - scale*(ptmp[0]*_b + ptmp[1]*_a);
    M[5] = pts0[1] + ptmp[1] - scale*(-ptmp[0]*_a + ptmp[1]*_b);
}

int main()
{
    double M[6];
    for (int i = 0; i < 10000; ++i)
        getAffineMatrix(src, DST_5POINTS, M);
    printMat(M, 2, 3);
    return 0;
}

align.py：

import numpy as np
import time


def getAffineMatrix(src, dst):
    # initialization
    # determine init rotmat
    ptmp = np.mean(src, axis=0)
    src = src - ptmp
    dst = dst - ptmp
    theta = np.arctan2(*((dst[3]+dst[4])/2-(dst[0]+dst[1])/2)) - np.arctan2(*((src[3]+src[4])/2-(src[0]+src[1])/2))
    # determine scale
    scale = np.linalg.norm((dst[3]+dst[4])/2-(dst[0]+dst[1])/2) / np.linalg.norm((src[3]+src[4])/2-(src[0]+src[1])/2)
    pts1 = np.dot(src, scale*np.array([[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]]))
    pts0 = np.mean(dst-pts1, axis=0)
    sqloss = np.sum((pts1+pts0-dst)**2)
    # optimization
    square_sum = np.sum(src*src)
    for i in range(200):
        print sqloss
        # determine angle
        _a = np.sum((pts0-dst)*(src[:,::-1]*np.array([1.,-1.])))
        _b = np.sum((pts0-dst)*src)
        print _a, _b
        pts1 = np.dot(src, np.sign(_b)*scale/np.sqrt(_a**2+_b**2)*np.array([[_b,-_a],[_a,_b]]))
        # determine scale
        print pts1
        _scale = np.sum((dst-pts0)*pts1) / (square_sum*scale)
        pts1 = _scale / scale * pts1
        scale = _scale
        pts0 = np.mean(dst-pts1, axis=0)

        _sqloss = np.sum((pts1+pts0-dst)**2)
        if np.abs(_sqloss-sqloss) < 1e-2:
            break
        sqloss = _sqloss
    print 
    print pts1+pts0+ptmp
    rotmat = np.sign(_b)*scale/np.sqrt(_a**2+_b**2)*np.array([[_b,-_a],[_a,_b]])
    spts = pts0 - np.dot(ptmp, rotmat) + ptmp
    M = np.hstack([rotmat.T, spts.reshape((2,1))])
    return M


DST_5POINTS = np.array([30.2946, 65.5318, 48.0252, 33.5493, 62.7299, 51.6963, 51.5014, 71.7366, 92.3655, 92.2041]).reshape(2,5).T
src = np.array([105.8306, 147.9323, 121.3533, 106.1169, 144.3622, 109.8005, 112.5533, 139.1172, 155.6359, 156.3451]).reshape(2,5).T

tic = time.time()
M = getAffineMatrix(src, DST_5POINTS)
toc = time.time()
print toc - tic
print M