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;
}
