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