void EyeRover::calibrate(){
std::cout << "Starting calibration procedure, EyeRover must be static" << endl;
std::cout << "Procedure should take about a second" << endl;
m_calibrated = false;
std::vector<std::vector<float> > _gyros_vals(NUMBER_OF_STEPS_FOR_CALIBRATION);
std::vector<std::vector<float> > _accs_vals(NUMBER_OF_STEPS_FOR_CALIBRATION);
read_gyroscope();
read_accelerometer();
read_magnetometer();
usleep(500000); //Sleep for half a second
for (int i=0; i<NUMBER_OF_STEPS_FOR_CALIBRATION; ++i){
_gyros_vals[i] = read_gyroscope();
_accs_vals[i] = read_accelerometer();
}
m_gyro_avg = get_mean(_gyros_vals);
m_accelerometer_avg = get_mean(_accs_vals);
if (get_std(_gyros_vals, 0, m_gyro_avg[0])> 0.01){
m_calibrated = false;
std::cout << "Calibration failed." << endl;
}
else{
m_calibrated = true;
std::cout << "Calibration succeeded." << endl;
}
}
double get_sigma(const std::vector<double>& iter) {
const double mean = get_mean(iter);
double sum = 0;
for (auto v=iter.begin(); v!=iter.end(); ++v)
sum += std::pow(*v - mean, 2.);
return std::sqrt(sum/(iter.size()-1));
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - |
// copula for BDMCMC sampling algorithm
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - |
void copula( double Z[], double K[], int R[], int not_continuous[], int *n, int *p )
{
int number = *n, dim = *p, nxp = number * dim, dimp1 = dim + 1;
#pragma omp parallel
{
double sigma, sd_j, mu_ij, lb, ub, runif_value, pnorm_lb, pnorm_ub;
int i, j;
#pragma omp for
for( int counter = 0; counter < nxp; counter++ )
{
j = counter / number;
i = counter % number;
if( not_continuous[ j ] )
{
sigma = 1.0 / K[ j * dimp1 ]; // 1.0 / K[ j * dim + j ];
sd_j = sqrt( sigma );
get_mean( Z, K, &mu_ij, &sigma, &i, &j, &number, &dim );
get_bounds_NA( Z, R, &lb, &ub, &i, &j, &number );
pnorm_lb = Rf_pnorm5( lb, mu_ij, sd_j, TRUE, FALSE );
pnorm_ub = Rf_pnorm5( ub, mu_ij, sd_j, TRUE, FALSE );
//runif_value = runif( pnorm_lb, pnorm_ub );
runif_value = pnorm_lb + unif_rand() * ( pnorm_ub - pnorm_lb );
Z[ counter ] = Rf_qnorm5( runif_value, mu_ij, sd_j, TRUE, FALSE );
}
}
}
}
MDArray<double> Shrinkage::get_target_B()
{
MDArray<double> target(vec(P,P));
double avg=get_mean(get_diag(S_MLE));
for(int i=0;i<P;i++)
for(int j=0;j<P;j++)
if (i==j)
target.set(i,i,avg);
return target;
}
int
update_stats(struct B * b, struct BenchmarkResult * result) {
result->ns_mean = get_mean(&b->samples[0], b->n);
result->ns_median = get_median(&b->samples[0], b->n);
result->s_mean = 0;
result->s_median = 0;
return B_SUCCESS;
}
int get_mean_on_table(void * db, char * table_name, char * data)
{
destroy_list_nbrs(MATH_LIST);
MATH_LIST = create_list_nbrs();
search_DB(db, table_name, data, math_callback);
int mean = get_mean(MATH_LIST);
return mean;
}
float mahal(gsl_vector* x, gsl_matrix* m, gsl_matrix* covm) {
gsl_vector* mean = gsl_vector_calloc(m->size2);
// calculate mean vector
get_mean(m,mean);
float d = 0.0;
// cov can be used
if (m->size2 > 1) {
SEXP xr, meanr, covmr;
//gsl_matrix* covm = gsl_matrix_calloc(m->size2, m->size2);
// calculate covariance
//cov(m,covm);
// set x vector
PROTECT(xr = allocVector(REALSXP, x->size));
for (unsigned int i=0; i<x->size; i++) REAL(xr)[i] = gsl_vector_get(x,i);
// set mean vector
PROTECT(meanr = allocVector(REALSXP, mean->size));
for (unsigned int i=0; i<x->size; i++) REAL(meanr)[i] = gsl_vector_get(mean,i);
// set cov matrix
PROTECT(covmr = allocMatrix(REALSXP, covm->size1, covm->size2));
for (unsigned int i = 0; i < covm->size1; i++) {
for (unsigned int j = 0; j < covm->size2; j++) {
REAL(covmr)[i+(covm->size1)*j] = gsl_matrix_get(covm, i, j);
}
}
// R CALC
SEXP dist;
PROTECT(dist = R_exec4("mahalanobis", xr, meanr, covmr));
d = sqrt(REAL(dist)[0]);
UNPROTECT(4);
//gsl_matrix_free(covm);
}
// cov can not be used -- use euclidean distance for 1-D
else {
d = fabs(gsl_vector_get(x,0) - gsl_vector_get(mean,0));
}
gsl_vector_free(mean);
return d;
}
double get_variance(struct List_Nbrs * l)
{
int i = 0;
double a = 0;
double sum = 0;
double mean = get_mean(l);
for (i = 0; i < l->nb_elt; i++)
{
a = l->nbrs[i] - mean;
a *= a;
sum += a;
}
return sum/l->nb_elt;
}
int main(void)
{
int size = SIZE_ARRAY;
int Array[SIZE_ARRAY];
double res;
load_array(size, Array);
show_array(size, Array);
res = get_mean(size, Array);
printf("%.3f\n", res);
return (0);
}
double Shrinkage::get_lambda_C()
{
double sum_up=0,sum_d=0,avg_diag=get_mean(get_diag(S_MLE)),avg_off_diag(get_mean(get_off_diag(S_MLE)));
generate_wk(data,0);
for(int i=0;i<P;i++)
for(int j=0;j<P;j++)
if (i!=j){
sum_up+=w_var(wk,i,j);
sum_d+=(S_MLE.get(i,j)-avg_off_diag)*(S_MLE.get(i,j)-avg_off_diag);
}
else{
sum_up+=w_var(wk,i,i);
sum_d+=(S_MLE.get(i,i)-avg_diag)*(S_MLE.get(i,i)-avg_diag);
}
return round_lambda(sum_up/sum_d);
}
box box_factory::get_stddev(vector<box> q)
{
box mean = get_mean(q);
map<string, capd::interval> f_map = mean.get_map();
// setting the initial box
map<string, capd::interval> init_map, div_map;
for(auto it = f_map.cbegin(); it != f_map.cend(); it++)
{
init_map.insert(make_pair(it->first, capd::interval(0.0)));
div_map.insert(make_pair(it->first, capd::interval(q.size())));
}
box sum(init_map), div(div_map);
for(box b : q)
{
sum = sum + (b.get_mean() - mean) * (b.get_mean() - mean);
}
return box_factory::sqrt(sum/div);
}
void calc_forcing_stats(int Nrecs,
atmos_data_struct *atmos, const int NR) {
/**********************************************************************
calc_forcing_stats.c Keith Cherkauer November 16, 2000
This routine finds the maximum, minimum and mean values for each
data type. Results are output to stdout for inclusion in screen or
log file output. These statistics are meant only to help the user
identify possible problems in their input forcing data and are not
an exhaustive study of that data.
**********************************************************************/
double *values = (double *) calloc ( Nrecs, sizeof(double) );
printf("Variable\tMean\tStd. Dev.\tSum\tMaximum\tMinimum\n");
/** Air Temperature **/
for (int rec = 0; rec < Nrecs; rec ++ )
values[rec] = atmos[rec].air_temp[NR];
printf("air temp (C):\t%f\t%f\t%f\t%f\t%f\n",
get_mean(values, Nrecs, INVALID),
get_stdev(values, Nrecs, get_mean(values, Nrecs, INVALID), INVALID),
get_sum(values, Nrecs, INVALID), get_max(values, Nrecs, INVALID),
get_min(values, Nrecs, INVALID) );
/** Density **/
for (int rec = 0; rec < Nrecs; rec ++ )
values[rec] = atmos[rec].density[NR];
printf("Density (kg/m^3):\t%f\t%f\t%f\t%f\t%f\n",
get_mean(values, Nrecs, INVALID),
get_stdev(values, Nrecs, get_mean(values, Nrecs, INVALID), INVALID),
get_sum(values, Nrecs, INVALID), get_max(values, Nrecs, INVALID),
get_min(values, Nrecs, INVALID) );
/** Longwave **/
for (int rec = 0; rec < Nrecs; rec ++ )
values[rec] = atmos[rec].longwave[NR];
printf("Longwave (W/m^2):\t%f\t%f\t%f\t%f\t%f\n",
get_mean(values, Nrecs, INVALID),
get_stdev(values, Nrecs, get_mean(values, Nrecs, INVALID), INVALID),
get_sum(values, Nrecs, INVALID), get_max(values, Nrecs, INVALID),
get_min(values, Nrecs, INVALID) );
/** Precipitation **/
for (int rec = 0; rec < Nrecs; rec ++ )
values[rec] = atmos[rec].prec[NR];
printf("Precip (mm):\t%f\t%f\t%f\t%f\t%f\n",
get_mean(values, Nrecs, INVALID),
get_stdev(values, Nrecs, get_mean(values, Nrecs, INVALID), INVALID),
get_sum(values, Nrecs, INVALID), get_max(values, Nrecs, INVALID),
get_min(values, Nrecs, INVALID) );
/** Pressure **/
for (int rec = 0; rec < Nrecs; rec ++ )
values[rec] = atmos[rec].pressure[NR];
printf("Pressure (Pa):\t%f\t%f\t%f\t%f\t%f\n",
get_mean(values, Nrecs, INVALID),
get_stdev(values, Nrecs, get_mean(values, Nrecs, INVALID), INVALID),
get_sum(values, Nrecs, INVALID), get_max(values, Nrecs, INVALID),
get_min(values, Nrecs, INVALID) );
/** Shortwave **/
for (int rec = 0; rec < Nrecs; rec ++ )
values[rec] = atmos[rec].shortwave[NR];
printf("Shortwave (W/m^2):\t%f\t%f\t%f\t%f\t%f\n",
get_mean(values, Nrecs, INVALID),
get_stdev(values, Nrecs, get_mean(values, Nrecs, INVALID), INVALID),
get_sum(values, Nrecs, INVALID), get_max(values, Nrecs, INVALID),
get_min(values, Nrecs, INVALID) );
/** Vapor Pressure **/
for (int rec = 0; rec < Nrecs; rec ++ )
values[rec] = atmos[rec].vp[NR];
printf("vp (Pa):\t%f\t%f\t%f\t%f\t%f\n",
get_mean(values, Nrecs, INVALID),
get_stdev(values, Nrecs, get_mean(values, Nrecs, INVALID), INVALID),
get_sum(values, Nrecs, INVALID), get_max(values, Nrecs, INVALID),
get_min(values, Nrecs, INVALID) );
/** Vapor Pressure Deficit **/
for (int rec = 0; rec < Nrecs; rec ++ )
values[rec] = atmos[rec].vpd[NR];
printf("vpd (Pa):\t%f\t%f\t%f\t%f\t%f\n",
get_mean(values, Nrecs, INVALID),
get_stdev(values, Nrecs, get_mean(values, Nrecs, INVALID), INVALID),
get_sum(values, Nrecs, INVALID), get_max(values, Nrecs, INVALID),
get_min(values, Nrecs, INVALID) );
/** Wind Speed **/
for (int rec = 0; rec < Nrecs; rec ++ )
values[rec] = atmos[rec].wind[NR];
printf("wind speed (m/s):\t%f\t%f\t%f\t%f\t%f\n",
get_mean(values, Nrecs, INVALID),
get_stdev(values, Nrecs, get_mean(values, Nrecs, INVALID), INVALID),
get_sum(values, Nrecs, INVALID), get_max(values, Nrecs, INVALID),
get_min(values, Nrecs, INVALID) );
fflush(stdout);
free(values);
}
int main (void)
{
cc3_histogram_pkt_t my_hist;
cc3_color_info_pkt_t s_pkt;
cc3_frame_diff_pkt_t fd_pkt;
cc3_timer_wait_ms (500);
cc3_gpio_set_mode (0, CC3_GPIO_MODE_SERVO);
cc3_gpio_set_mode (1, CC3_GPIO_MODE_SERVO);
cc3_gpio_set_mode (2, CC3_GPIO_MODE_SERVO);
cc3_gpio_set_mode (3, CC3_GPIO_MODE_SERVO);
// configure uarts
cc3_uart_init (0,
CC3_UART_RATE_115200,
CC3_UART_MODE_8N1,
CC3_UART_BINMODE_TEXT);
cc3_uart_init (1,
CC3_UART_RATE_38400,//CC3_UART_RATE_115200,
CC3_UART_MODE_8N1,
CC3_UART_BINMODE_BINARY);
// Make it so that stdout and stdin are not buffered
setvbuf (stdout, NULL, _IONBF, 0);
setvbuf (stdin, NULL, _IONBF, 0);
printf( "Opening UART1 file pointer\n" );
FILE *fp = cc3_uart_fopen(1, "r");
if (fp)
{
printf("Success");
int i = 0;
while (1)
{
fprintf(fp, "i = %08d\n", i++);
}
}
printf( "Calling camera init\n" );
cc3_camera_init ();
cc3_camera_set_colorspace (CAM_COLOURS);
cc3_camera_set_resolution (CAM_FORMAT);
cc3_pixbuf_frame_set_coi (CC3_CHANNEL_ALL);//for full 'colour_info'
//cc3_camera_set_colorspace (CC3_COLORSPACE_YCRCB);?All switches handled?
//cc3_camera_set_resolution (CC3_CAMERA_RESOLUTION_HIGH);// 352, 288
//cc3_pixbuf_frame_set_subsample(CC3_SUBSAMPLE_RANDOM, 2, 2);
printf( "Camera init done\n%d x %d\n",
cc3_g_pixbuf_frame.raw_width, cc3_g_pixbuf_frame.raw_height );
// frame difference
fd_pkt.coi = CC3_CHANNEL_ALL;
fd_pkt.template_width = 16;//8;
fd_pkt.template_height = 16;//8;
fd_pkt.total_x = cc3_g_pixbuf_frame.width;
fd_pkt.total_y = cc3_g_pixbuf_frame.height;
fd_pkt.load_frame = 1; // load a new frame
fd_pkt.previous_template = malloc (fd_pkt.template_width * fd_pkt.template_height * sizeof (uint32_t));
if (fd_pkt.previous_template == NULL)
printf ("Malloc FD startup error!\r");
cc3_camera_set_auto_white_balance (true);
cc3_camera_set_auto_exposure (true);
// The LED test masks the stabilization delays (~2000ms)
printf ("Waiting for image to stabilize\n");
led_test ();
cc3_camera_set_auto_white_balance (false);
cc3_camera_set_auto_exposure (false);
// printf ("\nPush button on camera back to continue\n");
// while (!cc3_button_get_state ())
// ;
cc3_led_set_state (0, true);
cc3_pixbuf_load ();
my_hist.channel = CC3_CHANNEL_ALL;
my_hist.bins = 24;
my_hist.hist = malloc (my_hist.bins * sizeof (uint32_t));
while (true) {
printf ("<3 EE\n 0x%02X\n ", (unsigned int)cc3_timer_get_current_ms());
// Grab an image and take a frame difference of it
cc3_pixbuf_load ();
frame_diff (&fd_pkt);
// Rewind and take a histogram of it
cc3_pixbuf_rewind ();
//get_histogram (&my_hist);
// Rewind and get some stats
cc3_pixbuf_rewind ();
get_mean(&s_pkt);
printf( "min = [%d,%d,%d] mean = [%d,%d,%d] max = [%d,%d,%d] deviation = [%d,%d,%d] ",
s_pkt.min.channel[0],s_pkt.min.channel[1],s_pkt.min.channel[2],
s_pkt.mean.channel[0],s_pkt.mean.channel[1],s_pkt.mean.channel[2],
s_pkt.max.channel[0],s_pkt.max.channel[1],s_pkt.max.channel[2],
s_pkt.deviation.channel[0],s_pkt.deviation.channel[1],s_pkt.deviation.channel[2]
);
/* printf ("hist[%d] = ", my_hist.bins);
for (uint32_t i = 0; i < my_hist.bins; i++)
{
printf ("%08X ", my_hist.hist[i]);
// sample non-blocking serial routine
if (!cc3_uart_has_data (1))
{
cc3_gpio_set_servo_position (0, SERVO_MID);
cc3_gpio_set_servo_position (1, SERVO_MID);
}
}
*/
printf ("\n");
cc3_timer_wait_ms(400);
// if (cc3_button_get_state())
// break;
}
printf("\n\nAll done!\n");
return 0;
}
/**
* Get divergence from optical flow vectors based on line sizes between corners
* @param[in] vectors The optical flow vectors
* @param[in] count The number of optical flow vectors
* @param[in] n_samples The number of line segments that will be taken into account. 0 means all line segments will be considered.
* @return divergence
*/
float get_size_divergence(struct flow_t *vectors, int count, int n_samples)
{
float distance_1;
float distance_2;
float *divs;
unsigned int sample;
float dx;
float dy;
float mean_divergence;
int n_elements;
unsigned int i, j;
// TODO: if count!/(2!*count-2!) lower than n_samples, do full sampling
if (count < 2) {
return NO_DIV;
}
if (n_samples == 0) {
// divs will contain the individual divergence estimates:
n_elements = (count * count - count) / 2;
divs = (float *) malloc(sizeof(float) * n_elements);
// go through all possible lines:
sample = 0;
for (i = 0; i < count; i++) {
for (j = i + 1; j < count; j++) {
// distance in previous image:
dx = (float)vectors[i].pos.x - (float)vectors[j].pos.x;
dy = (float)vectors[i].pos.y - (float)vectors[j].pos.y;
distance_1 = sqrt(dx * dx + dy * dy);
// distance in current image:
dx = (float)vectors[i].pos.x + (float)vectors[i].flow_x - (float)vectors[j].pos.x - (float)vectors[j].flow_x;
dy = (float)vectors[i].pos.y + (float)vectors[i].flow_y - (float)vectors[j].pos.y - (float)vectors[j].flow_y;
distance_2 = sqrt(dx * dx + dy * dy);
// calculate divergence for this sample:
divs[sample] = (distance_2 - distance_1) / distance_1;
sample++;
}
}
// calculate the mean divergence:
mean_divergence = get_mean(divs, n_elements);
// free the memory of divs:
free(divs);
} else {
// vector that will contain individual divergence estimates:
divs = (float *) malloc(sizeof(float) * n_samples);
// take random samples:
for (sample = 0; sample < n_samples; sample++) {
// take two random indices:
i = rand() % count;
j = rand() % count;
// ensure it is not the same index:
while (i == j) {
j = rand() % count;
}
// distance in previous image:
dx = (float)vectors[i].pos.x - (float)vectors[j].pos.x;
dy = (float)vectors[i].pos.y - (float)vectors[j].pos.y;
distance_1 = sqrt(dx * dx + dy * dy);
// distance in current image:
dx = (float)vectors[i].pos.x + (float)vectors[i].flow_x - (float)vectors[j].pos.x - (float)vectors[j].flow_x;
dy = (float)vectors[i].pos.y + (float)vectors[i].flow_y - (float)vectors[j].pos.y - (float)vectors[j].flow_y;
distance_2 = sqrt(dx * dx + dy * dy);
// calculate divergence for this sample:
divs[sample] = (distance_2 - distance_1) / distance_1;
}
// calculate the mean divergence:
mean_divergence = get_mean(divs, n_samples);
// free the memory of divs:
free(divs);
}
// return the calculated divergence:
return mean_divergence;
}
void fill_csv(Image::Pointer im1, Image::Pointer im2, Image::Pointer label, const char *output_excel)
{
CsvWriter csv = CsvWriter(QString(output_excel));
csv.insert(2,1,QString("T1"));
csv.insert(2,2,QString("WM"));
csv.insert(3,2,QString("GM"));
csv.insert(4,2,QString("CSF"));
csv.insert(1,3,QString("Volume"));
csv.insert(1,4,QString("Mean"));
csv.insert(1,5,QString("Min"));
csv.insert(1,6,QString("Max"));
csv.insert(7,1,QString("T2"));
csv.insert(7,2,QString("WM"));
csv.insert(8,2,QString("GM"));
csv.insert(9,2,QString("CSF"));
csv.insert(6,3,QString("Volume"));
csv.insert(6,4,QString("Mean"));
csv.insert(6,5,QString("Min"));
csv.insert(6,6,QString("Max"));
// White Matter
std::cout<<"White Matter statistics"<<std::endl;
Image::Pointer wm1 = binary_and(im1, label2mask(label,1));
unsigned int min = get_min(wm1);
unsigned int max = get_max(wm1);
float mean = get_mean(wm1);
unsigned int volume = get_volume(wm1);
csv.insert(2,3,QString("%1").arg(volume));
csv.insert(2,4,QString("%1").arg(mean));
csv.insert(2,5,QString("%1").arg(min));
csv.insert(2,6,QString("%1").arg(max));
Image::Pointer wm2 = binary_and(im2, label2mask(label,1));
min = get_min(wm2);
max = get_max(wm2);
mean = get_mean(wm2);
volume = get_volume(wm2);
csv.insert(7,3,QString("%1").arg(volume));
csv.insert(7,4,QString("%1").arg(mean));
csv.insert(7,5,QString("%1").arg(min));
csv.insert(7,6,QString("%1").arg(max));
// Gray Matter
std::cout<<"Gray Matter statistics"<<std::endl;
Image::Pointer gm1 = binary_and(im1, label2mask(label,2));
min = get_min(gm1);
max = get_max(gm1);
mean = get_mean(gm1);
volume = get_volume(gm1);
csv.insert(3,3,QString("%1").arg(volume));
csv.insert(3,4,QString("%1").arg(mean));
csv.insert(3,5,QString("%1").arg(min));
csv.insert(3,6,QString("%1").arg(max));
Image::Pointer gm2 = binary_and(im2, label2mask(label,2));
min = get_min(gm2);
max = get_max(gm2);
mean = get_mean(gm2);
volume = get_volume(gm2);
csv.insert(8,3,QString("%1").arg(volume));
csv.insert(8,4,QString("%1").arg(mean));
csv.insert(8,5,QString("%1").arg(min));
csv.insert(8,6,QString("%1").arg(max));
// CSF
std::cout<<"CSF statistics"<<std::endl;
Image::Pointer csf1 = binary_and(im1, label2mask(label,3));
min = get_min(csf1);
max = get_max(csf1);
mean = get_mean(csf1);
volume = get_volume(csf1);
csv.insert(4,3,QString("%1").arg(volume));
csv.insert(4,4,QString("%1").arg(mean));
csv.insert(4,5,QString("%1").arg(min));
csv.insert(4,6,QString("%1").arg(max));
Image::Pointer csf2 = binary_and(im2, label2mask(label,3));
min = get_min(csf2);
max = get_max(csf2);
mean = get_mean(csf2);
volume = get_volume(csf2);
csv.insert(9,3,QString("%1").arg(volume));
csv.insert(9,4,QString("%1").arg(mean));
csv.insert(9,5,QString("%1").arg(min));
csv.insert(9,6,QString("%1").arg(max));
std::cout<<"Updating cvs file"<<std::endl;
csv.update();
std::cout<<"cvs file updated"<<std::endl;
}
