int findClosest(double* p1, double* centers, long dim, long k){
long closest = 0, i;
double minDist=0, testDist=0;
minDist = euclidDist(p1, centers, dim);
for(i=1; i<k; ++i){
testDist = euclidDist(p1, centers+(i*dim), dim);
if( minDist > testDist){
closest = i;
minDist = testDist;
}
}
return closest;
}
int centerDiff(double* centers1, double* centers2, double tol, long k, long dim){
long i;
double accum=0;
for(i=0; i<k; ++i){
accum += euclidDist(centers1+(i*dim), centers2+(i*dim), dim);
}
if(accum > tol)
return 1;
else
return 0;
}
void operator()(const Range& range) const{ //重载操作符（）
int x, y;
for( y = range.start; y < range.end; y++ )
{
const uchar* p = src->datastart + cols*y*nchannels;
uchar* m = mask->datastart + cols*y;
for( x = 0; x < cols; x++, p += nchannels){
int count = 0,index = 0,dist = 0;
for(int index = 0;index<defaultNbSamples && count < defaultReqMatches;index++) {
dist = euclidDist(p[0],p[1],p[2],Samples[x][y][index][0],Samples[x][y][index][1],Samples[x][y][index][2]);
if(dist <defaultRadius)
count++;
}
if(count>=defaultReqMatches){
m[x]=BACKGROUNG;
int rand = GetRandom(0,defaultSubsamplingFactor - 1);
if(rand == 0){
rand = GetRandom(0,defaultNbSamples - 1);
Samples[x][y][rand][0] = p[0];
Samples[x][y][rand][1] = p[1];
Samples[x][y][rand][2] = p[2];
}
rand = GetRandom(0,defaultSubsamplingFactor - 1);
if(rand ==0){
int dx = GetRandom(-NEIGHBOR,NEIGHBOR);
int dy = GetRandom(-NEIGHBOR,NEIGHBOR);
int neighborX = x + dx;
neighborX = (0<=neighborX && neighborX<cols)? neighborX :x;
int neighborY = y + dy;
neighborY = (0<=neighborY && neighborY<rows)? neighborY :y;
int randSample = GetRandom(0,defaultNbSamples - 1);
Samples[neighborX][neighborY][randSample][0] = p[0];
Samples[neighborX][neighborY][randSample][1] = p[1];
Samples[neighborX][neighborY][randSample][2] = p[2];
}
}
else{
m[x]=FOREGROUNG;
}
//cout<<endl;
}
}
}
vector<coord*> algs::dijkstra(coord &source, map<coord, vector<coord> > &visibility_graph, coord &target){
map<coord, coord*> visibility_reference;
map<coord, vector<coord> >::iterator iter_g;
for(iter_g = visibility_graph.begin(); iter_g != visibility_graph.end(); ++iter_g){
visibility_reference[iter_g->first] = new coord(iter_g->first.x, iter_g->first.y);
}
visibility_reference[source] = &source;
visibility_reference[target] = ⌖
source.dist = 0;
vector<coord*> minHeap;
minHeap.push_back(visibility_reference[source]);
coord* vert;
while (minHeap.size() > 0) {
vert = minHeap.back();
minHeap.pop_back();
if(*vert == target){
break;
}
vert->known = true;
vector<coord> neighbors = visibility_graph[*vert];
for(int i = 0; i < (int) neighbors.size(); ++i){
coord *neighbor = visibility_reference[ neighbors[i] ];
double accumulate = vert->dist + euclidDist(*vert, *neighbor);
if( !neighbor->known && accumulate < neighbor->dist ){
neighbor->dist = accumulate;
neighbor->previous = vert;
minHeap.push_back(neighbor);
std::sort (minHeap.begin(), minHeap.end(), coordDistComp);
}
}
}
vector<coord*> path;
while( vert != NULL){
cout << " -curr- " << *vert << endl;
path.push_back(vert);
vert = vert->previous;
}
std::reverse(path.begin(), path.end());
return path;
}
double addRoundTrip(struct Arr2D* tour)
{
int i,x1,x2,y1,y2;
for (i = 0; i < tour->size[CITY];i++)
{
if (tour->array[ORDER][i] == 0)
{
x1 = tour->array[X][i];
y1 = tour->array[Y][i];
}
if (tour->array[ORDER][i] == tour->size[CITY] - 1 )
{
x2 = tour->array[X][i];
y2 = tour->array[Y][i];
}
}
return euclidDist(x1,x2,y1,y2);
}
double nearestNeighbor(struct Arr2D* tour,int seed)
{
int x1,x2,y1,y2,i;
int a = 0;
int temp;
int order_index = 0;
double distAB = 0.0f;
double minDistAB;
double totalDist = 0;
clearOrder(tour);
a = seed;
tour->array[ORDER][a] = order_index;
order_index++;
while(visitedAll(tour) == -1)
{
distAB = BIG_NUM;
minDistAB = BIG_NUM;
for (i = 0; i< tour->size[CITY]; i++)
{
if(tour->array[ORDER][i] == -1)
{
x1 = tour->array[X][a];
x2 = tour->array[X][i];
y1 = tour->array[Y][a];
y2 = tour->array[Y][i];
distAB = euclidDist(x1,x2,y1,y2);
if(distAB < minDistAB)
{
minDistAB = distAB;
temp = i;
}
}
else{
}
}
totalDist += minDistAB;
tour->array[ORDER][temp] = order_index;
a = temp;
temp = -1;
order_index++;
}
//add an edge from last visited to start point
totalDist+= addRoundTrip(tour);
return totalDist;
}
int RepNearestNeighbor(struct Arr2D* tour)
{
int x1,x2,y1,y2,i,j;
int seed;
int a = 0;
int temp;
int order_index = 0;
double distAB = BIG_NUM;
double minDistAB = BIG_NUM;
double totalDist = BIG_NUM;
double minTotalDist = BIG_NUM;
for(j = 0; j < tour->size[CITY];j++)
{
a = j;
order_index = 0;
totalDist = 0;
clearOrder(tour);
tour->array[ORDER][a] = order_index;
order_index++;
while(visitedAll(tour) == -1)
{
distAB = BIG_NUM;
minDistAB = BIG_NUM;
for (i = 0; i< tour->size[CITY]; i++)
{
if(tour->array[ORDER][i] == -1 && i != a)
{
x1 = tour->array[X][a];
x2 = tour->array[X][i];
y1 = tour->array[Y][a];
y2 = tour->array[Y][i];
distAB = euclidDist(x1,x2,y1,y2);
if(distAB < minDistAB)
{
minDistAB = distAB;
temp = i;
}
}
else{
}
}
totalDist += minDistAB;
tour->array[ORDER][temp] = order_index;
a = temp;
order_index++;
}
//add an edge from last visited to start point
totalDist+=addRoundTrip(tour);
if( totalDist < minTotalDist)
{
minTotalDist = totalDist;
seed = j;
}
}
return seed;
}
unsigned int FaceDetector::detectFaceCamShift(cv::Mat img)
{
/* Variables for CAM Shift */
cv::Mat hsv = cv::Mat(img.size(), CV_8UC3 );
cv::Mat mask = cv::Mat(img.size(), CV_8UC1);
cv::Mat grayscale;
cv::Mat backproject = cv::Mat( img.size(), CV_8UC1 );
cv::Mat histimg = cv::Mat::zeros( img.size(), CV_8UC3);
cv::Mat hue[hsv.channels()];
cv::MatND hist;
int vmin = 10, vmax = 256, smin = 30;
int n = 16;
float hranges_arr[] = {0,180};
const float* hranges = hranges_arr;
int channels[]={0};
cv::Rect track_window, face_roi;
cv::RotatedRect track_box;
int mean_score_threshold = 70; //Minimum value of mean score of the CAM Shift. Value between 0 and 100
float max_face_deslocation = detected_face_roi_.height/3.;
unsigned int max_distance_width = detected_face_roi_.width/3.;
unsigned int max_distance_height = detected_face_roi_.height/3.;
/*********************/
cv::cvtColor( img, hsv, CV_RGB2HSV );
cv::cvtColor(img, grayscale, CV_RGB2GRAY);
cv::inRange(hsv, cv::Scalar(0,smin,MIN(vmin,vmax),0),cv::Scalar(180,256,MAX(vmin,vmax),0), mask );
split( hsv,hue);
face_roi = detected_face_roi_;
if(!track_object_) //Select a region with the face color skin and calc histogram
{
double max_val = 0.0;
//ROI selection is a bit different now and uses operator ()
/*
* little_face is a ROI that contains exclusively the color skin
*/
cv::Rect little_face;
little_face = face_roi;
little_face.width = face_roi.width -face_roi.width/3;
little_face.height = face_roi.height -face_roi.height/3;
little_face.x += face_roi.width/(2.*3.);
little_face.y += face_roi.height/(2.*3.);
cv::Mat hueRoi=hue[0](little_face);
cv::Mat maskRoi=mask(little_face);
cv::calcHist( &hueRoi,1,channels, maskRoi , hist, 1 , &n , &hranges, true, 0 );
cv::minMaxLoc(hist, 0, &max_val, 0, 0);
float scale =max_val ? 255. / max_val : 0.;
hist.convertTo(hist,hist.type(),scale,0);
track_window = face_roi;
histimg.setTo(0);
}
cv::calcBackProject(hue,1,channels,hist,backproject,&hranges,1,true);
cv::bitwise_and( backproject, mask, backproject);
//Use camshift over computed histogram
track_box = cv::CamShift( backproject, track_window, cv::TermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ));
if(track_box.size.height>0 && track_box.size.width > 0
&& track_box.center.x>0 && track_box.center.y >0)
face_roi = track_box.boundingRect();
face_roi.height = face_ratio_*face_roi.width;
face_roi.x = min(face_roi.x, img.cols);
face_roi.y = min(face_roi.y, img.rows);
face_roi.width = min(img.cols-face_roi.x-3, face_roi.width);
face_roi.height = min(img.rows-face_roi.y-3, face_roi.height);
face_roi.height = max(face_roi.height, 0);
face_roi.width = max(face_roi.width, 0);
face_roi.x = max(face_roi.x, 0);
face_roi.y = max(face_roi.y, 0);
if(face_roi.height <= 0 || face_roi.width <= 0) //If face ROI has invalid dimensions
return 1;
/*
* This is used to check mean score of CAM shift selected ROI
*/
double mean_score = 0;
if(face_roi.x > 0 && face_roi.y > 0 && face_roi.width>20 && face_roi.height>20)
{
cv::Mat temp = backproject(face_roi);
for(int r = 0; r < temp.rows; r++)
for(int c = 0; c < temp.cols; c++)
mean_score+= temp.at<unsigned char>(r,c);
mean_score = mean_score/double(temp.rows*temp.cols);
}
if(mean_score<mean_score_threshold) //Let's see if CAM Shift respects mean score threshold
{
face_detected_bool_ = false;
return 1;
}
//If face position have moved considerably, ignore CAM shift
if(euclidDist(cv::Point2f(face_roi.x + face_roi.width/2.,face_roi.y+face_roi.height/2.),
cv::Point2f(detected_face_roi_.x+detected_face_roi_.width/2,detected_face_roi_.y+detected_face_roi_.height/2.)) > max_face_deslocation)
{
face_detected_bool_ = false;
return 1;
}
//This is to avoid big increases in the size of the detected_face_roi
if(abs(face_roi.width - detected_face_roi_.width)> max_distance_width
|| abs(face_roi.height - detected_face_roi_.height)> max_distance_height)
{
face_roi.x = face_roi.x + face_roi.width/2. -detected_face_roi_.width/2.;
face_roi.y = face_roi.y + face_roi.height/2. -detected_face_roi_.height/2.;
face_roi.width = detected_face_roi_.width;
face_roi.height = detected_face_roi_.height;
}
//Check if Face ROI respects image's dimensions
if(face_roi.x<0 || face_roi.y < 0
|| (face_roi.x+face_roi.width)>= img.cols || (face_roi.y+face_roi.height)>= img.rows)
return 1;
//CAM Shift have succesfully found face
detected_face_roi_ = face_roi;
detected_face_ = img(detected_face_roi_);
face_detected_bool_ = true;
return 0;
}
