00001 import sys
00002 import cv2
00003 import numpy as np
00004 import os
00005 import time
00006 
00007 
00008 
00009 
00010 
00011 
00012 import caffe
00013 
00014 
00015 def generate_bounding_box(map, reg, scale, t):
00016     stride = 2
00017     cellsize = 12
00018     map = map.T
00019     dx1 = reg[0, :, :].T
00020     dy1 = reg[1, :, :].T
00021     dx2 = reg[2, :, :].T
00022     dy2 = reg[3, :, :].T
00023     (x, y) = np.where(map >= t)
00024 
00025     yy = y
00026     xx = x
00027 
00028     score = map[x, y]
00029     reg = np.array([dx1[x, y], dy1[x, y], dx2[x, y], dy2[x, y]])
00030 
00031     if reg.shape[0] == 0:
00032         pass
00033     boundingbox = np.array([yy, xx]).T
00034 
00035     bb1 = np.fix((stride * (boundingbox)) / scale).T
00036     bb2 = np.fix((stride * (boundingbox) + cellsize) / scale).T
00037     score = np.array([score])
00038 
00039     boundingbox_out = np.concatenate((bb1, bb2, score, reg), axis=0)
00040 
00041     return boundingbox_out.T
00042 
00043 
00044 def nms(boxes, threshold, type):
00045     """nms
00046     :boxes: [:,0:5]
00047     :threshold: 0.5 like
00048     :type: 'Min' or others
00049     :returns: TODO
00050     """
00051     if boxes.shape[0] == 0:
00052         return np.array([])
00053     x1 = boxes[:, 0]
00054     y1 = boxes[:, 1]
00055     x2 = boxes[:, 2]
00056     y2 = boxes[:, 3]
00057     s = boxes[:, 4]
00058     area = np.multiply(x2-x1+1, y2-y1+1)
00059     
00060     I = np.array(s.argsort())
00061 
00062     pick = []
00063     while len(I) > 0:
00064         xx1 = np.maximum(x1[I[-1]], x1[I[0:-1]])
00065         yy1 = np.maximum(y1[I[-1]], y1[I[0:-1]])
00066         xx2 = np.minimum(x2[I[-1]], x2[I[0:-1]])
00067         yy2 = np.minimum(y2[I[-1]], y2[I[0:-1]])
00068         w = np.maximum(0.0, xx2 - xx1 + 1)
00069         h = np.maximum(0.0, yy2 - yy1 + 1)
00070         inter = w * h
00071         if type == 'Min':
00072             o = inter / np.minimum(area[I[-1]], area[I[0:-1]])
00073         else:
00074             o = inter / (area[I[-1]] + area[I[0:-1]] - inter)
00075         pick.append(I[-1])
00076         I = I[np.where(o <= threshold)[0]]
00077     return pick
00078 
00079 
00080 def bbreg(boundingbox, reg):
00081     reg = reg.T
00082     
00083     if reg.shape[1] == 1:
00084         print "reshape of reg"
00085         pass
00086     w = boundingbox[:, 2] - boundingbox[:, 0] + 1
00087     h = boundingbox[:, 3] - boundingbox[:, 1] + 1
00088 
00089     bb0 = boundingbox[:, 0] + reg[:, 0]*w
00090     bb1 = boundingbox[:, 1] + reg[:, 1]*h
00091     bb2 = boundingbox[:, 2] + reg[:, 2]*w
00092     bb3 = boundingbox[:, 3] + reg[:, 3]*h
00093 
00094     boundingbox[:, 0:4] = np.array([bb0, bb1, bb2, bb3]).T
00095     return boundingbox
00096 
00097 
00098 def pad(boxesA, w, h):
00099     boxes = boxesA.copy()
00100 
00101     tmph = boxes[:, 3] - boxes[:, 1] + 1
00102     tmpw = boxes[:, 2] - boxes[:, 0] + 1
00103     numbox = boxes.shape[0]
00104 
00105     dx = np.ones(numbox)
00106     dy = np.ones(numbox)
00107     edx = tmpw
00108     edy = tmph
00109 
00110     x = boxes[:, 0:1][:, 0]
00111     y = boxes[:, 1:2][:, 0]
00112     ex = boxes[:, 2:3][:, 0]
00113     ey = boxes[:, 3:4][:, 0]
00114 
00115     tmp = np.where(ex > w)[0]
00116     if tmp.shape[0] != 0:
00117         edx[tmp] = -ex[tmp] + w-1 + tmpw[tmp]
00118         ex[tmp] = w-1
00119 
00120     tmp = np.where(ey > h)[0]
00121     if tmp.shape[0] != 0:
00122         edy[tmp] = -ey[tmp] + h-1 + tmph[tmp]
00123         ey[tmp] = h-1
00124 
00125     tmp = np.where(x < 1)[0]
00126     if tmp.shape[0] != 0:
00127         dx[tmp] = 2 - x[tmp]
00128         x[tmp] = np.ones_like(x[tmp])
00129 
00130     tmp = np.where(y < 1)[0]
00131     if tmp.shape[0] != 0:
00132         dy[tmp] = 2 - y[tmp]
00133         y[tmp] = np.ones_like(y[tmp])
00134 
00135     
00136     dy = np.maximum(0, dy-1)
00137     dx = np.maximum(0, dx-1)
00138     y = np.maximum(0, y-1)
00139     x = np.maximum(0, x-1)
00140     edy = np.maximum(0, edy-1)
00141     edx = np.maximum(0, edx-1)
00142     ey = np.maximum(0, ey-1)
00143     ex = np.maximum(0, ex-1)
00144 
00145     return [dy, edy, dx, edx, y, ey, x, ex, tmpw, tmph]
00146 
00147 
00148 def rerec(bboxA):
00149     
00150     w = bboxA[:, 2] - bboxA[:, 0]
00151     h = bboxA[:, 3] - bboxA[:, 1]
00152     l = np.maximum(w, h).T
00153 
00154     bboxA[:, 0] = bboxA[:, 0] + w*0.5 - l*0.5
00155     bboxA[:, 1] = bboxA[:, 1] + h*0.5 - l*0.5
00156     bboxA[:, 2:4] = bboxA[:, 0:2] + np.repeat([l], 2, axis=0).T
00157     return bboxA
00158 
00159 
00160 class FaceDetector:
00161     def __init__(self, use_gpu=True):
00162         self.min_size = 20
00163         self.threshold = [0.6, 0.7, 0.7]
00164         self.factor = 0.709
00165         self.use_gpu = use_gpu
00166         
00167         
00168         
00169         caffe_model_path = os.path.dirname(os.path.dirname(os.path.abspath(sys.argv[0]))) + '/model'
00170         self.p_net = caffe.Net(caffe_model_path + "/det1.prototxt", caffe_model_path + "/det1.caffemodel", caffe.TEST)
00171         self.r_net = caffe.Net(caffe_model_path + "/det2.prototxt", caffe_model_path + "/det2.caffemodel", caffe.TEST)
00172         self.o_net = caffe.Net(caffe_model_path + "/det3.prototxt", caffe_model_path + "/det3.caffemodel", caffe.TEST)
00173 
00174     def find_faces(self, input_image):
00175         if self.use_gpu:
00176             caffe.set_mode_gpu()
00177             caffe.set_device(0)
00178         start_time = time.time()
00179         factor_count = 0
00180         total_boxes = np.zeros((0, 9), np.float)
00181         points = []
00182         h = input_image.shape[0]
00183         w = input_image.shape[1]
00184         minl = min(h, w)
00185         input_image = input_image.astype(float)
00186         m = 12.0 / self.min_size
00187         minl *= m
00188 
00189         
00190         scales = []
00191         while minl >= 12:
00192             scales.append(m * pow(self.factor, factor_count))
00193             minl *= self.factor
00194             factor_count += 1
00195 
00196         
00197         for scale in scales:
00198             hs = int(np.ceil(h * scale))
00199             ws = int(np.ceil(w * scale))
00200 
00201             
00202             im_data = cv2.resize(input_image, (ws, hs))
00203             im_data = 2 * im_data / 255 - 1
00204 
00205             im_data = np.swapaxes(im_data, 0, 2)
00206             im_data = np.array([im_data], dtype=np.float)
00207             self.p_net.blobs['data'].reshape(1, 3, ws, hs)
00208             self.p_net.blobs['data'].data[...] = im_data
00209             out = self.p_net.forward()
00210 
00211             boxes = generate_bounding_box(out['prob1'][0, 1, :, :], out['conv4-2'][0], scale, self.threshold[0])
00212             if boxes.shape[0] != 0:
00213                 pick = nms(boxes, 0.5, 'Union')
00214 
00215                 if len(pick) > 0:
00216                     boxes = boxes[pick, :]
00217 
00218             if boxes.shape[0] != 0:
00219                 total_boxes = np.concatenate((total_boxes, boxes), axis=0)
00220 
00221         numbox = total_boxes.shape[0]
00222         if numbox > 0:
00223             
00224             pick = nms(total_boxes, 0.7, 'Union')
00225             total_boxes = total_boxes[pick, :]
00226 
00227             
00228             regh = total_boxes[:, 3] - total_boxes[:, 1]
00229             regw = total_boxes[:, 2] - total_boxes[:, 0]
00230             t1 = total_boxes[:, 0] + total_boxes[:, 5] * regw
00231             t2 = total_boxes[:, 1] + total_boxes[:, 6] * regh
00232             t3 = total_boxes[:, 2] + total_boxes[:, 7] * regw
00233             t4 = total_boxes[:, 3] + total_boxes[:, 8] * regh
00234             t5 = total_boxes[:, 4]
00235             total_boxes = np.array([t1, t2, t3, t4, t5]).T
00236 
00237             
00238             total_boxes = rerec(total_boxes)
00239 
00240             total_boxes[:, 0:4] = np.fix(total_boxes[:, 0:4])
00241             [dy, edy, dx, edx, y, ey, x, ex, tmpw, tmph] = pad(total_boxes, w, h)
00242 
00243         numbox = total_boxes.shape[0]
00244         if numbox > 0:
00245             
00246             
00247             
00248             tempimg = np.zeros((numbox, 24, 24, 3))
00249             for k in range(numbox):
00250                 tmp = np.zeros((int(tmph[k]), int(tmpw[k]), 3))
00251 
00252                 tmp[int(dy[k]):int(edy[k]) + 1, int(dx[k]):int(edx[k]) + 1] = input_image[int(y[k]):int(ey[k]) + 1, int(x[k]):int(ex[k]) + 1]
00253 
00254                 tempimg[k, :, :, :] = cv2.resize(tmp, (24, 24))
00255 
00256             tempimg = 2 * tempimg / 255 - 1
00257 
00258             
00259             tempimg = np.swapaxes(tempimg, 1, 3)
00260 
00261             self.r_net.blobs['data'].reshape(numbox, 3, 24, 24)
00262             self.r_net.blobs['data'].data[...] = tempimg
00263             out = self.r_net.forward()
00264 
00265             score = out['prob1'][:, 1]
00266             pass_t = np.where(score > self.threshold[1])[0]
00267 
00268             score = np.array([score[pass_t]]).T
00269             total_boxes = np.concatenate((total_boxes[pass_t, 0:4], score), axis=1)
00270 
00271             mv = out['conv5-2'][pass_t, :].T
00272             if total_boxes.shape[0] > 0:
00273                 pick = nms(total_boxes, 0.7, 'Union')
00274                 if len(pick) > 0:
00275                     total_boxes = total_boxes[pick, :]
00276                     total_boxes = bbreg(total_boxes, mv[:, pick])
00277                     total_boxes = rerec(total_boxes)
00278 
00279             numbox = total_boxes.shape[0]
00280             if numbox > 0:
00281                 
00282 
00283                 total_boxes = np.fix(total_boxes)
00284                 [dy, edy, dx, edx, y, ey, x, ex, tmpw, tmph] = pad(total_boxes, w, h)
00285 
00286                 tempimg = np.zeros((numbox, 48, 48, 3))
00287                 for k in range(numbox):
00288                     tmp = np.zeros((int(tmph[k]), int(tmpw[k]), 3))
00289                     tmp[int(dy[k]):int(edy[k]) + 1, int(dx[k]):int(edx[k]) + 1] = input_image[int(y[k]):int(ey[k]) + 1, int(x[k]):int(ex[k]) + 1]
00290                     tempimg[k, :, :, :] = cv2.resize(tmp, (48, 48))
00291                 tempimg = 2 * tempimg / 255 - 1
00292 
00293                 
00294                 tempimg = np.swapaxes(tempimg, 1, 3)
00295                 self.o_net.blobs['data'].reshape(numbox, 3, 48, 48)
00296                 self.o_net.blobs['data'].data[...] = tempimg
00297                 out = self.o_net.forward()
00298 
00299                 score = out['prob1'][:, 1]
00300                 points = out['conv6-3']
00301                 pass_t = np.where(score > self.threshold[2])[0]
00302                 points = points[pass_t, :]
00303                 score = np.array([score[pass_t]]).T
00304                 total_boxes = np.concatenate((total_boxes[pass_t, 0:4], score), axis=1)
00305 
00306                 mv = out['conv6-2'][pass_t, :].T
00307                 w = total_boxes[:, 3] - total_boxes[:, 1] + 1
00308                 h = total_boxes[:, 2] - total_boxes[:, 0] + 1
00309 
00310                 points[:, 0:5] = np.tile(w, (5, 1)).T * points[:, 0:5] + np.tile(total_boxes[:, 0], (5, 1)).T - 1
00311                 points[:, 5:10] = np.tile(h, (5, 1)).T * points[:, 5:10] + np.tile(total_boxes[:, 1], (5, 1)).T - 1
00312 
00313                 if total_boxes.shape[0] > 0:
00314                     total_boxes = bbreg(total_boxes, mv[:, :])
00315                     pick = nms(total_boxes, 0.7, 'Min')
00316 
00317                     if len(pick) > 0:
00318                         total_boxes = total_boxes[pick, :]
00319                         points = points[pick, :]
00320         
00321 
00322         return total_boxes, points