bool Knowledge::IsInsideGoalShape(Vector2D pos, double goalLeftX, double goalRadius, double goalCcOffset)
{
double x = pos.x - goalLeftX;
Vector2D ccl(goalLeftX, goalCcOffset / 2), ccr(goalLeftX, -goalCcOffset / 2);
return (pos.dist(ccl) <= goalRadius || pos.dist(ccr) <= goalRadius ||
(x >= 0 && x <= goalRadius && fabs(pos.y) <= goalCcOffset / 2));
}
void Detector_Seg::PreprocessROIs(Matrix<int> &labeled_ROIs, Vector<ROI_SEG> &all_ROIs)
{
KConnectedComponentLabeler ccl(Globals::region_size_threshold, labeled_ROIs.data(), labeled_ROIs.x_size(), labeled_ROIs.y_size());
ccl.Process(); // Main function
ROI_SEG::ExtractROIs(all_ROIs, labeled_ROIs, ccl.m_ObjectNumber);
}
Vector<Vector<double> > Detector::EvaluateTemplate(const Matrix<double> &upper_body_template,
const Matrix<double> &depth_map,
Vector<Vector<double> > &close_range_BBoxes,
Vector<Vector<double> > distances)
{
int stride = Globals::evaluation_stride;
int nr_scales = Globals::evaluation_nr_scales;
int inc_cropped_height = Globals::evaluation_inc_cropped_height;
// performance helper variables: just for avoiding recalculation
int int_half_template_size = Globals::template_size / 2;
double double_half_template_size = Globals::template_size / 2.0;
Vector<Vector<double> > final_result;
// generate the scales
Vector<double> all_scales(nr_scales, 1.0);
all_scales(0) = 1;
for(int sc = 1; sc < nr_scales; ++sc)
{
all_scales(sc) = pow(Globals::evaluation_scale_stride,sc);
}
// Matrix<double> roi_img(Globals::dImWidth, Globals::dImHeight, 0);
if(visualize_roi)
roi_image = Matrix<int>(Globals::dImWidth, Globals::dImHeight, 0);
for (int i = 0; i < close_range_BBoxes.getSize(); i++)
{
Vector<Vector<double> > result;
int cropped_height = (int)(close_range_BBoxes(i)(3)/2.0);
cropped_height += (close_range_BBoxes(i)(3) * Globals::evaluation_inc_height_ratio)/2.0;
close_range_BBoxes(i)(1) -= (close_range_BBoxes(i)(3) * Globals::evaluation_inc_height_ratio)/2.0;
if( close_range_BBoxes(i)(1)+cropped_height >= Globals::dImHeight)
cropped_height = Globals::dImHeight - (int)close_range_BBoxes(i)(1) - 1;
ROS_DEBUG("(distances(i) %f radius %f", distances(i)(0), distances(i)(1)/2.0);
// if(Globals::verbose)
// cout << "(distances(i) " << distances(i)(0) << " radius " << distances(i)(1)/2.0 << endl;
// Cropped and Filter depth_map with respect to distance from camera
int start_column = (int)close_range_BBoxes(i)(0);
int end_column = (int)(close_range_BBoxes(i)(0) + close_range_BBoxes(i)(2));
int start_row = (int)max(0.0, close_range_BBoxes(i)(1));
int end_row = (int)close_range_BBoxes(i)(1) + cropped_height;
Matrix<double> cropped(end_column-start_column+1, end_row-start_row+1);
double min_distance_threshold = distances(i)(0)- (distances(i)(1)+0.2)/2.0;
double max_distance_threshold = distances(i)(0)+ (distances(i)(1)+0.2)/2.0;
for(int ii = 0, ii_depth = start_column; ii < cropped.x_size(); ii++, ii_depth++)
{
for(int jj = 0, jj_depth = start_row; jj < cropped.y_size(); jj++, jj_depth++)
{
if(depth_map(ii_depth,jj_depth) <= min_distance_threshold || depth_map(ii_depth,jj_depth) >= max_distance_threshold)
{
cropped(ii, jj) = 0;
}
else
{
cropped(ii, jj) = depth_map(ii_depth, jj_depth);
}
}
}
// roi_img.insert(cropped,start_column,start_row);
////////////////
if(visualize_roi)
{
for(int tmpx=start_column, tmpxx=0; tmpxx<cropped.x_size(); tmpx++,tmpxx++)
{
for(int tmpy=start_row, tmpyy=0; tmpyy<cropped.y_size(); tmpy++,tmpyy++)
{
if(tmpyy==0 || tmpyy==cropped.y_size()-1 || tmpxx==0 || tmpxx==cropped.x_size()-1)
roi_image(tmpx,tmpy)=i+1;
if(cropped(tmpxx,tmpyy)!=0)
roi_image(tmpx,tmpy)=i+1;
}
}
}
/////////////////////////////////
// Resize Cropped - with respect to template
double ratio = close_range_BBoxes(i)(3) / (Globals::template_size * 3.0);
int new_height = (int)(cropped.y_size() * all_scales(nr_scales-1) / ratio);
int new_width = (int)(cropped.x_size() * all_scales(nr_scales-1) / ratio);
if(new_height<=0 || new_width<=0)
continue;
if(cropped.y_size() > new_height)
{
cropped.DownSample(new_width, new_height);
}
else if(cropped.y_size() < new_height)
{
cropped.UpSample(new_width, new_height);
}
//*******************************************************************************************************
Matrix<int> b_cropped(cropped.x_size(), cropped.y_size());
for(int ic = 0; ic<cropped.x_size(); ++ic)
{
for(int j=0; j<cropped.y_size(); ++j)
{
if(cropped(ic,j)>0)
b_cropped(ic,j)=1;
else
b_cropped(ic,j)=0;
}
}
KConnectedComponentLabeler ccl(Globals::region_size_threshold, b_cropped.data(), b_cropped.x_size(), b_cropped.y_size());
ccl.Process();
Matrix<double> components(cropped.x_size(),cropped.y_size());
//*******************************************************************************************************
for(int cr_com = 0; cr_com < ccl.m_ObjectNumber; ++cr_com)
{
for(int x=0; x<cropped.x_size(); ++x)
{
for(int y=0; y<cropped.y_size(); ++y)
{
if(b_cropped(x,y)==cr_com+1)
components(x,y) = cropped(x,y);
else
components(x,y) = 0;
}
}
Matrix<double> copy_component = components;
for(int scale_index = 0; scale_index < all_scales.getSize(); scale_index++)
{
copy_component = components;
// Resize Cropped in loop with different scales
int xSizeCropped = (int)(copy_component.x_size() / all_scales(scale_index));
int ySizeCropped = (int)(copy_component.y_size() / all_scales(scale_index));
if(all_scales(scale_index) != 1)
copy_component.DownSample(xSizeCropped , ySizeCropped);
Matrix<double> extended_cropped(xSizeCropped + Globals::template_size, ySizeCropped+inc_cropped_height, 0.0);
extended_cropped.insert(copy_component, (int)(double_half_template_size)-1, 0);
//*Local Max *******************************
Matrix<double> extended_cropped_open = extended_cropped;
AncillaryMethods::MorphologyOpen(extended_cropped_open);
Vector<double> ys ,slopes;
AncillaryMethods::ExtractSlopsOnBorder(extended_cropped_open, ys, slopes);
Vector<int> max_xs = AncillaryMethods::FindLocalMax(slopes);
//******************************************
int distances_matrix_x_size = max_xs.getSize();
Vector<double> resulted_distances(distances_matrix_x_size);
Vector<double> resulted_medians(distances_matrix_x_size);
Vector<int> rxs(max_xs.getSize()),rys(max_xs.getSize());
for(int ii = 0; ii<max_xs.getSize(); ++ii)
{
int cx = max(max_xs(ii)-int_half_template_size,0);
int cy = max(extended_cropped.y_size()-(int)ys(max_xs(ii))-4, 0);
rxs(ii) = cx;
rys(ii) = cy;
double local_result, local_best=1000;
for(int y=cy; y<=cy+1*stride; y+=stride)
{
if(y>=extended_cropped.y_size() || y>=extended_cropped.y_size()) continue;
int y_size = min(extended_cropped.y_size()-1, y+Globals::template_size) - y;
int start_row = (int)max(0.0, y + y_size/2.0-5);
int end_row = (int)min((double)Globals::dImHeight-1, y + y_size/2.0+5);
if(end_row >=extended_cropped.y_size()) continue;
for(int x=max(0,cx-5*stride); x<=cx+5*stride; x+=stride)
{
if(x>=extended_cropped.x_size()) continue;
int x_size = min(extended_cropped.x_size()-1, x+Globals::template_size) - x;
int start_column = (int)max(0.0, x + x_size/2.0-5);
int end_column = (int)min((double)Globals::dImWidth-1, x + x_size/2.0+5);
// Normalize the cropped part of the image. regarding the current position of the template;
// Crop only some pixels in the middle
double median = AncillaryMethods::MedianOfMatrixRejectZero(extended_cropped, start_row, end_row, start_column, end_column);
if(median == 0)
{
resulted_distances(ii) = 1000;
continue;
}
// extended_cropped *= 1.0/median;
int x_start_of_temp = max(0, int_half_template_size - x);
double x_end_of_temp = Globals::template_size;
int evaluating_area = (x_end_of_temp - x_start_of_temp)*Globals::template_size+1;
double sum = 0;
if(evaluating_area > Globals::template_size * double_half_template_size)
{
for(int x_of_temp = 0; x_of_temp < x_end_of_temp; x_of_temp++)
{
int x_of_extended_cropp = x + x_of_temp;
for(int y_of_temp = 0; y_of_temp < Globals::template_size; y_of_temp++)
{
double difference = upper_body_template(x_of_temp, y_of_temp)-extended_cropped(x_of_extended_cropp, y_of_temp+y)/median;
sum += difference*difference;
}
}
local_result = sum/(double)evaluating_area;
if(local_best>local_result)
{
local_best = local_result;
resulted_medians(ii) = median;
rxs(ii)=x;
rys(ii)=y;
}
}
}
}
resulted_distances(ii) = local_best;
}
int n_xSizeTemp = (int)(Globals::template_size*ratio/all_scales(scale_index));
// double n_threshold = 0.15;
for(int ii =0; ii<resulted_distances.getSize(); ++ii)
{
if(resulted_distances(ii)<Globals::evaluation_NMS_threshold_LM)
{
int x = rxs(ii);
int y = rys(ii);
Vector<double> bbox(6);
bbox(0) = (x-double_half_template_size)*ratio/all_scales(scale_index) + close_range_BBoxes(i)(0);
bbox(1) = y*ratio/all_scales(scale_index) +close_range_BBoxes(i)(1);
bbox(2) = n_xSizeTemp;
bbox(3) = n_xSizeTemp;
bbox(4) = resulted_distances(ii);
bbox(5) = resulted_medians(ii);
result.pushBack(bbox);
}
}
}
}
AncillaryMethods::GreedyNonMaxSuppression(result, Globals::evaluation_greedy_NMS_overlap_threshold, Globals::evaluation_greedy_NMS_threshold, upper_body_template, final_result);
}
// roi_img.WriteToTXT("roi_img.txt");
return final_result;
}
/**
* Init GUI and load existing quick config
*/
quickconf::quickconf()
{
int x;
for (x=0; x<8; x++)
{
botfiles[x] = "";
shownames[x] = new QLabel (this);
shownames[x]->setGeometry (10,50+x*25,190,20);
shownames[x]->show();
team[x] = new QComboBox (this);
team[x]->setGeometry (215,50+x*25,40,20);
team[x]->insertItem ("1");
team[x]->insertItem ("2");
team[x]->insertItem ("3");
team[x]->insertItem ("4");
}
// press[0] = new PixButton( "load",1,this );
// press[0]->setGeometry( 0,0,80,40 );
// press[1] = new PixButton( "remove",1,this );
// press[1]->setGeometry( 85,0,80,40 );
ifteams = new QCheckBox ("Teams",this);
ifteams->setGeometry (170,10,70,20);
QObject::connect (press[0],SIGNAL (clicked()),this,
SLOT (choosefile()));
QObject::connect (press[1],SIGNAL (clicked()),this,
SLOT (dechoosefile()));
readyb = new PixButton ("write file",1,this);
readyb->setGeometry (100,500,80,40);
cancelb = new PixButton ("Exit",1,this);
cancelb->setGeometry (200,500,80,40);
QObject::connect (readyb,SIGNAL (clicked()),this,SLOT (ocl()));
QObject::connect (cancelb,SIGNAL (clicked()),this,SLOT (ccl()));
tnumfights = new QLabel ("Number of fights:",this);
tnumfights->setGeometry (10,300,100,20);
wnumfights = new QLineEdit (this);
wnumfights->setGeometry (120,300,40,20);
numfix = new QIntValidator (this);
wnumfights->setValidator (numfix);
lengthfight = new QLabel ("Max length of fight ( 50 ~ 1sec ):",this);
lengthfight->setGeometry (10,320,200,20);
length = new QLineEdit (this);
length->setGeometry (200,320,60,20);
maxxinfo = new QLabel ("The xsize of the battlearea: ",this);
maxxinfo->setGeometry (10,350,200,20);
maxx = new QSpinBox (8192,65535,512,this);
maxx->setGeometry (210,350,80,30);
maxx->setValue (32768);
maxyinfo = new QLabel ("The ysize of the battlearea: ",this);
maxyinfo->setGeometry (10,380,200,20);
maxy = new QSpinBox (8192,65535,512,this);
maxy->setGeometry (210,380,80,30);
maxy->setValue (32768);
QString temp = QDir::homeDirPath();
temp += "/droidbattles/quick.conf";
QFile f (temp);
if (f.exists() && f.open (QIODevice::ReadOnly))
{
Q3TextStream s (&f);
for (int x=0; x<8; x++)
{
s >> botfiles[x];
s >> temp;
if (botfiles[x] == QString ("fff"))
botfiles[x] = "";
team[x]->setCurrentItem (temp.toInt());
shownames[x]->setText (botfiles[x]);
}
s >> temp;
ifteams->setChecked (temp.toInt());
s >> temp;
wnumfights->setText (temp);
s >> temp;
length->setText (temp);
s >> temp;
maxx->setValue (temp.toInt());
s >> temp;
maxy->setValue (temp.toInt());
f.close();
}
