void SearchUme (Image &Img, Image *pLevMask, int iLev, CALLBACK_FUN callbackFunc, ClientData callbackData) { int x, y, i, j; if (pLevMask != NULL) { // For each 10x10 pixel block where the center of a face could be, // see if more than 5 pixel locations still need to be scanned. If // so, the block must be scanned. This is indicated by a flag placed // at the upper-left corner of each block. // In pLevMask: 0 means do scan, 255 means don't scan. int xp = (Img.width + 9) / 10; int yp = (Img.height + 9) / 10; for (y = 0; y < yp; y++) { for (x = 0; x < xp; x++) { int Total = 0, n = 0; for (j = y * 10; j < y * 10 + 10; j++) for (i = x * 10; i < x * 10 + 10; i++) if (i < pLevMask->width && j < pLevMask->height) { Total++; if (!(*pLevMask)(i, j)) n++; } if (n <= 5) (*pLevMask)(x * 10, y * 10) = 255; else (*pLevMask)(x * 10, y * 10) = 0; } } int TempImage[900]; // used to store the window 30x30 = 900 int Hist[256]; for (y = 0; y < Img.height; y += 10) // for each block { for (x = 0; x < Img.width; x += 10) { if (pLevMask != NULL && (*pLevMask)(x, y)) continue; memset(Hist, 0, sizeof(Hist)); // Copy the window from the image into TempImage. The first loop is used // when the window is entirely inside the image, the second one is // used otherwise. For pixels outside the image, the pixels at the // edge of the image are replicated. The histogram is updated. int *pTo = TempImage; if (x >= 10 && y >= 10 && x + 20 <= Img.width && y + 20 <= Img.height) { for (j = y - 10; j < y + 20; j++) for (i = x - 10; i < x + 20; i++) { int val = Img(i, j); Hist[val]++; (*(pTo++)) = val; } } else { for (j = y - 10; j < y + 20; j++) for (i = x - 10; i < x + 20; i++) { int ii = i; if (ii < 0) ii = 0; if (ii >= Img.width) ii = Img.width-1; int jj = j; if (jj<0) jj = 0; if (jj >= Img.height) jj = Img.height - 1; int val = Img(ii, jj); Hist[val]++; (*(pTo++)) = val; } } // build a cumulative histogram int CumHist[256]; pTo = CumHist; int *pFrom = Hist; int Total = 0; for (i = 0; i < 256; i++) { int old = Total; Total += *(pFrom++); *(pTo++) = old + Total; } // apply histogram equalization, write image into network input units const double Scale = 1.0 / Total; ForwardUnit *pUnit = &(gNetList[0]->pUnitList[1]); pFrom = TempImage; // 30x30 window for (i = 0; i < 900; i++) (pUnit++)->activation = (_FLOAT)(CumHist[*(pFrom++)] * Scale - 1.0); // Apply the network. // If there is a detection, call the callback function to record the detection. const double output = ForwardPass(gNetList[0]); if (output > 0) callbackFunc(callbackData, &Img, x-5, y-5, 20, 20, iLev, POW12(iLev), output); } } } }
bool fFindNewLocation (int *newX, int *newY, int *news, int numScales, Image ImagePyramid[], Image &FaceMask, int x, int y, int scale, int dx, int dy, int ds, int step, int iNet) { InitLightingCorrectionMat(FaceMask); double totX = 0; // used to record centroid of face detections double totY = 0; double totS = 0; double Total = 0; int iPixel; int hist[512]; // histogram int map[512]; // cumulative histogram Vec vec(3); // part of affine fitting int *tmp = new int[FaceMask.width * FaceMask.height]; // window int halfX = FaceMask.width/2; int halfY = FaceMask.height/2; int nFaceMaskSize = FaceMask.width * FaceMask.height; x += halfX; // Input location is upper left corner of y += halfY; // the "centered" face. This makes location be face center. ForwardStruct *net = gNetList[iNet]; ForwardStruct *net2 = gNetList[iNet+1]; for (int scale1 = scale-ds; scale1 <= scale + ds; scale1++) // for each scale { // milbo: removed comment that commented out two lines below (in original // Rowley code) because it caused a crash because scale1 was less than 0. if (scale1 < 0 || scale1 >= numScales) continue; // NOTE: continue int cx = (int)floor(x * POW12(scale - scale1) + 0.5); // map center position to scale int cy = (int)floor(y * POW12(scale - scale1) + 0.5); Image *pImg = &ImagePyramid[scale1]; for (int yy = cy - dy; yy <= cy + dy; yy += step) // scan over position { for (int xx = cx - dx; xx <= cx + dx; xx += step) { // if (yy!=cy && xx!=cx) continue; iPixel = 0; double v0 = 0, v1 = 0, v2 = 0; // The next two loops copy the window into the tmp variable, and // begin computing the affine fit (using only pixels inside the FaceMask) // The first version is for windows inside the image, the second // replicates the edge pixels of the image. if (xx - halfX >= 0 && yy - halfY >= 0 && xx + halfX <= pImg->width && yy + halfY <= pImg->height) { for (int iy = yy - halfY; iy < yy + halfY; iy++) for (int ix = xx - halfX; ix < xx + halfX; ix++) { int val = (*pImg)(ix,iy); tmp[iPixel] = val; if (FaceMask(iPixel++)) { v0 += (ix-xx) * val; v1 += (iy-yy) * val; v2 += val; } } } else { for (int iy = yy-halfY; iy<yy+halfY; iy++) for (int ix = xx-halfX; ix<xx+halfX; ix++) { int ii = ix; if (ii < 0) ii = 0; if (ii >= pImg->width) ii = pImg->width-1; int jj = iy; if (jj < 0) jj = 0; if (jj >= pImg->height) jj = pImg->height-1; int val = (*pImg)(ii,jj); tmp[iPixel] = val; if (FaceMask(iPixel++)) { v0 += (ix - xx) * val; v1 += (iy - yy) * val; v2 += val; } } } // actually compute the parameters of the affine fit vec(0) = v0; vec(1) = v1; vec(2) = v2; vec = gLightingCorrectionMat * vec; // using overloaded operators here v0 = vec(0); v1 = vec(1); v2 = vec(2); // apply the affine correction, and build the histogram based // on pixels in the FaceMask int i; for (i = 0; i < 512; i++) hist[i] = 0; iPixel = 0; for (int j = -FaceMask.height / 2; j < FaceMask.height / 2; j++) for (i = -FaceMask.width / 2; i < FaceMask.width / 2; i++) { int val = tmp[iPixel] - (int)(i * v0 + j * v1 + v2 - 256.5); if (val < 0) val = 0; if (val >= 512) val = 511; if (FaceMask(iPixel)) hist[val]++; tmp[iPixel++] = val; } // build the cummulative histogram int *to = map; int *from = hist; int total = 0; for (i = 0; i<512; i++) { int old = total; total += *(from++); *(to++) = old + total; } // apply histogram equalization and copy the window into the network inputs double scaleFactor = 1.0/total; ForwardUnit *pUnit = &(net->pUnitList[1]); int *p = tmp; for (i = 0; i<nFaceMaskSize; i++) (pUnit++)->activation = _FLOAT(map[*(p++)] * scaleFactor - 1.0); ForwardPass(net); // Check the two outputs (we applied one network, but really // it is two merged networks). If both responded postively, // then add the detection on to the centroid of detections. if (net->pUnitList[net->iFirstOutput].activation>0) { pUnit = &(net->pUnitList[1]); ForwardUnit *pUnit2 = &(net2->pUnitList[1]); for (i = 0; i < nFaceMaskSize; i++) (pUnit2++)->activation = (pUnit++)->activation; ForwardPass(net2); if (net2->pUnitList[net2->iFirstOutput].activation>0) { totS += scale1; totX += xx * POW12(scale1); totY += yy * POW12(scale1); Total++; } } } // for xx } // for yy } // for scale1 delete[] tmp; if (Total == 0) return false; // compute the centroid, first in scale and then in position at that scale int newS = iround(totS / Total); if (newS < 0) newS = 0; if (newS >= numScales) newS = numScales - 1; *newX = iround(totX / (Total * POW12(newS)) - halfX); *newY = iround(totY / (Total * POW12(newS)) - halfY); *news = newS; return true; }
void HDDP(options & probdata, options & nominal, int & go_to_step, int & current_iter, bool & converged){ double rho; cout << "Iteration " << current_iter << ": "; switch (go_to_step){ // Step 1: Computation of First and Second Order STMs case 1: cout << "Step 1: Computation of First and Second Order STMS \n"; ComputeSTMs(nominal); go_to_step = 2; break; // Step 2: Backward Sweep case 2: cout << "Step 2: Backward Sweep \n"; BackwardSweep(nominal); go_to_step = 3; break; // Step 3: Convergence Check case 3: cout << "Step 3: Convergence Check \n"; if ((abs(nominal.ER(0, 0)) < nominal.tol && (nominal.f < nominal.tol) && (nominal.delta <= nominal.delta_min + 1e-6)) || (nominal.delta <= nominal.delta_min + 1e-6)){ cout << "I think we're done here \n"; //cout << "Feasible Solution Within Tolerance \n"; converged = 1; break; } cout << nominal.ER(0, 0) << " " << nominal.J << " " << nominal.h << " " << nominal.f << " " << nominal.delta << "\n"; cout << probdata.ER(0, 0) << " " << probdata.J << " " << probdata.h << " " << probdata.f << " " << probdata.delta << "\n"; go_to_step = 4; break; // Step 4: Forward Pass case 4: current_iter++; probdata = nominal; cout << "Step 4: Forward Pass \n"; ForwardPass(probdata, nominal); go_to_step = 5; break; // Step 5: Trust Region Update case 5: cout << "Step 5: Trust Region Update \n"; eval_J(probdata); rho = (probdata.J - nominal.J) / probdata.ER(0, 0); if (current_iter < 0){ cout << "Accepted rho = " << rho << "\n"; go_to_step = 6; break; } //(.8 < rho && rho < 1.2){ else if (probdata.J < nominal.J){ cout << "Accepted rho = " << rho << "\n"; go_to_step = 6; break; } // second chance else { nominal.delta = max(nominal.delta*(1. - nominal.kappa), nominal.delta_min); cout << "Rejected rho = " << rho << "\n"; go_to_step = 2; break; } // Step 6: Nominal Solution Update case 6: cout << "Step 6: Nominal Solution Update \n"; rho = (probdata.J - nominal.J) / probdata.ER(0, 0); probdata.Multipliers += nominal.delta_l; /**/ // Update rule from Niu if (probdata.psi.norm() >= nominal.psi.norm()/1.2){ probdata.s = max(1.2*probdata.s, 1.2*probdata.Multipliers.norm()); } else { probdata.s = max(probdata.s, 1.2*probdata.Multipliers.norm()); } eval_J(probdata); nominal = probdata; if (.8 < rho && rho < 1.2){ nominal.delta = min(probdata.delta*pow((1. + probdata.kappa),2.0), probdata.delta_max); go_to_step = 1; // 2 to re-use STM } else if (rho > 0){ nominal.delta = min(probdata.delta*pow((1. + probdata.kappa), 1.0), probdata.delta_max); go_to_step = 1; } else { nominal.delta = max(nominal.delta*(1. - nominal.kappa), nominal.delta_min); go_to_step = 1; } WriteTrajectory(nominal, current_iter); break; } }
static void FindEyes ( double *pLex, // out: left eye position, or INVALID if can't find double *pLey, // out: double *pRex, // out: ditto for right eye double *pRey, // out: int xFace, int yFace, // in: position of top left corner of face box double DetWidth, // in: width of face detector box const Image Img, // in const Image &EyeMask, // in int iNet) // in: neural net index in gNetList { *pLex = *pRex = *pRex = *pRey = INVALID; // assume won't find eyes int EyeMaskSize = EyeMask.width * EyeMask.height; int EyeMaskWidth = EyeMask.width; int EyeMaskHeight = EyeMask.height; double xLeft = 0, yLeft = 0, xRight = 0, yRight = 0; double LeftWeight = 0, RightWeight = 0; int *pTmpImage = new int[EyeMaskSize]; // window for eye detector // possible upper-left X positions for the left eye int startxLeft = iround(xFace); int endxLeft = iround(xFace + DetWidth/2 - EyeMaskWidth); if (startxLeft < 0) startxLeft = 0; if (endxLeft > Img.width - EyeMaskWidth) endxLeft = Img.width - EyeMaskWidth; // possible upper-left X positions for the right eye int startxRight = iround(xFace + DetWidth/2); int endxRight = iround(xFace + DetWidth - EyeMaskWidth); if (startxRight < 0) startxRight = 0; if (endxRight > Img.width - EyeMaskWidth) endxRight = Img.width - EyeMaskWidth; // possible upper Y positions for the eyes int starty = iround(yFace); int endy = iround(yFace + DetWidth / 4); // exact value 4 is not critical if (starty < 0) starty = 0; if (endy >= Img.height - EyeMaskHeight) endy = Img.height - EyeMaskHeight; bool fDoneLeft = false; bool fDoneRight = false; // start at bottom of face and move up so eyes are before eyebrows, // thus avoiding false positives on the eyebrows for SKIP_WHEN_EYE_FOUND for (int y = endy-1; y >= starty; y--) { if (SKIP_WHEN_EYE_FOUND_DIST != 0) { // set these if already got the eye, to prevent false detects on eyebrows fDoneLeft = LeftWeight != 0 && y < yLeft / LeftWeight - SKIP_WHEN_EYE_FOUND_DIST; fDoneRight = RightWeight != 0 && y < yRight / RightWeight - SKIP_WHEN_EYE_FOUND_DIST; if (fDoneLeft && fDoneRight) break; } // Look for right eye on this scan line. We mirror it so it looks to the // net like a right eye because we trained the net on a left eye. int i, j, x; if (!fDoneRight) for (x = startxRight; x < endxRight; x++) { int iPixel = 0; int Hist[256]; memset(Hist, 0, sizeof(Hist)); // copy the window into pTmpImage (using mirror image), and compute // the histogram over the entire window for (j = 0; j < EyeMaskHeight; j++) for (i = EyeMaskWidth - 1; i >= 0; i--) // note: mirror here { int Pixel = Img(i+x, j+y); pTmpImage[iPixel++] = Pixel; Hist[Pixel]++; } // compute cumulative histogram int CumHist[256]; int Sum = 0; for (i = 0; i < 256; i++) { CumHist[i] = Sum; Sum += Hist[i]; } int Total = Sum; for (i = 255; i >= 0; i--) { CumHist[i] += Sum; Sum -= Hist[i]; } // apply the histogram equalization, and write window to network inputs const double Scale = 1.0 / Total; ForwardUnit *pUnit = &(gNetList[iNet]->pUnitList[1]); for (i = 0; i < EyeMaskSize; i++) (pUnit++)->activation = (_FLOAT)(CumHist[pTmpImage[i]] * Scale - 1.0); // if the network responds positively, add the detection to centroid const double Output = ForwardPass(gNetList[iNet]); if (Output > 0) { RightWeight += Output; xRight += Output * (x + EyeMaskWidth / 2); yRight += Output * (y + EyeMaskHeight / 2); } } // look for left eye on this scan line if (!fDoneLeft) for (x = startxLeft; x < endxLeft; x++) { int iPixel = 0; int Hist[256]; memset(Hist, 0, sizeof(Hist)); // copy the window into pTmpImage, and compute // the histogram over the entire window for (j = 0; j < EyeMaskHeight; j++) for (i = 0; i < EyeMaskWidth; i++) { int Pixel = Img(i+x, j+y); pTmpImage[iPixel++] = Pixel; Hist[Pixel]++; } // compute cumulative histogram int CumHist[256]; int Sum = 0; for (i = 0; i < 256; i++) { CumHist[i] = Sum; Sum += Hist[i]; } int Total = Sum; for (i = 255; i >= 0; i--) { CumHist[i] += Sum; Sum -= Hist[i]; } // apply the histogram equalization, and write window to network inputs const double Scale = 1.0 / Total; ForwardUnit *pUnit = &(gNetList[iNet]->pUnitList[1]); for (i = 0; i < EyeMaskSize; i++) (pUnit++)->activation = (_FLOAT)(CumHist[pTmpImage[i]] * Scale - 1.0); // if the network responds positively, add the detection to centroid const double Output = ForwardPass(gNetList[iNet]); if (Output > 0) { LeftWeight += Output; xLeft += Output * (x + EyeMaskWidth / 2); yLeft += Output * (y + EyeMaskHeight / 2); } } } // if the left eye was detected at least once, return centroid if (LeftWeight > 0) { *pLex = xLeft / LeftWeight; *pLey = yLeft / LeftWeight; } // if the right eye was detected at least once, return centroid if (RightWeight > 0) { *pRex = xRight / RightWeight; *pRey = yRight / RightWeight; } delete[] pTmpImage; }
int main(){ // Initialize options probdata = {}; Initialization(probdata); read_input_file("HDDP_input.txt", &probdata); options nominal = probdata; ForwardPass(probdata, nominal); eval_J(probdata); nominal = probdata; // Enter the HDDP Loop int current_iter = 0; int go_to_step = 1; bool converged = 0; WriteTrajectory(nominal, current_iter); // construct a trivial random generator engine from a time-based seed: unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); std::default_random_engine generator(seed); std::uniform_real_distribution<double> distribution(0.0, 1.0); while (!converged && probdata.delta > probdata.delta_min) HDDP(probdata, nominal, go_to_step, current_iter, converged); /* // MBH int max_MBH_iterations = 1000; double alpha = 1.5; for (int i = 0; i < max_MBH_iterations; i++){ WriteTrajectory(nominal); options probdata = {}; Initialization(probdata); options perturbed = probdata; for (int stage = 0; stage < probdata.nstages; stage++){ for (int k = 8; k < 11; k++){ double s = distribution(generator); double r = ((alpha - 1.0) / SMALL) / pow((SMALL / (SMALL + distribution(generator))), -alpha); perturbed.traj[stage][k] = nominal.traj[stage][k] + s*r; //cout << s << " " << r << " " << s*r << "\n"; } } ForwardPass(probdata, perturbed); probdata.Multipliers = nominal.Multipliers; eval_J(probdata); perturbed = probdata; go_to_step = 1; converged = 0; while (!converged) HDDP(probdata, perturbed, go_to_step, current_iter, converged); if (perturbed.J < nominal.J){ nominal = perturbed; } } */ // Save the Trajectory probdata.Multipliers.print_to_screen(); return 0; }