void GTVideo::estimateBackground()
{
int buffer_size = 30;
uchar* arr_R = new uchar[buffer_size];
uchar* arr_G = new uchar[buffer_size];
uchar* arr_B = new uchar[buffer_size];
int width = source.at(0).cols;
int height = source.at(0).rows;
for(int i=0;i<height;i++)
for(int j=0;j<width;j++)
{
for(int k=1;k<=buffer_size;k++)
{
arr_R[k]= source.at(k*10).data[(i*width+j)*3];
arr_G[k]= source.at(k*10).data[(i*width+j)*3 + 1];
arr_B[k]= source.at(k*10).data[(i*width+j)*3 + 2];
}
background.data[(i*width+j)*3]=quick_select(arr_R,buffer_size);
background.data[(i*width+j)*3+1]=quick_select(arr_G,buffer_size);
background.data[(i*width+j)*3+2]=quick_select(arr_B,buffer_size);
}
cv::imwrite("background.jpg",background);
}
int main(int argc, char **argv) {
//0 2 4 5 8 9 10 23
int arr[8] = {9, 2 ,4 ,5, 8, 10, 0, 23};
int select_index = 5;
int index = quick_select(arr, 8, select_index);
printf("the %dth index of array is %d\n", select_index, arr[index]);
return 0;
}
// get median value of an given image
CvScalar myCvMedian(IplImage* img) {
// split into 3 channels
CvMat *L = cvCreateMat(img->width, img->height, CV_8UC1),
*a = cvCreateMat(img->width, img->height, CV_8UC1),
*b = cvCreateMat(img->width, img->height, CV_8UC1);
cvSplit(img, L, a, b, NULL);
int size = img->width*img->height;
float arrL[size], arra[size], arrb[size];
// convert to array
int i = 0;
for(int row = 0; row < L->rows; row++) {
const float* ptr = (const float*)(L->data.ptr + row*L->step);
for(int col = 0; col < L->cols; col++) {
arrL[i++] = *ptr++;
}
}
i = 0;
for(int row = 0; row < a->rows; row++) {
const float* ptr = (const float*)(a->data.ptr + row*a->step);
for(int col = 0; col < a->cols; col++) {
arra[i++] = *ptr++;
}
}
i = 0;
for(int row = 0; row < b->rows; row++) {
const float* ptr = (const float*)(b->data.ptr + row*b->step);
for(int col = 0; col < b->cols; col++) {
arrb[i++] = *ptr++;
}
}
// get median of each array
int n = NELEMS(arrL);
int medL = quick_select(arrL, n),
meda = quick_select(arra, n),
medb = quick_select(arrb, n);
CvScalar re = cvScalar(medL, meda, medb, 0);
cvReleaseMat(&L);
cvReleaseMat(&a);
cvReleaseMat(&b);
return re;
}
/*---------------------------------------------------------------------------
// Function median:
// compute median
// Input float array t[] of length n
// computation in situ, t will be reordered
---------------------------------------------------------------------------*/
float median(float t[],int n)
{
float q,q2;
if (n%2 == 1) //odd
{
q = kth_smallest(t,n,(n+1)/2);
return q;
}
/* else even: */
/* q = kth_smallest(t,n,n/2);
q2 = kth_smallest(t,n,n/2 + 1); */
q = quick_select(t,n,n/2);
q2 = quick_select(t,n,n/2 + 1);
return 0.5 * (q + q2);
}
int LocBackSub(dtype* theArray, int* dims, int* box)
{
float* arr1 = new float[(2*box[0]+1)*(2*box[1]+1)];
for (int xi=0; xi<dims[0]; xi++)
for (int yi=0; yi<dims[1]; yi++)
{ int nuel = 0;
if (theArray[xi+dims[0]*yi]<0) continue;
for (int bxi=-box[0]; bxi<=box[0]; bxi++)
for (int byi=-box[1]; byi<=box[1]; byi++)
if (xi+bxi>=0 && xi+bxi<dims[0] && yi+byi>=0 && yi+byi<dims[1])
if (theArray[xi+bxi+dims[0]*(yi+byi)]>=0)
{ arr1[nuel] = theArray[xi+bxi+dims[0]*(yi+byi)];
nuel++;
}
if (nuel>0) theArray[xi+dims[0]*yi] -= quick_select(arr1, nuel);
}
delete arr1;
return 1;
}
void MedianCalculator::doRecalculate()
{
Recalculate = false;
// Set default result
MinValue = 0.;
MaxValue = 0.;
MeanValue = 0.;
MedianValue = 0.;
// Calculate median
const int count = GetCount();
for (int i = 0; i < count; ++i)
SortBuffer[i] = Data[i];
MedianValue = quick_select(&SortBuffer[0], count);
double minValue = Data[0], maxValue = Data[0], sumValue = Data[0];
for (int i = 1; i < count; ++i)
{
double value = Data[i];
if (value < minValue)
{
minValue = value;
}
if (value > maxValue)
{
maxValue = value;
}
sumValue += value;
}
MinValue = minValue;
MaxValue = maxValue;
MeanValue = sumValue / count;
OVR_ASSERT(MinValue <= MeanValue && MeanValue <= MaxValue);
OVR_ASSERT(MinValue <= MedianValue && MedianValue <= MaxValue);
}
void mitk::ToFCompositeFilter::ProcessStreamedQuickSelectMedianImageFilter(IplImage* inputIplImage)
{
float* data = (float*)inputIplImage->imageData;
int imageSize = inputIplImage->width * inputIplImage->height;
float* tmpArray;
if (this->m_TemporalMedianFilterNumOfFrames == 0)
{
return;
}
if (m_TemporalMedianFilterNumOfFrames != this->m_DataBufferMaxSize) // reset
{
//delete current buffer
for( int i=0; i<this->m_DataBufferMaxSize; i++ ) {
delete[] this->m_DataBuffer[i];
}
if (this->m_DataBuffer != NULL)
{
delete[] this->m_DataBuffer;
}
this->m_DataBufferMaxSize = m_TemporalMedianFilterNumOfFrames;
// create new buffer with current size
this->m_DataBuffer = new float*[this->m_DataBufferMaxSize];
for(int i=0; i<this->m_DataBufferMaxSize; i++)
{
this->m_DataBuffer[i] = NULL;
}
this->m_DataBufferCurrentIndex = 0;
}
int currentBufferSize = this->m_DataBufferMaxSize;
tmpArray = new float[this->m_DataBufferMaxSize];
// copy data to buffer
if (this->m_DataBuffer[this->m_DataBufferCurrentIndex] == NULL)
{
this->m_DataBuffer[this->m_DataBufferCurrentIndex] = new float[imageSize];
currentBufferSize = this->m_DataBufferCurrentIndex + 1;
}
for(int j=0; j<imageSize; j++)
{
this->m_DataBuffer[this->m_DataBufferCurrentIndex][j] = data[j];
}
float tmpValue = 0.0f;
for(int i=0; i<imageSize; i++)
{
if (m_ApplyAverageFilter)
{
tmpValue = 0.0f;
for(int j=0; j<currentBufferSize; j++)
{
tmpValue+=this->m_DataBuffer[j][i];
}
data[i] = tmpValue/currentBufferSize;
}
else if (m_ApplyTemporalMedianFilter)
{
for(int j=0; j<currentBufferSize; j++)
{
tmpArray[j] = this->m_DataBuffer[j][i];
}
data[i] = quick_select(tmpArray, currentBufferSize);
}
}
this->m_DataBufferCurrentIndex = (this->m_DataBufferCurrentIndex + 1) % this->m_DataBufferMaxSize;
delete[] tmpArray;
}
double nbody_scale(int N_node, double q, struct star *s, int *nnbmax_out, double *rs0_out)
{
int NNBMAX;
NNBMAX = 1.25*sqrt(N_node);
if (NNBMAX<30) NNBMAX=30;
if (N_node<=NNBMAX) NNBMAX=0.5*N_node;
*nnbmax_out = NNBMAX;
double RS0;
int i,j;
double Ek=0,Ep=0;
// scale to totle energy = -0.25
double Ep_nbody = -0.25/(1-q);
double Ek_nbody = 0.25*q/(1-q);
double r2,rscale,vscale;
double *rij;
struct star centre={0,{0,0,0,0,0,0}};
// centre of mass correction
for (i=0;i<N_node;++i) {
centre.m += s[i].m;
centre.x[0] += s[i].m * s[i].x[0];
centre.x[1] += s[i].m * s[i].x[1];
centre.x[2] += s[i].m * s[i].x[2];
centre.x[3] += s[i].m * s[i].x[3];
centre.x[4] += s[i].m * s[i].x[4];
centre.x[5] += s[i].m * s[i].x[5];
}
for (i=0;i<6;++i) centre.x[i] /= centre.m;
// all stars including binaries and com-binaries
for (i=0;i<N_node;++i) {
s[i].m /= centre.m;
s[i].x[0] -= centre.x[0];
s[i].x[1] -= centre.x[1];
s[i].x[2] -= centre.x[2];
s[i].x[3] -= centre.x[3];
s[i].x[4] -= centre.x[4];
s[i].x[5] -= centre.x[5];
}
RS0=0;
#ifdef _OPENMP
#pragma omp parallel private(rij)
{
rij = (double*) malloc (N_node*sizeof(double));
#pragma omp for private(i,j,r2) reduction(-:Ep) reduction(+:RS0) reduction(+:Ek)
for (i=0;i<N_node;++i) {
rij[i] = DBL_MAX;
for (j=0;j<N_node;++j) {
if (i!=j) {
r2 = (s[i].x[0] - s[j].x[0])*(s[i].x[0] - s[j].x[0]) +
(s[i].x[1] - s[j].x[1])*(s[i].x[1] - s[j].x[1]) +
(s[i].x[2] - s[j].x[2])*(s[i].x[2] - s[j].x[2]);
rij[j] = r2;
}
if (j>i) Ep -= s[i].m*s[j].m / sqrt(r2);
}
RS0 += sqrt( quick_select(rij,NNBMAX/5,N_node) );
Ek += s[i].m * (s[i].x[3]*s[i].x[3] + s[i].x[4]*s[i].x[4] + s[i].x[5]*s[i].x[5]);
}
free(rij);
}
Ek *= 0.5;
RS0 /= N_node;
#else
rij = (double*) malloc (N_node*sizeof(double));
for (i=0;i<N_node;++i) {
rij[i] = DBL_MAX;
for (j=0;j<N_node;++j) {
if (i != j) {
r2 = (s[i].x[0] - s[j].x[0])*(s[i].x[0] - s[j].x[0]) +
(s[i].x[1] - s[j].x[1])*(s[i].x[1] - s[j].x[1]) +
(s[i].x[2] - s[j].x[2])*(s[i].x[2] - s[j].x[2]);
rij[j] = r2;
}
if (j>i) Ep -= s[i].m*s[j].m / sqrt(r2);
}
RS0 += sqrt( quick_select(rij,NNBMAX/5,N_node) );
Ek += s[i].m * (s[i].x[3]*s[i].x[3] + s[i].x[4]*s[i].x[4] + s[i].x[5]*s[i].x[5]);
}
free(rij);
Ek *= 0.5;
RS0 /= N_node;
#endif
rscale = Ep/Ep_nbody;
if (Ek == 0) {
vscale = 1;
q = 0;
fprintf(stderr,"scale: kinetic energy = 0! no scale for velocity!\n");
} else {
vscale = sqrt(Ek_nbody/Ek);
}
fprintf(stderr,"scale: rscale, vscale: %lf, %lf\n",rscale,vscale);
*rs0_out = RS0*rscale;
fprintf(stderr,"NNBMAX=%d, RS0=%lf\n",NNBMAX,*rs0_out);
for (i=0;i<N_node;++i) {
s[i].x[0] *= rscale;
s[i].x[1] *= rscale;
s[i].x[2] *= rscale;
s[i].x[3] *= vscale;
s[i].x[4] *= vscale;
s[i].x[5] *= vscale;
}
return centre.m;
}
void bench(int detailed_printout, size_t array_size)
{
int i ;
int mednum ;
pixelvalue med[N_METHODS] ;
clock_t chrono ;
double elapsed ;
pixelvalue * array_init,
* array ;
/*
* initialize random generator with PID
* This is the only Unix-ish thing, replace this by any
* initialization scheme you wish.
*/
srand48(getpid()) ;
if (detailed_printout) {
printf("generating numbers...\n") ;
fflush(stdout) ;
}
if (array_size<1) array_size = BIG_NUM ;
if (detailed_printout) {
printf("array size: %ld\n", (long)array_size) ;
} else {
printf("%ld\t", (long)array_size) ;
}
array_init = malloc(array_size * sizeof(pixelvalue)) ;
array = malloc(array_size * sizeof(pixelvalue)) ;
if (array_init==NULL || array==NULL) {
printf("memory allocation failure: aborting\n") ;
return ;
}
for (i=0 ; i<array_size; i++) {
array_init[i] = (pixelvalue)(lrand48() % MAX_ARRAY_VALUE) ;
}
mednum = 0 ;
/* benchmark the quick select sort */
memcpy(array, array_init, array_size * sizeof(pixelvalue)) ;
if (detailed_printout) {
printf("quick select :\t") ;
fflush(stdout) ;
}
chrono = clock() ;
med[mednum] = quick_select(array, array_size) ;
elapsed = (double)(clock() - chrono) / (double)CLOCKS_PER_SEC ;
if (detailed_printout) {
printf("%5.3f sec\t", elapsed) ;
fflush(stdout) ;
printf("med %g\n", (double)med[mednum]) ;
fflush(stdout) ;
} else {
printf("%5.3f\t", elapsed) ;
fflush(stdout) ;
}
mednum++ ;
/* benchmark WIRTH */
memcpy(array, array_init, array_size * sizeof(pixelvalue)) ;
if (detailed_printout) {
printf("WIRTH median :\t") ;
fflush(stdout) ;
}
chrono = clock() ;
med[mednum] = median_WIRTH(array, array_size) ;
elapsed = (double)(clock() - chrono) / (double)CLOCKS_PER_SEC ;
if (detailed_printout) {
printf("%5.3f sec\t", elapsed) ;
fflush(stdout) ;
printf("med %g\n", (double)med[mednum]) ;
fflush(stdout) ;
} else {
printf("%5.3f\t", elapsed) ;
fflush(stdout) ;
}
mednum++ ;
/* benchmark the AHU sort */
memcpy(array, array_init, array_size * sizeof(pixelvalue)) ;
if (detailed_printout) {
printf("AHU median :\t") ;
fflush(stdout) ;
}
chrono = clock() ;
med[mednum] = median_AHU(array, array_size) ;
elapsed = (double)(clock() - chrono) / (double)CLOCKS_PER_SEC ;
if (detailed_printout) {
printf("%5.3f sec\t", elapsed) ;
fflush(stdout) ;
printf("med %g\n", (double)med[mednum]) ;
fflush(stdout) ;
} else {
printf("%5.3f\t", elapsed) ;
fflush(stdout) ;
}
mednum++ ;
/* benchmark torben's method */
memcpy(array, array_init, array_size * sizeof(pixelvalue)) ;
if (detailed_printout) {
printf("torben :\t") ;
fflush(stdout) ;
}
chrono = clock() ;
med[mednum] = torben(array, array_size) ;
elapsed = (double)(clock() - chrono) / (double)CLOCKS_PER_SEC ;
if (detailed_printout) {
printf("%5.3f sec\t", elapsed) ;
fflush(stdout) ;
printf("med %g\n", (double)med[mednum]) ;
fflush(stdout) ;
} else {
printf("%5.3f\t", elapsed) ;
fflush(stdout) ;
}
mednum++ ;
/* benchmark the eclipse fast pixel sort */
memcpy(array, array_init, array_size * sizeof(pixelvalue)) ;
if (detailed_printout) {
printf("fast pixel sort :\t") ;
fflush(stdout) ;
}
chrono = clock() ;
pixel_qsort(array, array_size) ;
elapsed = (double)(clock() - chrono) / (double)CLOCKS_PER_SEC ;
if (odd(array_size)) {
med[mednum] = array[array_size/2] ;
} else {
med[mednum] = array[(array_size/2) -1] ;
}
if (detailed_printout) {
printf("%5.3f sec\t", elapsed) ;
fflush(stdout) ;
printf("med %g\n", (double)med[mednum]) ;
fflush(stdout) ;
} else {
printf("%5.3f\t", elapsed) ;
fflush(stdout) ;
}
mednum++ ;
free(array) ;
free(array_init) ;
for (i=1 ; i<N_METHODS ; i++) {
if (med[i-1]!=med[i]) {
printf("diverging median values!\n") ;
fflush(stdout) ;
}
}
printf("\n") ;
fflush(stdout) ;
return ;
}
/**
* @brief This is the pressure control thread
* @param void* to a PID Loops configuration
* @retval msg_t status
*/
msg_t Pressure_Thread(void *This_Config) {
/* This thread is passed a pointer to a PID loop configuration */
PID_State Pressure_PID_Controllers[((Pressure_Config_Type*)This_Config)->Number_Setpoints];
memset(Pressure_PID_Controllers,0,((Pressure_Config_Type*)This_Config)->Number_Setpoints*sizeof(PID_State));/* Initialise as zeros */
float* Last_PID_Out=(float*)chHeapAlloc(NULL,sizeof(float)*((Pressure_Config_Type*)This_Config)->Number_Setpoints);/* PID output for interpol */
adcsample_t Pressure_Samples[PRESSURE_SAMPLES],Pressure_Sample;/* Use multiple pressure samples to drive down the noise */
float PID_Out,Pressure;//,step=0.01,sawtooth=0.7;
uint32_t Setpoint=0;
uint8_t Old_Setpoint=0, Previous_Setpoint;
chRegSetThreadName("PID_Pressure");
//palSetGroupMode(GPIOC, PAL_PORT_BIT(5) | PAL_PORT_BIT(4), 0, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOE, 9, PAL_MODE_ALTERNATE(1)); /* Only set the pin as AF output here, so as to avoid solenoid getting driven earlier*/
palSetPadMode(GPIOE, 11, PAL_MODE_ALTERNATE(1)); /* Experimental servo output here */
#ifndef USE_SERVO
/*
* Activates the PWM driver
*/
pwmStart(&PWM_Driver_Solenoid, &PWM_Config_Solenoid); /* Have to define the timer to use for PWM_Driver in hardware config */
/*
* Set the solenoid PWM to off
*/
pwmEnableChannel(&PWM_Driver_Solenoid, (pwmchannel_t)PWM_CHANNEL_SOLENOID, (pwmcnt_t)0);
#else
/*
* Activates the experimental servo driver
*/
pwmStart(&PWM_Driver_Servo, &PWM_Config_Servo); /* Have to define the timer to use for PWM_Driver in hardware config */
#endif
/*
* Activates the ADC2 driver *and the thermal sensor*.
*/
adcStart(&ADCD2, NULL);
//adcSTM32EnableTSVREFE();
/*
/ Now we run the sensor offset calibration loop
*/
do {
adcConvert(&ADCD2, &adcgrpcfg1, &Pressure_Sample, 1);/* This function blocks until it has one sample*/
} while(Calibrate_Sensor((uint16_t)Pressure_Sample));
systime_t time = chTimeNow(); /* T0 */
systime_t Interpolation_Timeout = time; /* Set to T0 to show there is no current interpolation */
/* Loop for the pressure control thread */
while(TRUE) {
/*
* Linear conversion.
*/
adcConvert(&ADCD2, &adcgrpcfg1, Pressure_Samples, PRESSURE_SAMPLES);/* This function blocks until it has the samples via DMA*/
/*
/ Now we process the data and apply the PID controller - we use a median filter to take out the non guassian noise
*/
Pressure_Sample = quick_select(Pressure_Samples, PRESSURE_SAMPLES);
Pressure = Convert_Pressure((uint16_t)Pressure_Sample);/* Converts to PSI as a float */
/* Retrieve a new setpoint from the setpoint mailbox, only continue if we get it*/
if(chMBFetch(&Pressures_Setpoint, (msg_t*)&Setpoint, TIME_IMMEDIATE) == RDY_OK) {
//Pressure=Run_Pressure_Filter(Pressure);/* Square root raised cosine filter for low pass with minimal lag */
Pressure = Pressure<0?0.0:Pressure; /* A negative pressure is impossible with current hardware setup - disregard*/
Setpoint &= 0x000000FF;
/* The controller is built around an interpolated array of independant PID controllers with seperate setpoints */
if(Setpoint != Old_Setpoint) { /* The setpoint has changed */
Previous_Setpoint = Old_Setpoint;/* This is for use by the interpolator */
Old_Setpoint = Setpoint; /* Store the setpoint */
/* Store the time at which the interpolation to new setpoint completes*/
Interpolation_Timeout = time + (systime_t)( 4.0 / ((Pressure_Config_Type*)This_Config)->Interpolation_Base );
}
if(Interpolation_Timeout > time) { /* If we have an ongoing interpolation - note, operates in tick units */
/* Value goes from 1 to -1 */
float interpol = erff( (float)(Interpolation_Timeout - time) *\
((Pressure_Config_Type*)This_Config)->Interpolation_Base - 2.0 );/* erf function interpolator */
interpol = ( (-interpol + 1.0) / 2.0);/* Interpolation value goes from 0 to 1 */
PID_Out = ( Last_PID_Out[Previous_Setpoint] * (1.0 - interpol) ) + ( Last_PID_Out[Setpoint] * interpol );
Pressure_PID_Controllers[Setpoint].Last_Input = Pressure;/* Make sure the input to next PID controller is continuous */
}
else {
PID_Out = Run_PID_Loop( ((Pressure_Config_Type*)This_Config)->PID_Loop_Config, &Pressure_PID_Controllers[Setpoint],\
(((Pressure_Config_Type*)This_Config)->Setpoints)[Setpoint], \
Pressure, (float)PRESSURE_TIME_INTERVAL/1000.0);/* Run PID */
Last_PID_Out[Setpoint] = PID_Out;/* Store for use by the interpolator */
}
}
else
PID_Out=0; /* So we can turn off the solenoid simply by failing to send Setpoints */
PID_Out=PID_Out>1.0?1.0:PID_Out;
PID_Out=PID_Out<0.0?0.0:PID_Out; /* Enforce range limits on the PID output */
//sawtooth+=step; /* Test code for debugging mechanics with a sawtooth */
//if(sawtooth>=1 || sawtooth<=0.65)
// step=-step;
//PID_Out=sawtooth;
#ifndef USE_SERVO
/*
/ Now we apply the PID output to the PWM based solenoid controller, and feed data into the mailbox output - Note fractional input
*/
pwmEnableChannel(&PWM_Driver_Solenoid, (pwmchannel_t)PWM_CHANNEL_SOLENOID, (pwmcnt_t)PWM_FRACTION_TO_WIDTH(&PWM_Driver_Solenoid, 1000\
, (uint32_t)(1000.0*PID_Out)));
#else
pwmEnableChannel(&PWM_Driver_Servo, (pwmchannel_t)PWM_CHANNEL_SERVO, (pwmcnt_t)PWM_FRACTION_TO_WIDTH(&PWM_Driver_Servo, 10000\
, (uint32_t)(1000.0*(PID_Out+1.0))));
#endif
chMBPost(&Pressures_Reported, *((msg_t*)&Pressure), TIME_IMMEDIATE);/* Non blocking write attempt to the Reported Pressure mailbox FIFO */
/*
/ The Thread is syncronised to system time
*/
time += MS2ST(PRESSURE_TIME_INTERVAL); /* Next deadline */
chThdSleepUntil(time); /* Gives us a thread with regular timing */
}
}
