Пример #1
0
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;
}
Пример #2
0
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 */
Пример #3
0
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;
    }
}
Пример #4
0
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;
}
Пример #5
0
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;
}
Пример #6
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
}
Пример #7
0
/* 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 */
Пример #8
0
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");
}
Пример #9
0
//
// 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;
}