Exemplo n.º 1
0
void MMCollisionInt::fit(int degree, doublereal deltastar,
                         doublereal* a, doublereal* b, doublereal* c)
{
    int i, n = m_nmax - m_nmin + 1;
    int ndeg=0;
    vector_fp values(n);
    doublereal rmserr;
    vector_fp w(n);
    doublereal* logT = DATA_PTR(m_logTemp) + m_nmin;
    for (i = 0; i < n; i++) {
        if (deltastar == 0.0) {
            values[i] = astar_table[8*(i + m_nmin + 1)];
        } else {
            values[i] = poly5(deltastar, DATA_PTR(m_apoly[i+m_nmin]));
        }
    }
    w[0]= -1.0;
    rmserr = polyfit(n, logT, DATA_PTR(values),
                     DATA_PTR(w), degree, ndeg, 0.0, a);

    for (i = 0; i < n; i++) {
        if (deltastar == 0.0) {
            values[i] = bstar_table[8*(i + m_nmin + 1)];
        } else {
            values[i] = poly5(deltastar, DATA_PTR(m_bpoly[i+m_nmin]));
        }
    }
    w[0]= -1.0;
    rmserr = polyfit(n, logT, DATA_PTR(values),
                     DATA_PTR(w), degree, ndeg, 0.0, b);

    for (i = 0; i < n; i++) {
        if (deltastar == 0.0) {
            values[i] = cstar_table[8*(i + m_nmin + 1)];
        } else {
            values[i] = poly5(deltastar, DATA_PTR(m_cpoly[i+m_nmin]));
        }
    }
    w[0]= -1.0;
    rmserr = polyfit(n, logT, DATA_PTR(values),
                     DATA_PTR(w), degree, ndeg, 0.0, c);
    if (DEBUG_MODE_ENABLED && m_loglevel > 2) {
        writelogf("\nT* fit at delta* = %.6g\n", deltastar);

        writelog("astar = [" + vec2str(vector_fp(a, a+degree+1))+ "]\n");
        if (rmserr > 0.01) {
            writelogf("Warning: RMS error = %12.6g for A* fit\n", rmserr);
        }

        writelog("bstar = [" + vec2str(vector_fp(b, b+degree+1))+ "]\n");
        if (rmserr > 0.01) {
            writelogf("Warning: RMS error = %12.6g for B* fit\n", rmserr);
        }

        writelog("cstar = [" + vec2str(vector_fp(c, c+degree+1))+ "]\n");
        if (rmserr > 0.01) {
            writelogf("Warning: RMS error = %12.6g for C* fit\n", rmserr);
        }
    }
}
Exemplo n.º 2
0
bool CAWSFile::CalculateCoefficient(AWS_Setting &set, AWS_CalCo &co)
{
	int n = 0;
	for(int i=0;i<10;i++)
	{
		if (n==0 && (_isnan(set.sample.dA_ratio[i]) || _isnan(set.sample.dB_ratio[i])))
		{
			n = i;
		}

		co.cal.dA_Eff[i] = set.sample.dA_CPM[i] / set.nA_DPM;
		co.cal.dB_Eff[i] = set.sample.dB_CPM[i] / set.nB_DPM;
		co.cal.dBA_CPM[i]= set.sample.dB_CPM[i]/set.sample.dB_A_CPM[i];
	}

	if (n==0) n = 10;
	if (n < 4) return false;
	try
	{
		polyfit(set.sample.dA_ratio,co.cal.dA_Eff,co.dAch_co,n,4);
		polyfit(set.sample.dB_ratio,co.cal.dB_Eff,co.dBch_co,n,4);
		polyfit(set.sample.dB_ratio,co.cal.dBA_CPM,co.d_BA_co,n,4);
	}
	catch(...)
	{
		return false;
	}

	return true;
}
Exemplo n.º 3
0
void IonFlow::setElectronTransport(vector_fp& tfix, vector_fp& diff_e,
                                   vector_fp& mobi_e)
{
    m_import_electron_transport = true;
    size_t degree = 5;
    size_t n = tfix.size();
    vector_fp tlog;
    for (size_t i = 0; i < n; i++) {
        tlog.push_back(log(tfix[i]));
    }
    vector_fp w(n, -1.0);
    m_diff_e_fix.resize(degree + 1);
    m_mobi_e_fix.resize(degree + 1);
    polyfit(n, degree, tlog.data(), diff_e.data(), w.data(), m_diff_e_fix.data());
    polyfit(n, degree, tlog.data(), mobi_e.data(), w.data(), m_mobi_e_fix.data());
}
Exemplo n.º 4
0
Arquivo: dfa.c Projeto: RobDurfee/Code
/* Detrended fluctuation analysis
    seq:	input data array
    npts:	number of input points
    nfit:	order of detrending (2: linear, 3: quadratic, etc.)
    rs:		array of box sizes (uniformly distributed on log scale)
    nr:		number of entries in rs[] and mse[]
    sw:		mode (0: non-overlapping windows, 1: sliding window)
   This function returns the mean squared fluctuations in mse[].
*/
void dfa(double *seq, long npts, int nfit, long *rs, int nr, int sw)
{
    long i, boxsize, inc, j;
    double stat;

    for (i = 1; i <= nr; i++) {
        boxsize = rs[i];
        if (sw) { inc = 1; stat = (int)(npts - boxsize + 1) * boxsize; }
	else { inc = boxsize; stat = (int)(npts / boxsize) * boxsize; }
        for (mse[i] = 0.0, j = 0; j <= npts - boxsize; j += inc)
            mse[i] += polyfit(x, seq + j, boxsize, nfit);
        mse[i] /= stat;
    }
}
Exemplo n.º 5
0
Arquivo: poly.c Projeto: nclack/whisk
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;
}
Exemplo n.º 6
0
int ZoomValueConvert::load_factors()
{
	/*
			v =
			   0    5638    8529   10336   11445   12384   13011   13637   14119   14505   14914   15179   15493   15733   15950   16119   16288   16384
			z = 
			   1 	 2      3		4		5		6		7		8		9		10		11		12		13		14		15		16		17		18
	 */

	const char *_vz = "1,0;2,5638;3,8529;4,10336;5,11445;6,12384;7,13011;8,13637;9,14119;10,14505;11,14914;12,15179;13,15493;14,15733;15,15950;16,16119;17,16288;18,16384";

	// 从配置文件加载倍率与zoom value之间的关系,并且拟合出多项式的系数
	const char *s = cfg_->get_value("cam_trace_zm_factors", _vz);
	char *factors = strdup(s);

	VD vs, zs;

	char *p = strtok(factors, ";");
	while (p) {
		int v, z;
		if (sscanf(p, "%d,%d", &v, &z) == 2) {
			vs.push_back(v * 1.0);
			zs.push_back(z * 1.0);
		}

		p = strtok(0, ";");
	}
	free(factors);

	double *pzs = (double*)malloc(zs.size()*sizeof(double));
	double *pvs = (double*)malloc(vs.size()*sizeof(double));
	int i = 0;
	for (VD::const_iterator it = vs.begin(); it != vs.end(); ++it)
		pvs[i++] = *it;
	i = 0;
	for (VD::const_iterator it = zs.begin(); it != zs.end(); ++it)
		pzs[i++] = *it;

	polyfit(vs.size(), pzs, pvs, 5, factors_);

	return 6;
}
Exemplo n.º 7
0
void MMCollisionInt::fit_omega22(int degree, doublereal deltastar,
                                 doublereal* o22)
{
    int i, n = m_nmax - m_nmin + 1;
    int ndeg=0;
    vector_fp values(n);
    doublereal rmserr;
    vector_fp w(n);
    doublereal* logT = DATA_PTR(m_logTemp) + m_nmin;
    for (i = 0; i < n; i++) {
        if (deltastar == 0.0) {
            values[i] = omega22_table[8*(i + m_nmin)];
        } else {
            values[i] = poly5(deltastar, DATA_PTR(m_o22poly[i+m_nmin]));
        }
    }
    w[0]= -1.0;
    rmserr = polyfit(n, logT, DATA_PTR(values),
                     DATA_PTR(w), degree, ndeg, 0.0, o22);
    if (DEBUG_MODE_ENABLED && m_loglevel > 0 && rmserr > 0.01) {
        writelogf("Warning: RMS error = %12.6g in omega_22 fit"
                  "with delta* = %12.6g\n", rmserr, deltastar);
    }
}
Exemplo n.º 8
0
doublereal MMCollisionInt::fitDelta(int table, int ntstar, int degree, doublereal* c)
{
    vector_fp w(8);
    doublereal* begin = 0;
    int ndeg=0;
    switch (table) {
    case 0:
        begin = omega22_table + 8*ntstar;
        break;
    case 1:
        begin = astar_table + 8*(ntstar + 1);
        break;
    case 2:
        begin = bstar_table + 8*(ntstar + 1);
        break;
    case 3:
        begin = cstar_table + 8*(ntstar + 1);
        break;
    default:
        return 0.0;
    }
    w[0] = -1.0;
    return polyfit(8, delta, begin, DATA_PTR(w), degree, ndeg, 0.0, c);
}
Exemplo n.º 9
0
/* from David Sandwell's code  */
void calc_height_velocity(struct ALOS_ORB *orb, struct PRM *prm, double t1, double t2,double *height, double *re2, double *vg, double *vtot, double *rdot)
{

int	k, ir, nt, nc=3;
double	xe, ye, ze;
double	xs, ys, zs;
double	x1, y1, z1;
double	x2, y2, z2;
double	vx, vy, vz, vs, rs;
double	rlat, rlatg;
double	st=0.0, ct=0.0, arg, re;
double	a[3], b[3], c[3];
double	time[1000],rng[1000],d[3];
double 	t0, ro, ra, rc, dt;

	if (verbose) fprintf(stderr," ... calc_height_velocity\n");

	ro = prm->near_range;
	ra = prm->ra;			/* ellipsoid parameters */
	rc = prm->rc;			/* ellipsoid parameters */

	dt = 200./prm->prf;

	/* ERSDAC  nt set to 31 instead of (nrows - az) / 100 */
	if (ALOS_format == 0) nt = (prm->nrows - prm->num_valid_az)/100.0; 
	if (ALOS_format == 1) nt = 31;

	/* make sure this number is at least 31 */
	if(nt < 31) nt = 31;

	/* more time stuff */
	t0 = (t1 + t2)/2.0;
	t1 = t0 - 2.0;
	t2 = t0 + 2.0;

	/* interpolate orbit 				*/
	/* _slow does memory allocation each time 	*/
	interpolate_ALOS_orbit_slow(orb, t0, &xs, &ys, &zs, &ir);
	interpolate_ALOS_orbit_slow(orb, t1, &x1, &y1, &z1, &ir);
	interpolate_ALOS_orbit_slow(orb, t2, &x2, &y2, &z2, &ir);

	rs = sqrt(xs*xs + ys*ys + zs*zs);

	/* calculate stuff */
	vx = (x2 - x1)/4.0;
	vy = (y2 - y1)/4.0;
	vz = (z2 - z1)/4.0;
	vs = sqrt(vx*vx + vy*vy + vz*vz);
	*vtot = vs;

	/* set orbit direction */
	if (vz > 0) {
		strcpy(prm->orbdir, "A");
	} else {
		strcpy(prm->orbdir, "D");
	}


	/* geodetic latitude of the satellite */
	rlat = asin(zs/rs);
	rlatg = atan(tan(rlat)*ra*ra/(rc*rc));  

	/* ERSDAC  use rlatg instead of latg */
	if (ALOS_format == 0){
		st = sin(rlat);
		ct = cos(rlat);
		}
	if (ALOS_format == 1){
		st = sin(rlatg);
		ct = cos(rlatg);
		}

	arg = (ct*ct)/(ra*ra) + (st*st)/(rc*rc);
	re = 1./(sqrt(arg));
	*re2 = re;
	*height = rs - *re2;

	/* compute the vector orthogonal to both the radial vector and velocity vector */
	a[0] = xs/rs;
	a[1] = ys/rs;
	a[2] = zs/rs;
	b[0] = vx/vs;
	b[1] = vy/vs;
	b[2] = vz/vs;

	cross3_(a,b,c);

	/*  compute the look angle */
	ct = (rs*rs+ro*ro-re*re)/(2.*rs*ro);
	st = sin(acos(ct));

	/* add the satellite and LOS vectors to get the new point */
	xe = xs+ro*(-st*c[0]-ct*a[0]);
	ye = ys+ro*(-st*c[1]-ct*a[1]);
	ze = zs+ro*(-st*c[2]-ct*a[2]);
	rlat = asin(ze/re);

	rlatg = atan(tan(rlat)*ra*ra/(rc*rc)); 

	/* ERSDAC  use rlatg instead of latg 		*/
	/*  compute elipse height in the scene 		*/
	if (ALOS_format == 0){
		st = sin(rlat);
		ct = cos(rlat);
		}
	if (ALOS_format == 1){
		st = sin(rlatg);
		ct = cos(rlatg);
		}

	arg = (ct*ct)/(ra*ra)+(st*st)/(rc*rc);
	re = 1.0/(sqrt(arg));

	/* now check range over time */
	for (k=0; k<nt; k++){
		time[k] = dt*(k - nt/2);
		t1 = t0+time[k];
		interpolate_ALOS_orbit_slow(orb, t1, &xs, &ys, &zs, &ir);
		rng[k] = sqrt((xe-xs)*(xe-xs) + (ye-ys)*(ye-ys) + (ze-zs)*(ze-zs)) - ro;
		}

	/* fit a second order polynomial to the range versus time function */
	polyfit(time,rng,d,&nt,&nc);
	*rdot = d[1];
	*vg=sqrt(ro*2.*d[2]);
}
Exemplo n.º 10
0
int main()
{
  uWS::Hub h;

  // MPC is initialized here!
  MPC mpc;

  h.onMessage([&mpc](uWS::WebSocket<uWS::SERVER> ws, char *data, size_t length,
                     uWS::OpCode opCode) {
    // "42" at the start of the message means there's a websocket message event.
    // The 4 signifies a websocket message
    // The 2 signifies a websocket event
    string sdata = string(data).substr(0, length);
    cout << sdata << endl;
    if (sdata.size() > 2 && sdata[0] == '4' && sdata[1] == '2')
    {
      string s = hasData(sdata);
      if (s != "")
      {
        auto j = json::parse(s);
        string event = j[0].get<string>();
        if (event == "telemetry")
        {
          // j[1] is the data JSON object
          vector<double> ptsx = j[1]["ptsx"];
          vector<double> ptsy = j[1]["ptsy"];
          double px = j[1]["x"];
          double py = j[1]["y"];
          double psi = j[1]["psi"];
          double v = j[1]["speed"];
          v *= 0.44704; //convert from mph to m/s

          double steer_value_in = j[1]["steering_angle"];
          steer_value_in *= deg2rad(25);
          double throttle_value_in = j[1]["throttle"];
          /*
          * TODO: Calculate steering angle and throttle using MPC.
          *
          * Both are in between [-1, 1].
          *
          */
          Eigen::VectorXd ptsx_car = Eigen::VectorXd(ptsx.size());
          Eigen::VectorXd ptsy_car = Eigen::VectorXd(ptsx.size());

          // Convert from the map coordinate system to the vehicle coordinate system
          for (int i = 0; i < ptsx.size(); i++)
          {
            auto car_coord = map_to_car_coord(psi, px, py, ptsx[i], ptsy[i]);
            ptsx_car[i] = car_coord[0];
            ptsy_car[i] = car_coord[1];
          }

          auto coeffs = polyfit(ptsx_car, ptsy_car, 3);

          double cte = polyeval(coeffs, 0) - 0.0;
          double epsi = -atan(coeffs[1]);

          // Create current state vector and solve
          // Add a latency of 100ms into the state before sending it to solver
          Eigen::VectorXd state(6);
          double latency = 0.1; //add a latency of 100ms
          double Lf = 2.67;
          double x_dl = (0.0 + v * latency);
          double y_dl = 0.0;
          double psi_dl = 0.0 + v * steer_value_in / Lf * latency;
          double v_dl = 0.0 + v + throttle_value_in * latency;
          double cte_dl = cte + (v * sin(epsi) * latency);
          double epsi_dl = epsi + v * steer_value_in / Lf * latency;

          state << x_dl, y_dl, psi_dl, v_dl, cte_dl, epsi_dl;

          auto result = mpc.Solve(state, coeffs);

          double steer_value = -result[6] / deg2rad(25);
          double throttle_value = result[7];

          json msgJson;
          msgJson["steering_angle"] = steer_value;
          msgJson["throttle"] = throttle_value;

          //Display the MPC predicted trajectory
          vector<double> mpc_x_vals = mpc.solution_x_;
          vector<double> mpc_y_vals = mpc.solution_y_;

          //.. add (x,y) points to list here, points are in reference to the vehicle's coordinate system
          vector<double> next_x;
          vector<double> next_y;

          for (int i = 0; i < ptsx.size(); i++)
          {
            //auto car_coord = map_to_car_coord(psi, px, py, ptsx[i], ptsy[i]);
            next_x.push_back(ptsx_car[i]);
            next_y.push_back(ptsy_car[i]);
          }

          msgJson["mpc_x"] = mpc_x_vals;
          msgJson["mpc_y"] = mpc_y_vals;

          msgJson["next_x"] = next_x;
          msgJson["next_y"] = next_y;

          auto msg = "42[\"steer\"," + msgJson.dump() + "]";
          std::cout << msg << std::endl;
          // Latency
          // The purpose is to mimic real driving conditions where
          // the car does actuate the commands instantly.
          //
          // Feel free to play around with this value but should be to drive
          // around the track with 100ms latency.
          //
          // NOTE: REMEMBER TO SET THIS TO 100 MILLISECONDS BEFORE
          // SUBMITTING.
          this_thread::sleep_for(chrono::milliseconds((int)(latency * 1000)));
          ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
        }
      }
      else
      {
        // Manual driving
        std::string msg = "42[\"manual\",{}]";
        ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
      }
    }
  });

  // We don't need this since we're not using HTTP but if it's removed the
  // program
  // doesn't compile :-(
  h.onHttpRequest([](uWS::HttpResponse *res, uWS::HttpRequest req, char *data,
                     size_t, size_t) {
    const std::string s = "<h1>Hello world!</h1>";
    if (req.getUrl().valueLength == 1)
    {
      res->end(s.data(), s.length());
    }
    else
    {
      // i guess this should be done more gracefully?
      res->end(nullptr, 0);
    }
  });

  h.onConnection([&h](uWS::WebSocket<uWS::SERVER> ws, uWS::HttpRequest req) {
    std::cout << "Connected!!!" << std::endl;
  });

  h.onDisconnection([&h](uWS::WebSocket<uWS::SERVER> ws, int code,
                         char *message, size_t length) {
    ws.close();
    std::cout << "Disconnected" << std::endl;
  });

  int port = 4567;
  if (h.listen(port))
  {
    std::cout << "Listening to port " << port << std::endl;
  }
  else
  {
    std::cerr << "Failed to listen to port" << std::endl;
    return -1;
  }
  h.run();
}
Exemplo n.º 11
0
/**
 * Calculate the equilibrium moisture content. This function determines the best
 * value of Xe to make a plot of \f$\ln\frac{X-X_e}{X_0-X_e}\f$ vs time linear.
 * In order to do this, it fits the data to the equation \f$y = a t + b\f$, where
 * \f$y = \ln(X-X_e)\f$ and \f$b = \ln(X_0-X_e)\f$, and then solves the equation
 * \f$F(X_e) = b - \ln(X_0-X_e) = 0\f$ using Newton's method.
 *
 * This function is made obsolete by CalcXeIt
 *
 * @param t Column matrix containing time during drying [s]
 * @param Xdb Column matrix of moisture content [kg/kg db]
 * @param Xe0 Initial guess for equilibrium moisture content.
 * @returns Equilibrium moisture content [kg/kg db]
 *
 * @see polyfit CalcXeIt
 */
double CalcXe(int initial, matrix *t, matrix *Xdb, double Xe0)
{
    double f, df, /* Function values and derivatives */
           b, /* Fitting parameters. Only the constant matters */
           tol = 1e-10, /* Tolerance for Newton's method */
           Xe = Xe0, /* Set Xe to the initial guess */
           Xep, /* Previous guess */
           X0, /* Initial moisture content */
           r2;
    matrix *beta, /* Matrix of fitting values */
           *y, /* Set equal to ln(X - Xe) */
           *Xadj,
           *tadj;
    int i, /* Loop index */
        iter = 0; /* Current iteration */

    /* Set the initial moisture content */
    X0 = val(Xdb, initial, 0);

    /* Make smaller matricies that contain only the "good" data. */
    tadj = CreateMatrix(nRows(Xdb) - initial, 1);
    Xadj = CreateMatrix(nRows(Xdb) - initial, 1);

    for(i=initial; i<nRows(t); i++) {
        setval(tadj, val(t, i, 0), i-initial, 0);
        setval(Xadj, val(Xdb, i, 0), i-initial, 0);
    }

    /* Actually find Xe */
    do {
        /* Make a y matrix containing ln(Xdb - Xe) */
        y = CreateMatrix(nRows(Xadj), 1);
        for(i=0; i<nRows(Xadj); i++)
            setval(y, log(val(Xadj, i, 0) - Xe), i, 0);

        /* Calculate b */
        beta = polyfit(tadj, y, 1);
        r2 = rsquared(tadj, y, beta);
        b = val(beta, 0, 0);

        /* Calculate f and df */
        f = b - log(X0 - Xe);
        df = 1/(X0 - Xe);

        /* Calculate the new value of Xe */
        Xep = Xe;
        Xe = Xe - f/df;

        /* Clean up */
        DestroyMatrix(y);
        DestroyMatrix(beta);

        /* Keep track of how many iterations we've gone through */
        iter++;

        /* Print out the current value */
        printf("Xe = %g, R^2 = %g\n", Xe, r2);
        if(Xe < 0) {
            printf("Failure to converge after %d iterations.\n", iter);
            return 0;
        }
    } while( fabs(Xe - Xep) > tol ); /* Check our value */

    /* Print out how many iterations it took to find Xe */
    printf("Solution converged after %d iterations.\n", iter);

    return Xe;
}
Exemplo n.º 12
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;
}
Exemplo n.º 13
0
void write_whiskpoly1_segment( FILE *file, Whisker_Seg *w )
{ typedef struct {int id; int time; int len;} trunc_WSeg;
  float path_length,     //               
        median_score;    //
  float *x = w->x,
        *y = w->y,
        *s = w->scores;
  int len = w->len;
  static double *workspace = NULL;
  double px[  WHISKER_IO_POLY_DEGREE+1 ],
         py[  WHISKER_IO_POLY_DEGREE+1 ];
  
  polyfit_realloc_workspace( len, WHISKER_IO_POLY_DEGREE, &workspace );

  // compute polynomial fit
  // ----------------------
  { 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
    { 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];
    }
    // Fit
    // ---
    { 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;
      int i;
      const int pad = MIN( WHISKER_IO_POLY_END_PADDING, len/4 );

      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_WHISKER_IO_POLYFIT_ERROR
      assert(t[0] == 0.0 );
      assert( (t[len-1] - 1.0)<1e-6 );
#endif

      // polynomial fit
      polyfit( t+pad, xd+pad, len-2*pad, WHISKER_IO_POLY_DEGREE, px, workspace );
      polyfit_reuse(  yd+pad, len-2*pad, WHISKER_IO_POLY_DEGREE, py, workspace );
    }
  }
  
  // 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;
  }
  
  // write a record
  // --------------
  if( w->len )
  { fwrite( (trunc_WSeg*) w, sizeof(trunc_WSeg) , 1                       , file );
    fwrite( &median_score  , sizeof(float)      , 1                       , file );
    fwrite( px             , sizeof(double)     , WHISKER_IO_POLY_DEGREE+1, file );
    fwrite( py             , sizeof(double)     , WHISKER_IO_POLY_DEGREE+1, file );
  }
}
Exemplo n.º 14
0
/* Function Definitions */
void polyPlot(const emxArray_real_T *x, const emxArray_real_T *y, int32_T degree,
              emxArray_real_T *coeff)
{
  polyfit(x, y, degree, coeff);
}
////////
/// \brief getGoodFrameLessBeta
/// \param AllFrame
/// \param count
/// \return
/// 检测到相对较好的帧中,根据拟合的结果,对最左端到最右端的一段进行间隔存储
bool getGoodFrameLessBeta(std::vector<GoodFrame> &AllFrame, int &count, std::vector<int> &good_frame_less)
{
    //std::cout << "begin" << std::endl;
    std::vector<int> good_frame = getGoodFrame(AllFrame, count);
//    std::cout << "getGoodFrame = " << good_frame.size() << std::endl;
//    std::cout << "AllFrame = " << AllFrame.size() << std::endl;
    std::vector<double> DiffCurrPreX_x, DiffCurrPreX_y;
    int id = 0; // X的变化量
    for (int i = 0; i < good_frame.size(); ++i)
    {
        DiffCurrPreX_x.push_back( (double)good_frame.at(i) );
        DiffCurrPreX_y.push_back( getDiffCurrPreId(AllFrame, good_frame.at(i), id) );
    }
//    std::cout << "fit" << std::endl;
    ///fit
    /// 
    std::vector<double> fit = polyfit( DiffCurrPreX_x, DiffCurrPreX_y, 4 );
//    cout << "fit.size()" << fit.size() << std::endl;
    if (fit.size() != 5)
    {
         return false;
    }

    double x[4];
    double a, b, c, d;
    a = fit.at(3)/fit.at(4);
    b = fit.at(2)/fit.at(4);
    c = fit.at(1)/fit.at(4);
    d = fit.at(0)/fit.at(4);
    // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method
    SolveP4(x, a, b, c, d) ;
//    cout << "solve" << endl;
//    cout << x[0] << " " << x[1] << " " << x[2] << " " << x[3] << endl;
    std::vector<int> id_x;
    for (int i = 0; i < 4; ++i)
    {
        id_x.push_back( (int)x[i] );
    }
    std::sort(id_x.begin(), id_x.end());

//    for (int i = 0; i < id_x.size(); ++i )
//        std::cout << id_x.at(i) << " ";
//    std::cout << std::endl;

    int begin_i, begin_id, end_i, end_id;
    std::pair<int, int> begin = find_similar(good_frame, id_x.at(1));
    std::pair<int, int> end = find_similar(good_frame, id_x.at(2));
    begin_i = begin.first; begin_id = begin.second;
    end_i = end.first; end_id = end.second;
    //std::cout << "begin_i_id = " << begin_i << "_____" << begin_id << std::endl;
    //std::cout << "end_i_id = " << end_i << "_____" << end_id << std::endl;
    //////
    //std::vector<int> good_frame_less;
    int cou = (int)((end_i - begin_i) / 18);
    if (cou <= 0)
        return false;
    for (int i = begin_i; (i < end_i) && ((i+cou) < end_i); i = i + cou)
        good_frame_less.push_back(good_frame.at(i));
    //return good_frame_less;
    return true; 
}
/**
 * Correct the data for absorption and multiple scattering effects. Allows
 * both histogram or point data. For histogram the TOF is taken to be
 * the mid point of a bin
 *
 * @return A histogram containing corrected values
 */
Mantid::HistogramData::Histogram
MayersSampleCorrectionStrategy::getCorrectedHisto() {

  // Temporary storage
  std::vector<double> xmur(N_MUR_PTS + 1, 0.0),
      yabs(N_MUR_PTS + 1, 1.0), // absorption signals
      wabs(N_MUR_PTS + 1, 1.0), // absorption weights
      yms(0),                   // multiple scattering signals
      wms(0);                   // multiple scattering weights
  if (m_pars.mscat) {
    yms.resize(N_MUR_PTS + 1, 0.0);
    wms.resize(N_MUR_PTS + 1, 100.0);
  }

  // Main loop over mur. Limit is nrpts but vectors are nrpts+1. First value set
  // by initial values above
  const double dmuR = (muRmax() - muRmin()) / to<double>(N_MUR_PTS - 1);
  for (size_t i = 1; i < N_MUR_PTS + 1; ++i) {
    const double muR = muRmin() + to<double>(i - 1) * dmuR;
    xmur[i] = muR;

    auto attenuation = calculateSelfAttenuation(muR);
    const double absFactor = attenuation / (M_PI * muR * muR);
    // track these
    yabs[i] = 1. / absFactor;
    wabs[i] = absFactor;
    if (m_pars.mscat) {
      // ratio of second/first scatter
      auto mscat = calculateMS(i, muR, attenuation);
      yms[i] = mscat.first;
      wms[i] = mscat.second;
    }
  }

  // Fit polynomials to absorption values to interpolate to input data range
  ChebyshevPolyFit polyfit(N_POLY_ORDER);
  auto absCoeffs = polyfit(xmur, yabs, wabs);
  decltype(absCoeffs) msCoeffs(0);
  if (m_pars.mscat)
    msCoeffs = polyfit(xmur, yms, wms);

  // corrections to input
  const double muMin(xmur.front()), muMax(xmur.back()),
      flightPath(m_pars.l1 + m_pars.l2),
      vol(M_PI * m_pars.cylHeight * pow(m_pars.cylRadius, 2));
  //  Oct 2003 discussion with Jerry Mayers:
  //  1E-22 factor in formula for RNS was introduced by Jerry to keep
  //   multiple scattering correction close to 1
  const double rns = (vol * 1e6) * (m_pars.rho * 1e24) * 1e-22;
  ChebyshevSeries chebyPoly(N_POLY_ORDER);

  auto outputHistogram = m_histogram;

  auto &sigOut = outputHistogram.mutableY();
  auto &errOut = outputHistogram.mutableE();

  for (size_t i = 0; i < m_histoYSize; ++i) {
    const double yin(m_histogram.y()[i]), ein(m_histogram.e()[i]);
    if (yin == 0) {
      // Detector with 0 signal received - skip this bin
      continue;
    }

    const double sigt = sigmaTotal(flightPath, m_tofVals[i]);
    const double rmu = muR(sigt);
    // Varies between [-1,+1]
    const double xcap = ((rmu - muMin) - (muMax - rmu)) / (muMax - muMin);
    double corrfact = chebyPoly(absCoeffs, xcap);
    if (m_pars.mscat) {
      const double msVal = chebyPoly(msCoeffs, xcap);
      const double beta = m_pars.sigmaSc * msVal / sigt;
      corrfact *= (1.0 - beta) / rns;
    }
    // apply correction

    sigOut[i] = yin * corrfact;
    errOut[i] = sigOut[i] * ein / yin;
  }
  return outputHistogram;
}