Whisker_Seg *read_segments_whiskpoly1( FILE *file, int *n)
{ typedef struct {int id; int time; int len;} trunc_WSeg;
Whisker_Seg *wv;
int i;
static double *t = NULL;
static size_t t_size = 0;
*n = peek_whiskpoly1_footer(file); //read in number of whiskers
#ifdef DEBUG_WHISKER_IO_POLYFIT_READ
debug("Number of segments: %d\n",*n);
#endif
wv = (Whisker_Seg*) Guarded_Malloc( sizeof(Whisker_Seg)*(*n), "read whisker segments - format: whiskpoly1");
for( i=0; i<(*n); i++ )
{ Whisker_Seg *w = wv + i;
int j,len;
double px[WHISKER_IO_POLY_DEGREE+1],
py[WHISKER_IO_POLY_DEGREE+1];
float s;
float *x, *y, *thick, *scores;
fread( w, sizeof( trunc_WSeg ), 1, file ); //populates id,time (a.k.a frame id),len
len = w->len;
linspace_d( 0.0, 1.0, len, &t, &t_size );
x = w->x = (float*) Guarded_Malloc( sizeof(float)*(w->len), "read whisker segments (whiskpoly1 format)" );
y = w->y = (float*) Guarded_Malloc( sizeof(float)*(w->len), "read whisker segments (whiskpoly1 format)" );
thick = w->thick = (float*) Guarded_Malloc( sizeof(float)*(w->len), "read whisker segments (whiskpoly1 format)" );
scores = w->scores = (float*) Guarded_Malloc( sizeof(float)*(w->len), "read whisker segments (whiskpoly1 format)" );
fread( &s, sizeof(float), 1, file );
fread( px, sizeof(double), WHISKER_IO_POLY_DEGREE+1, file );
fread( py, sizeof(double), WHISKER_IO_POLY_DEGREE+1, file );
#ifdef DEBUG_WHISKER_IO_POLYFIT_READ
debug("Row: %d\n"
" fid:%5d wid:%5d len:%5d\n"
" Median score: %f\n"
" px[0] %5.5g px[end] %5.5g\n"
" py[0] %5.5g py[end] %5.5g\n"
,i,w->time, w->id, w->len,s
,px[0],px[WHISKER_IO_POLY_DEGREE]
,py[0],py[WHISKER_IO_POLY_DEGREE]
);
#endif
for( j=0; j<len; j++ )
{ x[j] = (float) polyval( px, WHISKER_IO_POLY_DEGREE, t[j] );
y[j] = (float) polyval( py, WHISKER_IO_POLY_DEGREE, t[j] );
thick[j] = 1.0;
scores[j] = s;
}
}
return wv;
}
int main()
{
const int NP = 8; // Number of given data points
const int M = 50; // number of computed data points
/* const int NC = 2 ; Number of coefficients */
double x[] = {1995, 1996, 1997, 1998, 1999, 2000, 2001, 2002};
double y[] = {5500, 8500, 13500, 20500, 29500, 40500, 53500, 68500};
double coeff[NC]; // vector of coefficients
int i;
double xc[M]; /* computed values for x using polynomial */
double yc[M];
printf("Program finds best fit polynomial of degree %d \n",
NC-1);
printf("Data points (x,y): \n");
for (i = 0; i < NP; i++)
printf(" %f %f \n", x[i], y[i]);
polynomialfit(NP, NC, x, y, coeff); /* find coefficients */
printf("\n\nCoefficients of polynomial found\n");
for(i=0; i < NC; i++) {
printf("%lf\n", coeff[i]);
}
/* Evaluate the fitted polynomial */
linspace(xc, 0.0, 12.0, M);
polyval(coeff, xc, yc, NC, M);
printf("\nData points calculated with the polynomial \n");
for(i=0; i < M; i++)
printf("%d %+.18f %+.18f \n", i, xc[i], yc[i]);
return 0;
} /* end main */
void
mdraw( /* draw to next point */
int x, int y
)
{
if (inpoly) {
polyval(x, y);
} else if (x != curx || y != cury) {
plseg(cura0, curx, cury, x, y);
curx = x;
cury = y;
}
}
char test_polyval()
{
double p[] = {1., 2., -3.};
double inputs[] = {0., 1., 2., 3.};
double expected[] = {-3., 0., 5., 12.};
double e = 0.00001;
int i=0;
printf("test_polyval... ");
for(i=0;i<4;i++)
if(!nearly_equal(expected[i], polyval(p,3,inputs[i]), e))
return false;
return true;
}
int main(int argc, char* argv[])
{ double p[10], *workspace;
workspace = polyfit_alloc_workspace( N, DEG );
polyfit( X, Y, N, DEG, p, workspace );
polyfit_free_workspace(workspace);
printf("--Expected--\n");
mat_print( P, DEG+1, 1 );
printf("-- Got --\n");
mat_print( p, DEG+1, 1 );
{ //do polyval check
int i;
double tol = 1e-6;
for(i=0; i<N; i++)
assert( fabs( Y[i] - polyval(p,DEG,X[i]) ) < tol );
}
return 0;
}
// Return control outputs based on references and feedback signals.
extern void set_actuators(struct control *controlData_ptr) {
// Enforce surface limits and apply calibration
dthr_cnts = polyval(dthr_cal, saturation(controlData_ptr->dthr,THROTTLE_MIN,THROTTLE_MAX),dthr_ord);
de_cnts = polyval(de_cal, saturation(controlData_ptr->de,ELEVATOR_MIN,ELEVATOR_MAX),de_ord);
da_cnts = polyval(da_cal, saturation(controlData_ptr->da,AILERON_MIN,AILERON_MAX),da_ord);
l1_cnts = polyval(l1_cal, saturation(controlData_ptr->l1,L1_MIN,L1_MAX),l1_ord);
r1_cnts = polyval(r1_cal, saturation(controlData_ptr->r1,R1_MIN,R1_MAX),r1_ord);
l4_cnts = polyval(l4_cal, saturation(controlData_ptr->l4,L4_MIN,L4_MAX), l4_ord);
r4_cnts = polyval(r4_cal, saturation(controlData_ptr->r4,R4_MIN,R4_MAX), r4_ord);
// Enforce absolute PWM limits for servos and write to mpc5200 PWM channels
GPT_PWM_Write_Width(PWMOUT_DTHR_CH, (uint16_t) saturation(dthr_cnts,PWMOUT_1MSEC,PWMOUT_2MSEC));// throttle
GPT_PWM_Write_Width(PWMOUT_DE_CH, (uint16_t) saturation(de_cnts,PWMOUT_1MSEC,PWMOUT_2MSEC)); // elevator
GPT_PWM_Write_Width(PWMOUT_DA_CH, (uint16_t) saturation(da_cnts,PWMOUT_1MSEC,PWMOUT_2MSEC)); // left aileron
GPT_PWM_Write_Width(PWMOUT_L1_CH, (uint16_t) saturation(l1_cnts,PWMOUT_1MSEC,PWMOUT_2MSEC)); // L1
GPT_PWM_Write_Width(PWMOUT_R1_CH, (uint16_t) saturation(r1_cnts,PWMOUT_1MSEC,PWMOUT_2MSEC)); // R1
GPT_PWM_Write_Width(PWMOUT_L4_CH, (uint16_t) saturation(l4_cnts,PWMOUT_1MSEC,PWMOUT_2MSEC)); // L4
GPT_PWM_Write_Width(PWMOUT_R4_CH, (uint16_t) saturation(r4_cnts,PWMOUT_1MSEC,PWMOUT_2MSEC)); // R4
}
/* const int N = 4; num of coefficients of polynomial */
int main () {
int i;
int j;
/* coefficients of Polynomial */
double constants[N] = {6.5, 0, 0, 0, 0, 3}; /* coefficients */
double xi, xf;
double x[M];
double y[M];
printf("Evaluating polynomial with coefficients: \n");
for (i=0; i < N; i++)
printf("%f ", constants[i]);
printf(" \n");
/* Evaluate the polynomial at the following points in x */
xi = -5.0; /* first value of x */
xf = 5.0; /* final value of x */
linspace(x, xi, xf, M); // vector of values for x
polyval(constants, x, y, N, M);
printf(" \n");
printf("Data points calculated of the polynomial x y \n");
for(j=0; j < M; j++)
printf("%+.10f %+.10f \n", x[j], y[j]);
return 0;
} /* end main */
void buildCovarianceTable(geoPoint2** locations, int n_locations, berkeleyAverageOptions* options, float** weights, int* n_weights, float* nugget)
{
tprintf("Begin of Build Covariance Table\n");
tprintf("%f %f %f\n", locations[0]->latitude, locations[0]->longitude, locations[0]->elevation);
// Precomputed monthly covariance information
real* p= options->correlationParameters;
int n_p= options->n_correlationParameters;
real maxd= options->correlationLimitDistance;
// Data locations
real* targ_x= rnalloc(n_locations);
real* targ_y= rnalloc(n_locations);
real* targ_z= rnalloc(n_locations);
int i, j, k;
for ( i= 0; i < n_locations; ++i )
{
targ_x[i]= locations[i]->x;
targ_y[i]= locations[i]->y;
targ_z[i]= locations[i]->z;
}
int n_targx, n_targy, n_targz;
n_targx= n_targy= n_targz= i;
// Eliminate station locations with no usable data -- original comment
// I cant see how the collapsing of three vectors to a matrix eliminates anything
real* R= rnalloc(n_targx*3);
for ( i= 0; i < n_targx; ++i )
{
R[i*3 + 0]= targ_x[i];
R[i*3 + 1]= targ_y[i];
R[i*3 + 2]= targ_z[i];
}
int lenR= n_targx;
float tmp1, tmp2, tmp3;
*weights= (float*)malloc(sizeof(float)*(lenR*lenR));
*n_weights= lenR;
for ( j= 0; j < lenR; ++j )
{
for ( i= 0; i < lenR; ++i )
{
tmp1= R[j*3 + 0] - R[i*3 + 0];
tmp1*= tmp1;
tmp2= R[j*3 + 1] - R[i*3 + 1];
tmp2*= tmp2;
tmp3= R[j*3 + 2] - R[i*3 + 2];
tmp3*= tmp3;
(*weights)[i*lenR + j]= sqrt(tmp1 + tmp2 + tmp3);
}
for ( i= 0; i < lenR; ++i )
if ( (*weights)[i*lenR + j] <= maxd )
(*weights)[i*lenR + j]= exp(polyval(p, n_p, (*weights)[i*lenR + j]));
else
(*weights)[i*lenR + j]= 0;
}
*nugget= 1.0 - exp(polyval(p, n_p, 0));
tprintf("End of Build Covariance Table\n");
}
//
// Measure Whisker Segment Features
// --------------------------------
// <face_axis> indicates the orientation of the mouse head with respect to
// the image.
// <face_axis> == 'x' --> horizontally (along x axis)
// <face_axis> == 'y' --> vertically (along y axis)
//
void Whisker_Seg_Measure( Whisker_Seg *w, double *dest, int facex, int facey, char face_axis )
{ float path_length, //
median_score, //
root_angle_deg, // side poly
mean_curvature, //(side) poly quad? (depends on side for sign)
follicle_x, // side
follicle_y, // side
tip_x, // side
tip_y; // side
float *x = w->x,
*y = w->y,
*s = w->scores;
int len = w->len,
idx_follicle,
idx_tip;
float dx;
static double *cumlen = NULL;
static size_t cumlen_size = 0;
cumlen = request_storage( cumlen, &cumlen_size, sizeof(double), len, "measure: cumlen");
cumlen[0] = 0.0;
// path length
// -----------
// XXX: an alternate approach would be to compute the polynomial fit
// and do quadrature on that. Might be more precise.
// Although, need cumlen (a.k.a cl) for polyfit anyway
{ float *ax = x + 1, *ay = y + 1,
*bx = x, *by = y;
double *cl = cumlen + 1, *clm = cumlen;
while( ax < x + len )
*cl++ = (*clm++) + hypotf( (*ax++) - (*bx++), (*ay++) - (*by++) );
path_length = cl[-1];
}
// median score
// ------------
{ qsort( s, len, sizeof(float), _score_cmp );
if(len&1) // odd
median_score = s[ (len-1)/2 ];
else //even
median_score = ( s[len/2 - 1] + s[len/2] )/2.0;
}
// Follicle and root positions
// ---------------------------
dx = _side( w, facex, facey, &idx_follicle, &idx_tip );
follicle_x = x[ idx_follicle ];
follicle_y = y[ idx_follicle ];
tip_x = x[ idx_tip ];
tip_y = y[ idx_tip ];
// Polynomial based measurements
// (Curvature and angle)
// -----------------------------
{ double px[ MEASURE_POLY_FIT_DEGREE+1 ],
py[ MEASURE_POLY_FIT_DEGREE+1 ],
xp[ MEASURE_POLY_FIT_DEGREE+1 ],
yp[ MEASURE_POLY_FIT_DEGREE+1 ],
xpp[ MEASURE_POLY_FIT_DEGREE+1 ],
ypp[ MEASURE_POLY_FIT_DEGREE+1 ],
mul1[ 2*MEASURE_POLY_FIT_DEGREE ],
mul2[ 2*MEASURE_POLY_FIT_DEGREE ],
num[ 2*MEASURE_POLY_FIT_DEGREE ],
den[ 2*MEASURE_POLY_FIT_DEGREE ];
static double *t = NULL;
static size_t t_size = 0;
static double *xd = NULL;
static size_t xd_size = 0;
static double *yd = NULL;
static size_t yd_size = 0;
static double *workspace = NULL;
static size_t workspace_size = 0;
int i;
const int pad = MIN( MEASURE_POLY_END_PADDING, len/4 );
// parameter for parametric polynomial representation
t = request_storage(t, &t_size, sizeof(double), len, "measure");
xd = request_storage(xd, &xd_size, sizeof(double), len, "measure");
yd = request_storage(yd, &yd_size, sizeof(double), len, "measure");
{ int i = len; // convert floats to doubles
while(i--)
{ xd[i] = x[i];
yd[i] = y[i];
}
}
for( i=0; i<len; i++ )
t[i] = cumlen[i] / path_length; // [0 to 1]
#ifdef DEBUG_MEASURE_POLYFIT_ERROR
assert(t[0] == 0.0 );
assert( (t[len-1] - 1.0)<1e-6 );
#endif
// polynomial fit
workspace = request_storage( workspace,
&workspace_size,
sizeof(double),
polyfit_size_workspace( len, 2*MEASURE_POLY_FIT_DEGREE ), //need 2*degree for curvature eval later
"measure: polyfit workspace" );
polyfit( t+pad, xd+pad, len-2*pad, MEASURE_POLY_FIT_DEGREE, px, workspace );
polyfit_reuse( yd+pad, len-2*pad, MEASURE_POLY_FIT_DEGREE, py, workspace );
#ifdef DEBUG_MEASURE_POLYFIT_ERROR
{ double err = 0.0;
int i;
for( i=pad; i<len-2*pad; i++ )
err += hypot( xd[i] - polyval( px, MEASURE_POLY_FIT_DEGREE, t[i] ),
yd[i] - polyval( py, MEASURE_POLY_FIT_DEGREE, t[i] ) );
err /= ((float)len);
debug("Polyfit root mean squared residual: %f\n", err );
assert( err < 1.0 );
}
#endif
// first derivative
memcpy( xp, px, sizeof(double) * ( MEASURE_POLY_FIT_DEGREE+1 ) );
memcpy( yp, py, sizeof(double) * ( MEASURE_POLY_FIT_DEGREE+1 ) );
polyder_ip( xp, MEASURE_POLY_FIT_DEGREE+1, 1 );
polyder_ip( yp, MEASURE_POLY_FIT_DEGREE+1, 1 );
// second derivative
memcpy( xpp, xp, sizeof(double) * ( MEASURE_POLY_FIT_DEGREE+1 ) );
memcpy( ypp, yp, sizeof(double) * ( MEASURE_POLY_FIT_DEGREE+1 ) );
polyder_ip( xpp, MEASURE_POLY_FIT_DEGREE+1, 1 );
polyder_ip( ypp, MEASURE_POLY_FIT_DEGREE+1, 1 );
// Root angle
// ----------
{ double teval = (idx_follicle == 0) ? t[pad] : t[len-pad-1];
static const double rad2deg = 180.0/M_PI;
switch(face_axis)
{ case 'h':
case 'x':
root_angle_deg = atan2( dx*polyval(yp, MEASURE_POLY_FIT_DEGREE, teval ),
dx*polyval(xp, MEASURE_POLY_FIT_DEGREE, teval ) ) * rad2deg;
break;
case 'v':
case 'y':
root_angle_deg = atan2( dx*polyval(xp, MEASURE_POLY_FIT_DEGREE, teval ),
dx*polyval(yp, MEASURE_POLY_FIT_DEGREE, teval ) ) * rad2deg;
break;
default:
error("In Whisker_Seg_Measure\n"
"\tParameter <face_axis> must take on a value of 'x' or 'y'\n"
"\tGot value %c\n",face_axis);
}
}
// Mean curvature
// --------------
// Use the most naive of integration schemes
{ double *V = workspace; // done with workspace, so reuse it for vandermonde matrix (just alias it here)
static double *evalnum = NULL,
*evalden = NULL;
static size_t evalnum_size = 0,
evalden_size = 0;
size_t npoints = len-2*pad;
evalnum = request_storage( evalnum, &evalnum_size, sizeof(double), npoints, "numerator" );
evalden = request_storage( evalden, &evalden_size, sizeof(double), npoints, "denominator" );
Vandermonde_Build( t+pad, npoints, 2*MEASURE_POLY_FIT_DEGREE, V ); // used for polynomial evaluation
// numerator
memset( mul1, 0, 2*MEASURE_POLY_FIT_DEGREE*sizeof(double) );
memset( mul2, 0, 2*MEASURE_POLY_FIT_DEGREE*sizeof(double) );
polymul( xp, MEASURE_POLY_FIT_DEGREE+1,
ypp, MEASURE_POLY_FIT_DEGREE+1,
mul1 );
polymul( yp, MEASURE_POLY_FIT_DEGREE+1,
xpp, MEASURE_POLY_FIT_DEGREE+1,
mul2 );
polysub( mul1, 2*MEASURE_POLY_FIT_DEGREE,
mul2, 2*MEASURE_POLY_FIT_DEGREE,
num );
// denominator
memset( mul1, 0, 2*MEASURE_POLY_FIT_DEGREE*sizeof(double) );
memset( mul2, 0, 2*MEASURE_POLY_FIT_DEGREE*sizeof(double) );
polymul( xp, MEASURE_POLY_FIT_DEGREE+1,
xp, MEASURE_POLY_FIT_DEGREE+1,
mul1 );
polymul( yp, MEASURE_POLY_FIT_DEGREE+1,
yp, MEASURE_POLY_FIT_DEGREE+1,
mul2 );
polyadd( mul1, 2*MEASURE_POLY_FIT_DEGREE,
mul2, 2*MEASURE_POLY_FIT_DEGREE,
den );
// Eval
matmul( V, npoints, MEASURE_POLY_FIT_DEGREE*2,
num, MEASURE_POLY_FIT_DEGREE*2, 1,
evalnum );
matmul( V, npoints, MEASURE_POLY_FIT_DEGREE*2,
den, MEASURE_POLY_FIT_DEGREE*2, 1,
evalden );
// compute kappa at each t
{ int i;
for(i=0; i<npoints; i++ )
evalnum[i] /= pow( evalden[i], 3.0/2.0 )*dx; //dx is 1 or -1 so dx = 1/dx;
mean_curvature = evalnum[0] * (t[1]-t[0]);
for(i=1; i<npoints; i++ )
mean_curvature += evalnum[i] * ( t[i]-t[i-1] );
}
}
}
// fill in fields
dest[0] = path_length;
dest[1] = median_score;
dest[2] = root_angle_deg;
dest[3] = mean_curvature;
dest[4] = follicle_x;
dest[5] = follicle_y;
dest[6] = tip_x;
dest[7] = tip_y;
}