void display_pixel_set(int row, int col,int val) { // invert the pixel at the given row and column
if(val) {
gddram[pixel_pos(row,col)] |= pixel_mask(row); // set the pixel
} else {
gddram[pixel_pos(row,col)] &= ~pixel_mask(row); // clear the pixel
}
}
int display_pixel_get(int row, int col) {
return (gddram[pixel_pos(row,col)] & pixel_mask(row)) != 0;
}
/**
* @brief IntegratePeaksCWSD::simplePeakIntegration
* Purpose:
* Integrate a single crystal peak with the simplest algorithm, i.e.,
* by adding all the signal with normalization to monitor counts
* Requirements:
* Valid MDEventWorkspace
* Valid PeaksWorkspace
* Guarantees:
* A valid value is given
*/
void IntegratePeaksCWSD::simplePeakIntegration(
const std::vector<detid_t> &vecMaskedDetID,
const std::map<int, signal_t> &run_monitor_map) {
// Check requirements
if (!m_inputWS)
throw std::runtime_error("MDEventWorkspace is not defined.");
// Go through to get value
API::IMDIterator *mditer = m_inputWS->createIterator();
size_t nextindex = 1;
bool scancell = true;
// size_t currindex = 0;
// Assuming that MDEvents are grouped by run number, there is no need to
// loop up the map for peak center and monitor counts each time
int current_run_number = -1;
signal_t current_monitor_counts = 0;
Kernel::V3D current_peak_center = m_peakCenter;
// signal_t total_signal = 0;
double min_distance = 10000000;
double max_distance = -1;
while (scancell) {
// Go through all the MDEvents in one cell.
size_t numeventincell = mditer->getNumEvents();
for (size_t iev = 0; iev < numeventincell; ++iev) {
// Get signal to add and skip if signal is zero
signal_t signal = mditer->getInnerSignal(iev);
if (signal <= THRESHOLD_SIGNAL)
continue;
uint16_t run_number = mditer->getInnerRunIndex(iev);
int run_number_i = static_cast<int>(run_number);
/* debug: record raw signals
if (run_number_i % 1000 == testrunnumber)
{
total_signal += signal;
++ num_det;
}
// ... debug */
// Check whether this detector is masked
if (vecMaskedDetID.size() > 0) {
detid_t detid = mditer->getInnerDetectorID(iev);
std::vector<detid_t>::const_iterator it;
it = find(vecMaskedDetID.begin(), vecMaskedDetID.end(), detid);
if (it != vecMaskedDetID.end()) {
// The detector ID is found among masked detector IDs
// Skip this event and move to next
/* debug: record masked detectors
if (run_number_i % 1000 == testrunnumber)
{
num_masked_det += 1;
g_log.warning() << "Masked detector ID = " << detid << ", Signal = "
<< signal << "\n";
}
// ... debug */
continue;
}
}
/* debug: record unmasked detectors
if (run_number_i % 1000 == testrunnumber)
num_unmasked_det += 1;
// ... debug */
// Check whether to update monitor counts and peak center
if (current_run_number != run_number_i) {
// update run number
current_run_number = run_number_i;
// update monitor counts
if (m_normalizeByMonitor) {
std::map<int, signal_t>::const_iterator m_finder =
run_monitor_map.find(current_run_number);
if (m_finder != run_monitor_map.end())
current_monitor_counts = m_finder->second;
else {
std::stringstream errss;
errss << "Unable to find run number " << current_run_number
<< " in monitor counts map";
throw std::runtime_error(errss.str());
}
} else {
current_monitor_counts = 1.;
}
// update peak center
if (!m_useSinglePeakCenterFmUser)
current_peak_center = m_runPeakCenterMap[current_run_number];
}
// calculate distance
float tempx = mditer->getInnerPosition(iev, 0);
float tempy = mditer->getInnerPosition(iev, 1);
float tempz = mditer->getInnerPosition(iev, 2);
Kernel::V3D pixel_pos(tempx, tempy, tempz);
double distance = current_peak_center.distance(pixel_pos);
/* debug: record unmasked signal
if (run_number_i % 1000 == testrunnumber)
{
total_unmasked_signal += signal;
}
// ... debug */
if (distance < m_peakRadius) {
// FIXME - Is it very costly to use map each time???
// total_signal += signal/current_monitor_counts;
m_runPeakCountsMap[run_number] += signal / current_monitor_counts;
} else {
g_log.debug() << "Out of radius " << distance << " > " << m_peakRadius
<< ": Center = " << current_peak_center.toString()
<< ", Pixel = " << pixel_pos.toString() << "\n";
}
if (distance < min_distance)
min_distance = distance;
if (distance > max_distance)
max_distance = distance;
}
// Advance to next cell
if (mditer->next()) {
// advance to next cell
mditer->jumpTo(nextindex);
++nextindex;
} else {
// break the loop
scancell = false;
}
} // END-WHILE (scan-cell)
// Summarize
g_log.notice() << "Distance range is " << min_distance << ", " << max_distance
<< "\n";
/*
g_log.warning() << "Debug output: run 13: Number masked detectors = " <<
num_masked_det
<< ", Total signal = " << total_signal << "\n";
g_log.warning() << " Number of unmasked detectors = " << num_unmasked_det
<< ", Total unmasked signal = " << total_unmasked_signal <<
"\n";
g_log.warning() << " Number of total detectors = " << num_det << "\n";
*/
}
int RenderCore::step(Rand& rand, int groupIdx)
{
const Camera* active_camera = &camera;
float4x4 inv_view = glm::inverse(active_camera->view());
float4x4 inv_proj = glm::inverse(active_camera->projection());
float3 o = active_camera->eye;
int samples_n = 0;
bool clear = clearSignal_[groupIdx];
for(int i = 0; i < size_.y; i++)
{
if((i % cachedWorkThreadN_) != groupIdx) continue;
for(int j = 0; j < size_.x; j++)
{
int fb_i = i * size_.x + j;
Sample sample;
sample.reset();
if(clear)
{
linear_fb[fb_i] = float4(0);
}
float2 pixel_pos(j, i);
float2 sample_pos = pixel_pos + float2(rand.next01(), rand.next01());
float4 ndc((sample_pos.x/size_.x-0.5)*2, (sample_pos.y/size_.y-0.5)*2, 1, 1);
float4 d_comp = inv_proj * ndc;
d_comp = d_comp/d_comp.w;
d_comp = inv_view * d_comp;
float3 d = normalize(float3(d_comp));
Ray ray(o, d);
sample.ray = ray;
sample.xy = int2(j, i);
sampleDebugger_.shr.newSample(sample.xy);
int bounces = 8 ;
if(1)
{
ptMISRun(*scene, bounces, rand, &sample, false);
}
else if(0)
{
ptRun(*scene, bounces, rand, &sample, false);
}
else if(0)
{
bdptRun(*scene, bounces, rand, &sample);
}
else
{
bdptMisRun(*scene, bounces, active_camera->forward, rand, &sample);
}
samples_n += 1;
float3 color = sample.radiance;
sampleDebugger_.shr.record(sample.xy, 7, "Radiance", color);
float3 existing_color = float3(linear_fb[fb_i]);
sampleDebugger_.shr.record(sample.xy, 7, "Existing Mix", existing_color);
float samples_count = linear_fb[fb_i].w + 1;
float3 mixed_color = float3((samples_count - 1) / samples_count) * existing_color +
float3(1/samples_count) * color;
sampleDebugger_.shr.record(sample.xy, 7, "New Mix", mixed_color);
linear_fb[fb_i] = float4(mixed_color, samples_count);
}
}
//we don't clear unconditionally b/c clear may have been enabled since we last checked
if(clear) clearSignal_[groupIdx] = false;
return samples_n;
}
int main(int argc, char **argv) {
int **ima,**varnum,**varnuminv,*dir,*size,*numvois,**vois,**voisinv,*mag_clipped,generate_image,numell,nflip,wrongpolarized,numpolarized,error;
double *yy,*cpx,*cpy,**field,**hatf,*hloc,jcoup,**jj,*jchain,err_mess,onsager,frustr,hftyp,mse,sum,damping,fieldav,fieldnew;
int L,Lmax,N,M,Mmax,seed,n,i,j,k,mu,numdir,iter,niter,sizemax,sizetot,s,writeperiod,numcan,numfrozen,nummicroc,microc_threshold,typecalc,nvmax,wrongspins;
FILE *ImageOrig,*ImageRec,*Imtoimport,*binary_out;
double *htot;
if(argc!=11) {printf("Usage : %s L(0 to read in trybin_256.txt, -1 for the 1025-size one) numell numdir jcoup niter writeperiod microc_threshold damping BIGFIELD seed \n",argv[0]);exit(1);}
L=atoi(argv[1]);// L=0 means that one reads the file trybin_256.txt. Any other L means one generates the image, of size L*L
numell=atoi(argv[2]);// number of ellipses in the image
numdir=atoi(argv[3]);//number of directions of measurement
jcoup=atof(argv[4]);
niter=atoi(argv[5]);// maximal number of iterations
writeperiod=atoi(argv[6]);//period at which one fprints the spins and the local fields
microc_threshold=atoi(argv[7]);// in principle, should be 1. But a smaller number, like .5, improves convergence...
damping=atof(argv[8]);
BIGFIELD=atof(argv[9]);
seed=atoi(argv[10]);// seed for random number generator
onsager=1;
binary_out=fopen("binary_out.dat","w");
generate_image=1;
if(L==0){
L=256;
generate_image=0;
Imtoimport=fopen("trybin_256.txt","r");
}
if(L==-1){
L=1025;
generate_image=0;
Imtoimport=fopen("trybin.txt","r");
}
N=L*L;
Lmax=2*L;// maximum number of variables involved in one measurement
printf("L=%i N=%i \n",L,N);
initialize_rangen(seed);
ima=imatrix(1,L,1,L);
Mmax=numdir*L*SQR2;// upper bound on the number of measurements
varnum=imatrix(1,Mmax,1,Lmax);//varnum[mu][r] gives the index of the r-th spin in the line mu.
varnuminv=imatrix(1,Mmax,1,Lmax);// s=varnuminv[mu][k] <=> vois[varnum[mu][k]][s]=mu (mu is the s-th neighbour of his k-th neighbour)
yy=dvector(1,Mmax);//results of measurements
dir=ivector(1,Mmax);// in what direction was a measurement
size=ivector(1,Mmax);//number of variables involved in a measurement
jj=dmatrix(1,Mmax,1,Lmax);//Ising couplings in the chains
jchain=dvector(1,Lmax);//Ising couplings in one given chain
numvois=ivector(1,N);// number of measurements in which a varaible appears
vois=imatrix(1,N,1,numdir);// list of measurements where a variable appears: vois[i][r] is the index of the r-th line where the vertex number i is present
voisinv=imatrix(1,N,1,numdir);// s=voisinv[i][r] <=> varnum[vois[i][r]][s]=i (i is the s-th neighbour of his r-th neighbour).
mag_clipped=ivector(1,N);// magnetization found by clipping htot
htot=dvector(1,N);// full local field found by BP
// Generate or read the image
ImageOrig=fopen("ImageOrig.dat","w");
if(generate_image==1){
cpx=dvector(1,N);
cpy=dvector(1,N);
pixel_pos(L,cpx,cpy);
genimage(L,numell,ima,cpx,cpy);
free_dvector(cpx,1,N);
free_dvector(cpy,1,N);
}
else{
for (i=1;i<=L;i++) {
for(j=1;j<=L;j++) {
fscanf(Imtoimport,"%i ",&k);
if(k==0)ima[i][j]=1;
else ima[i][j]=-1;
}
}
}
for(n=1;n<=N;n++){
matrixcoordinates(L,n,&i,&j);
fprintf(ImageOrig,"%i ",ima[i][j]);
//fprintf(ImageOrig,"\n");
}
fclose(ImageOrig);
// Perform the measurements (Radon)
calc_measurements(L,Lmax,numdir,ima,varnum,yy,dir,size,vois,numvois,jj,jcoup,&M);
if(M>Mmax){
printf("M>Mmax, please increase Mmax: %i %i\n",M,Mmax);
exit(2);
}
sum=0;
for (i=1;i<=L;i++) {
for(j=1;j<=L;j++) sum+=ima[i][j];
}
printf("generated an image with %i pixels and magnetization %i\n",L*L,(int)sum);
// Geometry of the factor graph and checks
calc_voisinv(N,numdir,vois,numvois,size,voisinv,varnum);
calc_varnuminv(M,numdir,vois,numvois,size,varnum,varnuminv);
sizemax=0;sizetot=0;
for(mu=1;mu<=M;mu++){
sizetot+=size[mu];
if(size[mu]>sizemax) sizemax=size[mu];
if(verbose>5){
printf("Constraint mu=%i yy[mu]=%f size=%i: ",mu,yy[mu],size[mu]);
for(i=1;i<=size[mu];i++) printf("%i ",varnum[mu][i]);
printf("\n");
}
}
if(sizemax>Lmax){
printf("STOP sizemax>Lmax\n");
exit(12);
}
nvmax=0;
for(i=1;i<=N;i++){
if(numvois[i]>nvmax)nvmax=numvois[i];
}
//Initialisation of the messages
field=dmatrix(1,M,1,sizemax);// field[mu][r] is the local magnetic field h_{mu to i} where i is the r-th neighbour of mu
hatf=dmatrix(1,N,1,nvmax);//hatf[i][r] is the local magnetic field \hat h_{i to mu} where mu is the r-th neighbour of i
initfield(M,field,size,yy);
calc_hatf(N,L,field,hatf,htot,numvois,vois,voisinv,varnum,&frustr,&hftyp,ima,&mse);
if(verbose>12){
printf("Initial field: \n");
for(mu=1;mu<=M;mu++){
for(n=1;n<=size[mu];n++) printf("%f ",field[mu][n]);
printf("\n\n");
}
printf("Initial hatf: \n");
for(n=1;n<=N;n++) {
for (i=1;i<=numvois[n];i++) printf("%f ",hatf[n][i]);
printf("\n\n");
}
}
hloc=dvector(1,sizemax);
printf("%i Measurements done; starting iteration, niter=%i\n",M,niter);fflush(stdout);
fprintf(binary_out,"# N=%i numdir=%i M=%i BIGFIELD= %f microc_threshold=%i\n",N, numdir, M, BIGFIELD,microc_threshold);
for(n=1;n<=N;n++) mag_clipped[n]=1;
//START BP ITERATION HERE!
for(iter=0;iter<=niter;iter++){
err_mess=0;
sizetot=0;
fieldav=0;
numcan=0;nummicroc=0;numfrozen=0;
for(mu=1;mu<=M;mu++){
//update all messages h_{mu to i}
if(verbose>10) printf("\n iter=%i: Solving chain mu=%i, with %i spins ",iter,mu,size[mu]);
calc_field(mu,size,microc_threshold,onsager,varnum,varnuminv,yy,jj,hatf,hloc,&typecalc);
if(typecalc==0) numfrozen++;
if(typecalc==1) nummicroc++;
if(typecalc==2) numcan++;
for(k=1;k<=size[mu];k++) { // compute field
err_mess+=fabs(tanh(hloc[k])-tanh(field[mu][k]));
sizetot++;
fieldnew=damping*field[mu][k]+(1.-damping)*hloc[k];
update_hatf(mu,k,varnum,varnuminv,vois,numvois,hatf,htot,fieldnew-field[mu][k]);// update hatf for all constraints nu neighbours of varaible i, except nu;
field[mu][k]=fieldnew;
fieldav+=fabs(field[mu][k]);
}
}
//printf("\n");
err_mess/=sizetot;
fieldav/=sizetot;
printf("iter= %i, err_mess=%f, fieldav=%f ; frustration=%f, typical hatfield=%f, mse=%e, numcan=%i, nummicroc=%i,numfrozen=%i \n",
iter,err_mess,fieldav,frustr,hftyp,mse,numcan,nummicroc,numfrozen);
fflush(stdout);
// Monitoring of the progress of BP: compute present image obtained by clipping, and how well it reconstructs
nflip=0;
wrongpolarized=0;numpolarized=0;wrongspins=0;
for(n=1;n<=N;n++){
if(fabs(htot[n])>2*BIGFIELD) numpolarized++;
if(htot[n]*mag_clipped[n]<0)nflip++;
if(htot[n]>0) mag_clipped[n]=1;
else mag_clipped[n]=-1;
matrixcoordinates(L,n,&i,&j);
if(mag_clipped[n]!=ima[i][j]) {
wrongspins++;
if(fabs(htot[n])>2*BIGFIELD) wrongpolarized++;
}
}
error=0;
for(mu=1;mu<=M;mu++){//check the constraints
sum=0;
for(k=1;k<=size[mu];k++) sum+=mag_clipped[varnum[mu][k]];
if(sum!=yy[mu]) error++;
}
printf("Number of wrong spins=%i, fraction wrong = %f ; nflip=%i ; number of unsat constraints=%i numpolarized=%i wrongpolarized=%i\n",
wrongspins,((double)wrongspins)/N,nflip,error,numpolarized,wrongpolarized);
if(wrongpolarized>0) printf("ATTENTION wrongpolarized!!!!!!!!!!\n");
fprintf(binary_out," %i %i %f %i %i\n",iter,(int)mse, mse/N,nflip,error);
if(err_mess<.0001) break;//BP has converged
if(error==0)break;// All constraints are satisfied
}
// fprintf the final image obtained by BP reconstruction
ImageRec=fopen("ImageRec.dat","w");
for(i=1;i<=L;i++){
for(j=1;j<=L;j++){
fprintf(ImageRec,"%i ",mag_clipped[indexsite(L,i,j)]);
}
fprintf(ImageRec,"\n");
}
fprintf(ImageRec,"\n");
return 1;
}
void calc_measurements(int L,int Lmax, int numdir, int **ima, int ** varnum,double *yy,int *dir,int *size,int **vois,int *numvois,double **jj,double jcoup, int *Mtrue){
int mu,sum,i,j,k;
int N=L*L,n,bin[L*L+1],sizebin[2*L+1],indpix[2*L+1][2*L+1],ibin,ibinmax,indspin[2*L+1];
double cpx[L*L+1],cpy[L*L+1],theta,ymin,ymax,xx[2*L+1];
mu=0;
for (k=1;k<=L*L;k++) numvois[k]=0;
//
for(theta=0;theta<PI-.00001;theta+=PI/numdir){
pixel_pos(L,cpx,cpy);
rotate_image(N,cpx,cpy,theta);
calc_ybounds(N,cpy,&ymin,&ymax);
printf("theta=%f ymin=%f ymax=%f\n",theta,ymin,ymax);
for(ibin=0;ibin<=2*L;ibin++) sizebin[ibin]=0;
for(n=1;n<=N;n++) {
bin[n]=cpy[n]-ymin+1;// bin belongs to 1,...,(int)(ymax-ymin)+1;
if(bin[n]>2*L) {printf("cata bin\n");exit(2);}
sizebin[bin[n]]++;
indpix[bin[n]][sizebin[bin[n]]]=n;
}
// For each bin, numbered as ibin, we have the number of spins in it, sizebin[ibin] and their list indpix[ibin,1]... indpix[ibin,sizebin[ibin]
ibinmax= 1+ymax-ymin;
if(verbose>8){
for(ibin=1;ibin<=ibinmax;ibin++) {
printf("bin number %i size %i list:",ibin,sizebin[ibin]);
for(i=1;i<=sizebin[ibin];i++) printf(" %i ",indpix[ibin][i]);
printf("\n");
}
}
for(ibin=1;ibin<=ibinmax;ibin++) {
if(sizebin[ibin]>0){
mu++;
size[mu]=sizebin[ibin];
// build ordered list of the spins in the constraint mu, ordered by the value of their x coordinate
for(i=1;i<=sizebin[ibin];i++) {
indspin[i]=indpix[ibin][i];
xx[i]=cpx[indpix[ibin][i]];
}
sort2_MM(sizebin[ibin],xx,indspin);
if(verbose>15){
printf("bin number %i size %i ordered list:",ibin,sizebin[ibin]);
for(i=1;i<=sizebin[ibin];i++) printf(" %i %f ;",indspin[i],xx[i]);
printf("\n");
}
for(i=1;i<=size[mu];i++) varnum[mu][i]=indspin[i];// i-ieme voisin dans mu
for(i=1;i<size[mu];i++) {
if(verbose>10) printf("... measurement mu=%i. Compute the coupling between variables %i and %i\n",mu,varnum[mu][i],varnum[mu][i+1]);
jj[mu][i]=calcjeff(jcoup,cpx[varnum[mu][i]],cpx[varnum[mu][i+1]],cpy[varnum[mu][i]],cpy[varnum[mu][i+1]]);
}
sum=0;
for(n=1;n<=size[mu];n++) {
k=varnum[mu][n];
numvois[k]++;
vois[k][numvois[k]]=mu;
//printf("TESTTEST mu=%i k=%i numvois=%i vois=%i\n",mu,k,numvois[k],vois[k][numvois[k]]);
matrixcoordinates(L,k,&i,&j);
sum+= ima[i][j];
}
yy[mu]=sum;
dir[mu]=theta;
}
}
}
*Mtrue=mu;
for(mu=1;mu<=*Mtrue;mu++){
if(size[mu]>Lmax){
printf("DESASTRE mu=%i size=%i Lmax=%i\n",mu,size[mu],Lmax);
}
}
if(verbose>4){
for(mu=1;mu<=*Mtrue;mu++){
printf("Measurement mu=%i, size=%i, list=",mu,size[mu]);
for(n=1;n<=size[mu];n++){
printf("%i ",varnum[mu][n]);
matrixcoordinates(L,varnum[mu][n],&i,&j);
printf("ima(%i,%i)=%i ;",i,j,ima[i][j]);
}
printf("\n couplings=");
for(i=1;i<size[mu];i++) printf("%f ",jj[mu][i]);
printf(" yy=%f\n",yy[mu]);
}
for(i=1;i<=L*L;i++){
printf(" variable %i belongs to these %i checks :",i,numvois[i]);
for(n=1;n<=numvois[i];n++) printf(" %i",vois[i][n]);
printf("\n");
}
}
}
ProcessResult PluginDetectBalls::process ( FrameData * data, RenderOptions * options ) {
( void ) options;
if ( data==0 ) return ProcessingFailed;
SSL_DetectionFrame * detection_frame = 0;
detection_frame= ( SSL_DetectionFrame * ) data->map.get ( "ssl_detection_frame" );
if ( detection_frame == 0 ) detection_frame= ( SSL_DetectionFrame * ) data->map.insert ( "ssl_detection_frame",new SSL_DetectionFrame() );
int color_id_ball = _lut->getChannelID ( _settings->_color_label->getString() );
if ( color_id_ball == -1 ) {
printf ( "Unknown Ball Detection Color Label: '%s'\nAborting Plugin!\n",_settings->_color_label->getString().c_str() );
return ProcessingFailed;
}
//delete any previous detection results:
detection_frame->clear_balls();
//TODO: add a vartype notifier for better performance.
//initialize filter:
if (vnotify.hasChanged()) {
max_balls = _settings->_max_balls->getInt();
filter.setWidth ( _settings->_ball_min_width->getInt(),_settings->_ball_max_width->getInt() );
filter.setHeight ( _settings->_ball_min_height->getInt(),_settings->_ball_max_height->getInt() );
filter.setArea ( _settings->_ball_min_area->getInt(),_settings->_ball_max_area->getInt() );
field_filter.update ( field );
//copy all vartypes to local variables for faster repeated lookup:
filter_ball_in_field = _settings->_ball_on_field_filter->getBool();
filter_ball_on_field_filter_threshold = _settings->_ball_on_field_filter_threshold->getDouble();
filter_ball_in_goal = _settings->_ball_in_goal_filter->getBool();
filter_ball_histogram = _settings->_ball_histogram_enabled->getBool();
if ( filter_ball_histogram ) {
if ( color_id_ball != color_id_orange ) {
printf ( "Warning: ball histogram check is only configured for orange balls!\n" );
printf ( "Please disable the histogram check in the Ball Detection Plugin settings\n" );
}
if ( color_id_pink==-1 || color_id_orange==-1 || color_id_yellow==-1 || color_id_field==-1 ) {
printf ( "WARNING: some LUT color labels where undefined for the ball detection plugin\n" );
printf ( " Disabling histogram check!\n" );
filter_ball_histogram=false;
}
}
min_greenness = _settings->_ball_histogram_min_greenness->getDouble();
max_markeryness = _settings->_ball_histogram_max_markeryness->getDouble();
//setup values used for the gaussian confidence measurement:
filter_gauss = _settings->_ball_gauss_enabled->getBool();
exp_area_min = _settings->_ball_gauss_min->getInt();
exp_area_max = _settings->_ball_gauss_max->getInt();
exp_area_var = sq ( _settings->_ball_gauss_stddev->getDouble() );
z_height= _settings->_ball_z_height->getDouble();
near_robot_filter = _settings->_ball_too_near_robot_enabled->getBool();
near_robot_dist_sq = sq(_settings->_ball_too_near_robot_dist->getDouble());
}
const CMVision::Region * reg = 0;
//acquire orange region list from data-map:
CMVision::ColorRegionList * colorlist;
colorlist= ( CMVision::ColorRegionList * ) data->map.get ( "cmv_colorlist" );
if ( colorlist==0 ) {
printf ( "error in ball detection plugin: no region-lists were found!\n" );
return ProcessingFailed;
}
reg = colorlist->getRegionList ( color_id_ball ).getInitialElement();
//acquire color-labeled image from data-map:
const Image<raw8> * image = ( Image<raw8> * ) ( data->map.get ( "cmv_threshold" ) );
if ( image==0 ) {
printf ( "error in ball detection plugin: no color-thresholded image was found!\n" );
return ProcessingFailed;
}
int robots_blue_n=0;
int robots_yellow_n=0;
bool use_near_robot_filter=near_robot_filter;
if ( use_near_robot_filter ) {
SSL_DetectionFrame * detection_frame = ( SSL_DetectionFrame * ) data->map.get ( "ssl_detection_frame" );
if ( detection_frame==0 ) {
use_near_robot_filter=false;
} else {
robots_blue_n=detection_frame->robots_blue_size();
robots_yellow_n=detection_frame->robots_yellow_size();
if (robots_blue_n==0 && robots_yellow_n==0) use_near_robot_filter=false;
}
}
if ( max_balls > 0 ) {
list<BallDetectResult> result;
filter.init ( reg );
while ( ( reg = filter.getNext() ) != 0 ) {
float conf = 1.0;
if ( filter_gauss==true ) {
int a = reg->area - bound ( reg->area,exp_area_min,exp_area_max );
conf = gaussian ( a / exp_area_var );
}
//TODO: add a plugin for confidence masking... possibly multi-layered.
// to replace the commented det.mask.get(...) below:
//if (filter_conf_mask) conf*=det.mask.get(reg->cen_x,reg->cen_y));
//convert from image to field coordinates:
vector2d pixel_pos ( reg->cen_x,reg->cen_y );
vector3d field_pos_3d;
camera_parameters.image2field ( field_pos_3d,pixel_pos,z_height );
vector2d field_pos ( field_pos_3d.x,field_pos_3d.y );
//filter points that are outside of the field:
if ( filter_ball_in_field==true && field_filter.isInFieldPlusThreshold ( field_pos, max(0.0,filter_ball_on_field_filter_threshold) ) ==false ) {
conf = 0.0;
}
//filter out points that are deep inside the goal-box
if ( filter_ball_in_goal==true && field_filter.isFarInGoal ( field_pos ) ==true ) {
conf = 0.0;
}
//TODO add ball-too-near-robot filter
if ( use_near_robot_filter && conf > 0.0 ) {
int robots_n=0;
for (int team = 0; team < 2; team++) {
if (team==0) {
robots_n=robots_blue_n;
} else {
robots_n=robots_yellow_n;
}
if (robots_n > 0) {
::google::protobuf::RepeatedPtrField< ::SSL_DetectionRobot > * robots;
if (team==0) {
robots = detection_frame->mutable_robots_blue();
} else {
robots = detection_frame->mutable_robots_yellow();
}
for (int r = 0; r < robots_n; r++) {
const SSL_DetectionRobot & robot = robots->Get(r);
if (robot.confidence() > 0.0) {
if ((sq((double)(robot.x())-(double)(field_pos.x)) + sq((double)(robot.y())-(double)(field_pos.y))) < near_robot_dist_sq) {
conf = 0.0;
break;
}
}
}
if (conf==0.0) break;
}
}
}
// histogram check if enabled
if ( filter_ball_histogram && conf > 0.0 && checkHistogram ( image, reg, min_greenness, max_markeryness ) ==false ) {
conf = 0.0;
}
// add filtered region to the region list
if(conf > 0) {
result.push_back(BallDetectResult(reg,conf));
}
}
// sort result by confidence and output first max_balls region(s)
result.sort();
int num_ball = 0;
list<BallDetectResult>::reverse_iterator it;
for(it=result.rbegin(); it!=result.rend(); it++) {
if(++num_ball > max_balls)
break;
//update result:
SSL_DetectionBall* ball = detection_frame->add_balls();
ball->set_confidence ( it->conf );
vector2d pixel_pos ( it->reg->cen_x,it->reg->cen_y );
vector3d field_pos_3d;
camera_parameters.image2field ( field_pos_3d,pixel_pos,z_height );
ball->set_area ( it->reg->area );
ball->set_x ( field_pos_3d.x );
ball->set_y ( field_pos_3d.y );
ball->set_pixel_x ( it->reg->cen_x );
ball->set_pixel_y ( it->reg->cen_y );
}
}
return ProcessingOk;
}
