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);
}
}
}
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;
}
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());
}
/* 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;
}
}
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;
}
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;
}
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);
}
}
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);
}
/* 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]);
}
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();
}
/**
* 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;
}
//
// 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;
}
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 );
}
}
/* 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;
}