FuzzyIndices::CampelloIndices::CampelloIndices(FuzzyIndices& outer) :
outer(outer) {
std::shared_ptr<std::vector<int>> elements = outer.elements();
a = compute_a(*elements.get());
b = compute_b(*elements.get());
c = compute_c(*elements.get());
d = compute_d(*elements.get());
}
/********************************************************************************
This function compute the line among those crossing each labeled point (pos_vect[i], neg_vect[i]) which correspond to the highest Fscore.
The points labels are contained in the vector "labels. The algorithm is made up by two steps:
Step 1) compute the line (hence its angle alpha) which maximizes the Fscore among those crossing the origin and
the points (pos_vect[i], neg_vect[i])
Step 2) compute the line (hence its intercept) which maximizes the Fscore among those having slope tan(alpha) and crossing
the points (pos_vect[i], neg_vect[i])
During the optinization, for each line two possibility are considered: the positive half-plane is above or below the line. The variable
"positive_halfplane" will contain the choice which correspond to the highest Fscore.
INPUT
pos_vect: vector in which position "i" contains the weighted sum of positive neighbors of item "i"
neg_vect: vector in which position "i" contains the weighted sum of negative neighbors of item "i"
labels: vector containg the item labels
n: number of points
theta: output variable which will contain the angle formed with the x axis by the line corresponding to the highest Fscore
c: output variable which will contain the optimal threshold c = -q*cos(theta), where q is the intercept corresponding
to the optimal line
opt_hmean: output variable containing the Fscore corresponding to the optimum line
positive_halfplane: output variable containing the position of the positive half-plane: 1 over, -1 under the line
*/
void error_minimization(double *pos_vect, double *neg_vect, int *labels, int *n,
double *theta, double *c, double *opt_hmean, int *positive_halfplane)
{
int N_pos = 0, pos_halfplane = 0, N_neg, tp_o, fn_o, fp_o, tn_o, tp_u, fn_u, fp_u, tn_u,
cnt, pos_labels, neg_labels, opt_fp_o = 0, opt_fp_u = 0, opt_fn_o=0, opt_fn_u=0, opt_tp_o=0, opt_tp_u=0;
register int i, h;
const int n_ = (*n);
int order_thetas[n_], order_c_values[n_];
double max_F = 0, theta_best = 0, c_best = 0, opt_hmean_over = 0, opt_hmean_under = 0;
double theta_best_over, theta_best_under=0,tmp_hmean_under, tmp_hmean_over;
double thetas[n_], c_values[n_];
// finding the number of positive labels
N_pos = count_positives(labels, n_);
N_neg = n_ - N_pos;
// initial errors when positive halfplane 'over' the line
tp_o = N_pos; fp_o = N_neg; tn_o = 0; fn_o = 0;
tmp_hmean_over = compute_F(tp_o, fn_o, fp_o);
// initial errors when positive halfplane 'under' the line
tp_u = 0; fp_u = 0; tn_u = N_neg; fn_u = N_pos; tmp_hmean_under = 0.0;
// computing the angles of each line passing through the origin and a point of the training set
compute_angles(pos_vect, neg_vect, order_thetas, thetas, n_);
// sorting angles and their indices
quicksort(thetas, order_thetas, 0, (n_)-1);
// scanning ordered angles to find the optimum line
for(i = 0; i < n_; i++)
{
if(thetas[i] >= 1.57)break;
cnt = 0; pos_labels = 0; neg_labels = 0;
// counting the number of collinear points
while(thetas[i] == thetas[i + cnt + 1]) cnt++;
if(i != (n_-1)){
for(h = 0; h <= cnt; h++)
{
if(labels[ order_thetas[i + h] ] > 0)
pos_labels++;
else
neg_labels++;
}
}
// updating actual errors
tp_o -= pos_labels; fn_o += pos_labels; fp_o -= neg_labels; tn_o += neg_labels;
tp_u += pos_labels; fn_u -= pos_labels; fp_u += neg_labels; tn_u -= neg_labels;
tmp_hmean_over = compute_F(tp_o, fn_o, fp_o);
tmp_hmean_under = compute_F(tp_u, fn_u, fp_u);
// check whether current F-scores is greater than actual maximum Fscores
check_update(tmp_hmean_under, &opt_hmean_under, &theta_best_under, thetas[i],
&opt_tp_u, &opt_fp_u, &opt_fn_u, tp_u, fp_u, fn_u);
check_update(tmp_hmean_over, &opt_hmean_over, &theta_best_over, thetas[i],
&opt_tp_o, &opt_fp_o, &opt_fn_o, tp_o, fp_o, fn_o);
// increment in order to avoid to consider again collinear points
i = i + cnt;
}
// choosing the optimum half-plane, fscore and angle
check_halfplane(opt_hmean_over, opt_hmean_under, theta_best_over, theta_best_under, &pos_halfplane, &max_F, &theta_best);
// ------- Step 2: computing best intercept---------------
compute_c(pos_vect, neg_vect, order_c_values, c_values, theta_best, n_);
// sorting intercepts and their indices
quicksort(c_values, order_c_values, 0, n_-1);
tp_u = 0; fp_u = 0; tn_u = N_neg; fn_u = N_pos;
compute_best_c(*n, tp_u, fp_u, tn_u, fn_u, labels, c_values, order_c_values, &max_F, &c_best, theta_best);
theta_best = theta_best + DBL_MIN;
*opt_hmean = max_F;
*theta = theta_best;
*c = -c_best * cos(*theta);
*positive_halfplane = (int)pos_halfplane;
}
/* Returns the normalized MI scores.
*
* Parameters
* dx (IN) : x-data sorted in increasing order by dx-values
* dy (IN) : y-data sorted in increasing order by dx-values
* n (IN) : number of elements in dx and dy
* Q_map (IN) : the map Q computed by EquipartitionYAxis() sorted in
* increasing order by dx-values
* q (IN) : number of partitions in Q_map
* P_map (IN) : the map P computed by GetSuperclumpsPartition() sorted
* in increasing order by Dx-values
* p (IN) : number of partitions in P_map
* x (IN) : maximum grid size on dx-values
* score (OUT) : mutual information scores. score must be a
* preallocated array of dimension x-1
* Returns
* 0 on success, 1 if an error occurs
*/
int OptimizeXAxis(double *dx, double *dy, int n, int *Q_map, int q,
int *P_map, int p, int x, double *score)
{
int i, s, t, l;
int *c;
int **cumhist;
double **I, **HP2Q;
double F, F_max, HQ, ct, cs;
/* return score=0 if p=1 */
if (p == 1)
{
for (i=0; i<x-1; i++)
score[i] = 0.0;
return 0;
}
/* compute c */
c = compute_c(P_map, p, n);
if (c == NULL)
goto error_c;
/* compute the cumulative histogram matrix along P_map */
cumhist = compute_cumhist(Q_map, q, P_map, p, n);
if (cumhist == NULL)
goto error_cumhist;
/* I matrix initialization */
I = init_I(p, x);
if (I == NULL)
goto error_I;
/* Precomputes the HP2Q matrix */
HP2Q = compute_HP2Q(cumhist, c, q, p);
if (HP2Q == NULL)
goto error_HP2Q;
/* compute H(Q) */
HQ = hq(cumhist, q, p, n);
/*
* Find the optimal partitions of size 2
* Algorithm 2 in SOM, lines 3-8
*/
for (t=2; t<=p; t++)
{
F_max = -DBL_MAX;
for (s=1; s<=t; s++)
{
F = hp3(c, s, t) - hp3q(cumhist, c, q, p, s, t);
if (F > F_max)
{
I[t][2] = HQ + F;
F_max = F;
}
}
}
/*
* Inductively build the rest of the table of
* optimal partitions
* Algorithm 2 in SOM, lines 10-17
*/
for (l=3; l<=x; l++)
{
for (t=l; t<=p; t++)
{
ct = (double) c[t-1];
F_max = -DBL_MAX;
for (s=l-1; s<=t; s++)
{
cs = (double) c[s-1];
F = ((cs/ct) * (I[s][l-1]-HQ))
- (((ct-cs)/ct) * HP2Q[s][t]);
if (F > F_max)
{
I[t][l] = HQ + F;
F_max = F;
}
}
}
}
/* Algorithm 2 in SOM, line 19 */
for (i=p+1; i<=x; i++)
I[p][i] = I[p][p];
/* score */
for (i=2; i<=x; i++)
score[i-2] = I[p][i] / MIN(log(i), log(q));
/* start frees */
for (i=0; i<=p; i++)
free(HP2Q[i]);
free(HP2Q);
for (i=0; i<=p; i++)
free(I[i]);
free(I);
for (i=0; i<q; i++)
free(cumhist[i]);
free(cumhist);
free (c);
/* end frees*/
return 0;
/* gotos*/
error_HP2Q:
for (i=0; i<=p; i++)
free(I[i]);
free(I);
error_I:
for (i=0; i<q; i++)
free(cumhist[i]);
free(cumhist);
error_cumhist:
free(c);
error_c:
return 1;
}
/*
*if task_type == TASK_RUN,setup task_segList;
*/
static void runTask_init(task t)
{
mylogfd(-1,"[Control]/motor/runTask_init\n");
int i = 0;;
double rate = 1;
int delta = 0;
double r = 0 ;
maxa = t.maxa;
Point *circle;
circle = (Point*)malloc(sizeof(struct _Point));
for (i = 0; i < task_segNum; i++) {
if (fabs((vleft + vright)/2 * (vleft + vright)/2)
>=2*maxa*(task_segNum-i-1)*SEGDIS) {
break;
}
task_segList[i].type = TASK_RUN;
task_segList[i].start.x = t.trace->ps[i].x;
task_segList[i].start.y = t.trace->ps[i].y;
task_segList[i].end.x = t.trace->ps[i+1].x;
task_segList[i].end.y = t.trace->ps[i+1].y;
if ((task_segList[i].start.x == task_segList[i].end.x)
&&(task_segList[i].start.y == task_segList[i].end.y)) {
task_segList[i].type = TASK_ROTA;
int rot = t.trace->direction[i+1]-t.trace->direction[i];
if (rot > 0) {
task_segList[i].lspeed = -ROTV;
task_segList[i].rspeed = ROTV;
} else {
task_segList[i].lspeed = ROTV;
task_segList[i].rspeed = -ROTV;
}
task_segList[i].rotangle = rot;
task_segList[i].dur_time = compute_t(task_segList[i].rotangle,
DD/2,task_segList[i].lspeed);
continue;
}
//直线
if (t.trace->direction[i] == t.trace->direction[i+1]) {
if (t.trace->inverse[i] == 1) { //反向
if (vleft != vright) {
if (fabs(vleft) > fabs(vright)) {
if (vleft < 0)
vright = vleft;
else {
vleft = 0;
vright = 0;
}
} else {
if (vright < 0)
vleft = vright;
else {
vleft = 0;
vright = 0;
}
}
}
task_segList[i].lspeed = vleft - ADDV;
//vleft = task_segList[i].lspeed;
task_segList[i].rspeed = vright - ADDV;
//vright = task_segList[i].rspeed;
} else { //正向
if (vleft != vright) {
if (fabs(vleft) > fabs(vright)) {
if (vleft > 0)
vright = vleft;
else {
vleft = 0;
vright = 0;
}
} else {
if (vright > 0)
vleft = vright;
else {
vleft = 0;
vright = 0;
}
}
}
task_segList[i].lspeed = vleft + ADDV;
//vleft = task_segList[i].lspeed;
task_segList[i].rspeed = vright + ADDV;
//vright = task_segList[i].rspeed;
}
if (task_segList[i].lspeed > MAXV)
task_segList[i].lspeed = MAXV;
if (task_segList[i].lspeed < -MAXV)
task_segList[i].lspeed = -MAXV;
if (task_segList[i].rspeed > MAXV)
task_segList[i].rspeed = MAXV;
if (task_segList[i].rspeed < -MAXV)
task_segList[i].rspeed = -MAXV;
double d = sqrt((t.trace->ps[i].x - t.trace->ps[i+1].x)
*(t.trace->ps[i].x - t.trace->ps[i+1].x)
+(t.trace->ps[i].y - t.trace->ps[i+1].y)
*(t.trace->ps[i].y - t.trace->ps[i+1].y));
task_segList[i].dur_time = fabs(d / task_segList[i].lspeed);
compute_a(&task_segList[i].lspeed,&task_segList[i].rspeed,
task_segList[i].dur_time,t.trace->inverse[i],1);
task_segList[i].dur_time = fabs(d / task_segList[i].lspeed);
}
else { //圆弧
//计算圆心
compute_c(circle,t.trace->ps[i],t.trace->ps[i+1],
t.trace->direction[i],t.trace->direction[i+1]);
//计算半径
r = t.trace->r[i];
//速度比
rate = (r+DD/2)/(r-DD/2);
if (t.trace->direction[i] < t.trace->direction[i+1])
rate = 1/rate;
//右旋
if (rate > 1) {
if (t.trace->inverse[i] == 0) {
if (vleft == vright)
vright = vleft/rate;
task_segList[i].lspeed = vleft + ADDV;
//vleft = task_segList[i].lspeed;
task_segList[i].rspeed = task_segList[i].lspeed/rate;
//vright = task_segList[i].rspeed;
} else {
if (vleft == vright)
vright = vleft/rate;
task_segList[i].lspeed = vleft - ADDV;
//vleft = task_segList[i].lspeed;
task_segList[i].rspeed = task_segList[i].lspeed/rate;
//vright = task_segList[i].rspeed;
}
delta = abs(t.trace->direction[i] - t.trace->direction[i+1]);
task_segList[i].dur_time = compute_t(delta,(r + DD/2),
task_segList[i].lspeed);
compute_a(&task_segList[i].lspeed,&task_segList[i].rspeed,
task_segList[i].dur_time,t.trace->inverse[i],rate);
if (task_segList[i].lspeed > MAXV) {
task_segList[i].lspeed = MAXV;
task_segList[i].rspeed = MAXV/rate;
}
if (task_segList[i].lspeed < -MAXV) {
task_segList[i].lspeed = -MAXV;
task_segList[i].rspeed = -MAXV/rate;
}
task_segList[i].dur_time = compute_t(delta,(r + DD/2),
task_segList[i].lspeed);
}
else { //左旋
if (t.trace->inverse[i] == 0) {
if (vleft == vright)
vleft = vright*rate;
task_segList[i].rspeed = vright + ADDV;
//vright = task_segList[i].rspeed;
task_segList[i].lspeed = task_segList[i].rspeed*rate;
//vleft = task_segList[i].lspeed;
} else {
if (vleft == vright)
vleft = vright*rate;
task_segList[i].rspeed = vright - ADDV;
//vright = task_segList[i].rspeed;
task_segList[i].lspeed = task_segList[i].rspeed*rate;
//vleft = task_segList[i].lspeed;
}
delta = abs(t.trace->direction[i] - t.trace->direction[i+1]);
task_segList[i].dur_time = compute_t(delta,(r + DD/2),
task_segList[i].rspeed);
compute_a(&task_segList[i].lspeed,&task_segList[i].rspeed,
task_segList[i].dur_time,t.trace->inverse[i],rate);
if (task_segList[i].rspeed > MAXV) {
task_segList[i].rspeed = MAXV;
task_segList[i].lspeed = MAXV*rate;
}
if (task_segList[i].rspeed < -MAXV) {
task_segList[i].rspeed = -MAXV;
task_segList[i].lspeed = -MAXV*rate;
}
task_segList[i].dur_time = compute_t(delta,(r + DD/2),
task_segList[i].rspeed);
}
}
task_segList[i].rotangle = 0;
//mylogfd(1,"<%d,%d>",(int)task_segList[i].lspeed,(int)task_segList[i].rspeed);
}
for (; i < task_segNum-1; i++) {
task_segList[i].type = TASK_RUN;
task_segList[i].start.x = t.trace->ps[i].x;
task_segList[i].start.y = t.trace->ps[i].y;
task_segList[i].end.x = t.trace->ps[i+1].x;
task_segList[i].end.y = t.trace->ps[i+1].y;
//直线
if (t.trace->direction[i] == t.trace->direction[i+1]) {
if (t.trace->inverse[i] == 1) { //反向
if (vleft != vright) {
if (fabs(vleft) > fabs(vright)) {
if (vleft < 0)
vright = vleft;
else {
vleft = 0;
vright = 0;
}
} else {
if (vright < 0)
vleft = vright;
else {
vleft = 0;
vright = 0;
}
}
}
task_segList[i].lspeed = vleft + ADDV;
//vleft = task_segList[i].lspeed;
task_segList[i].rspeed = task_segList[i].lspeed;
//vright = task_segList[i].rspeed;
if (task_segList[i].lspeed > -MIN_SPEED)
task_segList[i].lspeed = -MIN_SPEED;
if (task_segList[i].rspeed > -MIN_SPEED)
task_segList[i].rspeed = -MIN_SPEED;
}
else { //正向
if (vleft != vright) {
if (fabs(vleft) > fabs(vright)) {
if (vleft > 0)
vright = vleft;
else {
vleft = 0;
vright = 0;
}
} else {
if (vright > 0)
vleft = vright;
else {
vleft = 0;
vright = 0;
}
}
}
task_segList[i].lspeed = vleft - ADDV;
//vleft = task_segList[i].lspeed;
task_segList[i].rspeed = task_segList[i].lspeed;
//vright = task_segList[i].rspeed;
if (task_segList[i].lspeed < MIN_SPEED)
task_segList[i].lspeed = MIN_SPEED;
if (task_segList[i].rspeed < MIN_SPEED)
task_segList[i].rspeed = MIN_SPEED;
}
if (task_segList[i].lspeed > MAXV)
task_segList[i].lspeed = MAXV;
if (task_segList[i].lspeed < -MAXV)
task_segList[i].lspeed = -MAXV;
if (task_segList[i].rspeed > MAXV)
task_segList[i].rspeed = MAXV;
if (task_segList[i].rspeed < -MAXV)
task_segList[i].rspeed = -MAXV;
double d = sqrt((t.trace->ps[i].x - t.trace->ps[i+1].x)
*(t.trace->ps[i].x - t.trace->ps[i+1].x)
+(t.trace->ps[i].y - t.trace->ps[i+1].y)
*(t.trace->ps[i].y - t.trace->ps[i+1].y));
task_segList[i].dur_time = fabs(d / task_segList[i].lspeed);
compute_a(&task_segList[i].lspeed,&task_segList[i].rspeed,
task_segList[i].dur_time,1-t.trace->inverse[i],1);
task_segList[i].dur_time = fabs(d / task_segList[i].lspeed);
}
else { //圆弧
//计算圆心
compute_c(circle,t.trace->ps[i],t.trace->ps[i+1],
t.trace->direction[i],t.trace->direction[i+1]);
//计算半径
r = t.trace->r[i];
//速度比
rate = (r+DD/2)/(r-DD/2);
if (t.trace->direction[i] < t.trace->direction[i+1])
rate = 1/rate;
//右旋
if (rate > 1) {
if (t.trace->inverse[i] == 0) {
if (vleft == vright)
vright = vleft/rate;
task_segList[i].lspeed = vleft - ADDV;
//vleft = task_segList[i].lspeed;
task_segList[i].rspeed = task_segList[i].lspeed/rate;
//vright = task_segList[i].rspeed;
} else {
if (vleft == vright)
vright = vleft/rate;
task_segList[i].lspeed = vleft + ADDV;
//vleft = task_segList[i].lspeed;
task_segList[i].rspeed = task_segList[i].lspeed/rate;
//vright = task_segList[i].rspeed;
}
delta = abs(t.trace->direction[i] - t.trace->direction[i+1]);
task_segList[i].dur_time = compute_t(delta,(r + DD/2),
task_segList[i].lspeed);
compute_a(&task_segList[i].lspeed,&task_segList[i].rspeed,
task_segList[i].dur_time,1-t.trace->inverse[i],rate);
if (task_segList[i].lspeed < MIN_SPEED) {
task_segList[i].lspeed = MIN_SPEED;
task_segList[i].rspeed = MIN_SPEED/rate;
}
if (task_segList[i].rspeed < MIN_SPEED/rate) {
task_segList[i].lspeed = MIN_SPEED;
task_segList[i].rspeed = MIN_SPEED/rate;
}
if (task_segList[i].lspeed > MAXV) {
task_segList[i].lspeed = MAXV;
task_segList[i].rspeed = MAXV/rate;
}
if (task_segList[i].lspeed < -MAXV) {
task_segList[i].lspeed = -MAXV;
task_segList[i].rspeed = -MAXV/rate;
}
task_segList[i].dur_time = compute_t(delta,(r + DD/2),
task_segList[i].lspeed);
}
else { //左旋
if (t.trace->inverse[i] == 0) {
if (vleft == vright)
vleft = vright*rate;
task_segList[i].rspeed = vright - ADDV;
//vright = task_segList[i].rspeed;
task_segList[i].lspeed = task_segList[i].rspeed*rate;
//vleft = task_segList[i].lspeed;
} else {
if (vleft == vright)
vleft = vright*rate;
task_segList[i].rspeed = vright - ADDV;
//vright = task_segList[i].rspeed;
task_segList[i].lspeed = task_segList[i].rspeed*rate;
//vleft = task_segList[i].lspeed;
}
delta = abs(t.trace->direction[i] - t.trace->direction[i+1]);
task_segList[i].dur_time = compute_t(delta,(r + DD/2),
task_segList[i].rspeed);
compute_a(&task_segList[i].lspeed,&task_segList[i].rspeed,
task_segList[i].dur_time,1-t.trace->inverse[i],rate);
if (task_segList[i].lspeed < MIN_SPEED*rate) {
task_segList[i].lspeed = MIN_SPEED*rate;
task_segList[i].rspeed = MIN_SPEED;
}
if (task_segList[i].rspeed < MIN_SPEED) {
task_segList[i].lspeed = MIN_SPEED*rate;
task_segList[i].rspeed = MIN_SPEED;
}
if (task_segList[i].rspeed > MAXV) {
task_segList[i].rspeed = MAXV;
task_segList[i].lspeed = MAXV*rate;
}
if (task_segList[i].rspeed < -MAXV) {
task_segList[i].rspeed = -MAXV;
task_segList[i].lspeed = -MAXV*rate;
}
task_segList[i].dur_time = compute_t(delta,(r + DD/2),
task_segList[i].rspeed);
}
}
task_segList[i].rotangle = 0;
/* mylogfd(1,"(%d,%d)<%d,%d>{%lf}",circle->x,circle->y,(int)task_segList[i].lspeed,(int)task_segList[i].rspeed,rate); */
}
task_segList[i].type = TASK_RUN;
task_segList[i].start.x = t.trace->ps[i].x;
task_segList[i].start.y = t.trace->ps[i].y;
task_segList[i].end.x = t.trace->ps[i+1].x;
task_segList[i].end.y = t.trace->ps[i+1].y;
task_segList[i].lspeed = 0;
task_segList[i].rspeed = 0;
task_segList[i].dur_time = 0;
task_segList[i].rotangle = 0;
}
static void dotshootTask_init(task t)
{
mylogfd (MOTOFD, "dotshootTask_init\n");
int i = 0;
double rate = 1;
int delta = 0;
double r = 0;
maxa = t.maxa;
for (i = 0; i < task_segNum - 2; i++) {
task_segList[i].type = TASK_RUN;
task_segList[i].start.x = t.trace->ps[i].x;
task_segList[i].start.y = t.trace->ps[i].y;
task_segList[i].end.x = t.trace->ps[i+1].x;
task_segList[i].end.y = t.trace->ps[i+1].y;
if ((task_segList[i].start.x == task_segList[i].end.x)
&&(task_segList[i].start.y == task_segList[i].end.y)) {
task_segList[i].type = TASK_ROTA;
int rot = t.trace->direction[i+1]-t.trace->direction[i];
if (rot > 0) {
task_segList[i].lspeed = 2*-ROTV;
task_segList[i].rspeed = 2*ROTV;
} else {
task_segList[i].lspeed = 2*ROTV;
task_segList[i].rspeed = 2*-ROTV;
}
task_segList[i].rotangle = rot;
task_segList[i].dur_time = compute_t(task_segList[i].rotangle,
DD/2,task_segList[i].lspeed);
continue;
}
if (t.trace->direction[i] == t.trace->direction[i+1]) {
if (t.trace->inverse[i] == 1) { //反向
if (vleft != vright) {
if (fabs(vleft) > fabs(vright)) {
if (vleft < 0)
vright = vleft;
else {
vleft = 0;
vright = 0;
}
} else {
if (vright < 0)
vleft = vright;
else {
vleft = 0;
vright = 0;
}
}
}
task_segList[i].lspeed = vleft - 2*ADDV;
//vleft = task_segList[i].lspeed;
task_segList[i].rspeed = task_segList[i].lspeed;
//vright = task_segList[i].rspeed;
} else { //正向
if (vleft != vright) {
if (fabs(vleft) > fabs(vright)) {
if (vleft > 0)
vright = vleft;
else {
vleft = 0;
vright = 0;
}
} else {
if (vright > 0)
vleft = vright;
else {
vleft = 0;
vright = 0;
}
}
}
task_segList[i].lspeed = vleft + ADDV;
//vleft = task_segList[i].lspeed;
task_segList[i].rspeed = task_segList[i].lspeed;
//vright = task_segList[i].rspeed;
}
double d = sqrt((t.trace->ps[i].x - t.trace->ps[i+1].x)
*(t.trace->ps[i].x - t.trace->ps[i+1].x)
+(t.trace->ps[i].y - t.trace->ps[i+1].y)
*(t.trace->ps[i].y - t.trace->ps[i+1].y));
task_segList[i].dur_time = fabs(d / task_segList[i].lspeed);
compute_a(&task_segList[i].lspeed,&task_segList[i].rspeed,
task_segList[i].dur_time,t.trace->inverse[i],1);
if (task_segList[i].lspeed > MAXV)
task_segList[i].lspeed = MAXV;
if (task_segList[i].lspeed < -MAXV)
task_segList[i].lspeed = -MAXV;
if (task_segList[i].rspeed > MAXV)
task_segList[i].rspeed = MAXV;
if (task_segList[i].rspeed < -MAXV)
task_segList[i].rspeed = -MAXV;
task_segList[i].dur_time = fabs(d / task_segList[i].lspeed);
} else { //圆弧
//计算圆心
Point *circle;
circle = (Point*)malloc(sizeof(struct _Point));
compute_c(circle,t.trace->ps[i],t.trace->ps[i+1],
t.trace->direction[i],t.trace->direction[i+1]);
//计算半径
r = t.trace->r[i];
//速度比
rate = (r+DD/2)/(r-DD/2);
mylogfd(MOTOFD,"get in arc r=%f rate=%f\n",r,rate);
if (t.trace->direction[i] < t.trace->direction[i+1])
rate = 1/rate;
//右旋
if (rate > 1) {
if (t.trace->inverse[i] == 0) {
if (vleft < 0)
vleft = -vleft;
if (vleft == vright)
vright = vleft/rate;
task_segList[i].lspeed = vleft + ADDV;
//vleft = task_segList[i].lspeed;
task_segList[i].rspeed = task_segList[i].lspeed/rate;
//vright = task_segList[i].rspeed;
} else {
if (vleft > 0)
vleft = -vleft;
if (vleft == vright)
vright = vleft/rate;
task_segList[i].lspeed = vleft - ADDV;
//vleft = task_segList[i].lspeed;
task_segList[i].rspeed = task_segList[i].lspeed/rate;
//vright = task_segList[i].rspeed;
}
delta = abs(t.trace->direction[i] - t.trace->direction[i+1]);
task_segList[i].dur_time = compute_t(delta,(r + DD/2),
task_segList[i].lspeed) - 0.07;
compute_a(&task_segList[i].lspeed,&task_segList[i].rspeed,
task_segList[i].dur_time,t.trace->inverse[i],rate);
if (task_segList[i].lspeed > MAXV) {
task_segList[i].lspeed = MAXV;
task_segList[i].rspeed = MAXV/rate;
}
if (task_segList[i].lspeed < -MAXV) {
task_segList[i].lspeed = -MAXV;
task_segList[i].rspeed = -MAXV/rate;
}
task_segList[i].dur_time = compute_t(delta,(r + DD/2),
task_segList[i].lspeed);
task_segList[i].dur_time = task_segList[i].dur_time * 13.0f / 12 - 0.07;
}
else { //左旋
if (t.trace->inverse[i] == 0) {
if (vleft < 0)
vleft = -vleft;
if (vleft == vright)
vleft = vright*rate;
task_segList[i].rspeed = vright + ADDV;
//vright = task_segList[i].rspeed;
task_segList[i].lspeed = task_segList[i].rspeed*rate;
//vleft = task_segList[i].lspeed;
} else {
if (vleft > 0)
vleft = -vleft;
if (vleft == vright)
vleft = vright*rate;
task_segList[i].rspeed = vright - ADDV;
//vright = task_segList[i].rspeed;
task_segList[i].lspeed = task_segList[i].rspeed*rate;
//vleft = task_segList[i].lspeed;
}
delta = abs(t.trace->direction[i] - t.trace->direction[i+1]);
task_segList[i].dur_time = compute_t(delta,(r + DD/2),
task_segList[i].rspeed);
compute_a(&task_segList[i].lspeed,&task_segList[i].rspeed,
task_segList[i].dur_time,t.trace->inverse[i],rate);
if (task_segList[i].rspeed > MAXV) {
task_segList[i].rspeed = MAXV;
task_segList[i].lspeed = MAXV*rate;
}
if (task_segList[i].rspeed < -MAXV) {
task_segList[i].rspeed = -MAXV;
task_segList[i].lspeed = -MAXV*rate;
}
task_segList[i].dur_time = compute_t(delta,(r + DD/2),
task_segList[i].rspeed);
task_segList[i].dur_time = task_segList[i].dur_time * 13.0f / 12 - 0.07;
}
}
task_segList[i].rotangle = 0;
//mylogfd(-1,"<%d,%d>",(int)task_segList[i].lspeed,(int)task_segList[i].rspeed);
}
task_segList[i].type = TASK_RUN;
task_segList[i].start.x = 0;
task_segList[i].start.y = 0;
task_segList[i].end.x = 0;
task_segList[i].end.y = 0;
task_segList[i].lspeed = task_segList[i-1].lspeed;
//vleft = 0;
task_segList[i].rspeed = task_segList[i-1].rspeed;
//vright = 0;
task_segList[i].rotangle = 0;
task_segList[i].dur_time = MOTO_SHOOT_DIS/task_segList[i].lspeed;
i++;
task_segList[i].type = TASK_STOP;
task_segList[i].start.x = 0;
task_segList[i].start.y = 0;
task_segList[i].end.x = 0;
task_segList[i].end.y = 0;
task_segList[i].lspeed = task_segList[i-1].lspeed;
vleft = 0;
task_segList[i].rspeed = task_segList[i-1].rspeed;
vright = 0;
task_segList[i].rotangle = 0;
task_segList[i].dur_time = 0;
mylogfd (MOTOFD, "dotshootTask_init over\n");
}
/**
* ahrs_step() takes the values from the IMU and produces the
* new estimated attitude.
*/
void
ahrs_step(
v_t angles_out,
f_t dt,
f_t p,
f_t q,
f_t r,
f_t ax,
f_t ay,
f_t az,
f_t heading
)
{
m_t C;
m_t A;
m_t AT;
v_t X_dot;
m_t temp;
v_t Xm;
v_t Xe;
/* Construct the quaternion omega matrix */
rotate2omega( A, p, q, r );
/* X_dot = AX */
mv_mult( X_dot, A, X, 4, 4 );
/* X += X_dot dt */
v_scale( X_dot, X_dot, dt, 4 );
v_add( X, X_dot, X, 4 );
v_normq( X, X );
/*
printf( "X =" );
Vprint( X, 4 );
printf( "\n" );
*/
/* AT = transpose(A) */
m_transpose( AT, A, 4, 4 );
/* P = A * P * AT + Q */
m_mult( temp, A, P, 4, 4, 4 );
m_mult( P, temp, AT, 4, 4, 4 );
m_add( P, Q, P, 4, 4 );
compute_c( C, X );
/* Compute the euler angles of the measured attitude */
accel2euler(
Xm,
ax,
ay,
az,
heading
);
/*
* Compute the euler angles of the estimated attitude
* Xe = quat2euler( X )
*/
quat2euler( Xe, X );
/* Kalman filter update */
kalman_update(
P,
R,
C,
X,
Xe,
Xm
);
/* Estimated attitude extraction */
quat2euler( angles_out, X );
}
int
write_index
(
char * filename
)
{
// Output files.
char * sarfile = malloc(strlen(filename)+5);
strcpy(sarfile, filename);
strcpy(sarfile + strlen(filename), ".sar");
char * occfile = malloc(strlen(filename)+5);
strcpy(occfile, filename);
strcpy(occfile + strlen(filename), ".occ");
char * genfile = malloc(strlen(filename)+5);
strcpy(genfile, filename);
strcpy(genfile + strlen(filename), ".gen");
// char * lutfile = malloc(strlen(filename)+5);
// strcpy(lutfile, filename);
// strcpy(lutfile + strlen(filename), ".lut");
char * lcpfile = malloc(strlen(filename)+5);
strcpy(lcpfile, filename);
strcpy(lcpfile + strlen(filename), ".lcp");
// Open files.
int fsa = open(sarfile, O_WRONLY | O_CREAT | O_TRUNC, 0644);
int foc = open(occfile, O_WRONLY | O_CREAT | O_TRUNC, 0644);
int fgn = open(genfile, O_WRONLY | O_CREAT | O_TRUNC, 0644);
// int flt = open(lutfile, O_WRONLY | O_CREAT | O_TRUNC, 0644);
int flc = open(lcpfile, O_WRONLY | O_CREAT | O_TRUNC, 0644);
// Error control.
if (fsa == -1) {
fprintf(stderr, "error in write_index (open %s): %s.\n", sarfile, strerror(errno));
exit(EXIT_FAILURE);
}
if (foc == -1) {
fprintf(stderr, "error in write_index (open %s): %s.\n", occfile, strerror(errno));
exit(EXIT_FAILURE);
}
if (fgn == -1) {
fprintf(stderr, "error in write_index (open %s): %s.\n", genfile, strerror(errno));
exit(EXIT_FAILURE);
}
/*
if (flt == -1) {
fprintf(stderr, "error in write_index (open %s): %s.\n", lutfile, strerror(errno));
exit(EXIT_FAILURE);
}
*/
if (flc == -1) {
fprintf(stderr, "error in write_index (open %s): %s.\n", lcpfile, strerror(errno));
exit(EXIT_FAILURE);
}
free(sarfile);
free(occfile);
free(genfile);
// free(lutfile);
free(lcpfile);
clock_t tstart;
size_t s = 0, stot = 0, bytes;
// Parse genome and reverse complement.
uint64_t gsize;
fprintf(stderr, "reading genome file ..."); tstart = clock();
char * genome = compact_genome(filename, &gsize);
fprintf(stderr, " done [%.3fs]\n", (clock()-tstart)*1.0/CLOCKS_PER_SEC);
// Compute suffix array.
fprintf(stderr, "computing suffix array ..."); tstart = clock();
uint64_t * sa = (uint64_t *)compute_sa(genome, gsize);
fprintf(stderr, " done [%.3fs]\n", (clock()-tstart)*1.0/CLOCKS_PER_SEC);
// Compute OCC.
uint64_t occ_size;
fprintf(stderr, "computing occ table ..."); tstart = clock();
uint64_t * occ = compute_occ(genome, sa, gsize, &occ_size);
fprintf(stderr, " done [%.3fs]\n", (clock()-tstart)*1.0/CLOCKS_PER_SEC);
// Compress suffix array.
fprintf(stderr, "compressing suffix array ..."); tstart = clock();
uint64_t sa_bits = 0;
while (gsize > ((uint64_t)1 << sa_bits)) sa_bits++;
uint64_t sa_size = compact_array(sa, gsize, sa_bits);
sa = realloc(sa, sa_size*sizeof(uint64_t));
fprintf(stderr, " done [%.3fs]\n", (clock()-tstart)*1.0/CLOCKS_PER_SEC);
// Compute C.
fprintf(stderr, "computing C table ..."); tstart = clock();
uint64_t * c = compute_c(occ + occ_size - NUM_BASES);
fprintf(stderr, " done [%.3fs]\n", (clock()-tstart)*1.0/CLOCKS_PER_SEC);
// Compute LUT
/*
fprintf(stderr, "computing lookup table ..."); tstart = clock();
uint64_t * lut = compute_lut(c, occ, LUT_KMER_SIZE);
fprintf(stderr, " done [%.3fs]\n", (clock()-tstart)*1.0/CLOCKS_PER_SEC);
// Compress LUT.
fprintf(stderr, "compressing lookup table ..."); tstart = clock();
uint64_t lut_kmers = 1 << (2*LUT_KMER_SIZE);
uint64_t lut_size = compact_array(lut, lut_kmers, sa_bits);
lut = realloc(lut, lut_size*sizeof(uint64_t));
fprintf(stderr, " done [%.3fs]\n", (clock()-tstart)*1.0/CLOCKS_PER_SEC);
// Write .LUT file
fprintf(stderr, "writing lut...");
bytes = s = 0;
uint64_t kmer_size = LUT_KMER_SIZE;
while (s < sizeof(uint64_t)) s += write(flt, &kmer_size, sizeof(uint64_t));
bytes += s;
s = 0;
while (s < lut_size*sizeof(uint64_t)) s += write(flt, lut + s/sizeof(uint64_t), lut_size*sizeof(uint64_t) - s);
bytes += s;
stot += bytes;
fprintf(stderr, " %ld bytes written.\n",bytes);
free(lut);
*/
// Write .OCC file
fprintf(stderr, "writing occ...");
// mark interval
s = 0;
uint64_t mark_int = OCC_MARK_INTERVAL;
while (s < sizeof(uint64_t)) s += write(foc, &mark_int, sizeof(uint64_t));
stot += s;
// Write C
s = 0;
while (s < (NUM_BASES+1)*sizeof(uint64_t)) s += write(foc, c + s/sizeof(uint64_t), (NUM_BASES+1)*sizeof(uint64_t) - s);
stot += s;
// Write OCC.
s = 0;
while (s < occ_size * sizeof(uint64_t)) s += write(foc,occ + s/sizeof(uint64_t), occ_size*sizeof(uint64_t) - s);
stot += s;
fprintf(stderr, " %ld bytes written.\n",stot);
free(c); free(occ);
// Compute LCP.
fprintf(stderr, "computing LCP intervals..."); tstart = clock();
lcp_t lcp = compute_lcp(gsize, LCP_MIN_DEPTH, sa_bits, sa, genome);
fprintf(stderr, " done [%.3fs]\n", (clock()-tstart)*1.0/CLOCKS_PER_SEC);
// Write .GEN FILE
fprintf(stderr, "writing gen...");
// Write genome bases (forward and reverse strand).
s = 0;
while (s < gsize*sizeof(char)) s += write(fgn, genome + s/sizeof(char), gsize*sizeof(char) - s);
stot += s;
fprintf(stderr, " %ld bytes written.\n",s);
// .SAR FILE
fprintf(stderr, "writing sar...");
// Write sa word width.
bytes = s = 0;
while (s < sizeof(uint64_t)) s += write(fsa, &sa_bits, sizeof(uint64_t));
bytes += s;
s = 0;
while (s < sa_size*sizeof(uint64_t)) s += write(fsa, sa + s/sizeof(uint64_t), sa_size*sizeof(uint64_t) - s);
bytes += s;
stot += bytes;
fprintf(stderr, " %ld bytes written.\n",bytes);
// .LCP FILE
fprintf(stderr, "writing lcp...");
// LCP index.
bytes = s = 0;
mark_int = LCP_MARK_INTERVAL;
while (s < sizeof(uint64_t)) s += write(flc, &mark_int, sizeof(uint64_t));
bytes += s;
s = 0;
uint64_t min_depth = LCP_MIN_DEPTH;
while (s < sizeof(uint64_t)) s += write(flc, &min_depth, sizeof(uint64_t));
bytes += s;
// Write sample index szie.
s = 0;
while(s < sizeof(uint64_t)) s += write(flc, &(lcp.lcpidx_size), sizeof(uint64_t));
bytes += s;
// Write sample index.
s = 0;
while(s < lcp.lcpidx_size*sizeof(uint64_t)) s += write(flc, lcp.idx_sample + s/sizeof(uint64_t), lcp.lcpidx_size*sizeof(uint64_t) - s);
bytes += s;
// Write extended index size.
s = 0;
while(s < sizeof(uint64_t)) s += write(flc, &(lcp.extidx_size), sizeof(uint64_t));
bytes += s;
// Write extended index.
s = 0;
while(s < lcp.extidx_size*sizeof(uint64_t)) s += write(flc, lcp.idx_extend + s/sizeof(uint64_t), lcp.extidx_size*sizeof(uint64_t) - s);
bytes += s;
// Write LCP samples.
s = 0;
uint64_t nsamples = (lcp.lcp_sample)->pos;
while(s < sizeof(uint64_t)) s+= write(flc, &nsamples, sizeof(uint64_t));
bytes += s;
s = 0;
while(s < nsamples*sizeof(int8_t)) s += write(flc, (lcp.lcp_sample)->val + s/sizeof(int8_t), nsamples * sizeof(int8_t) - s);
bytes += s;
// Write ext samples.
s = 0;
nsamples = (lcp.lcp_extend)->pos;
while(s < sizeof(uint64_t)) s+= write(flc, &nsamples, sizeof(uint64_t));
bytes += s;
s = 0;
while(s < nsamples*sizeof(int64_t)) s += write(flc, (lcp.lcp_extend)->val + s/sizeof(int64_t), nsamples * sizeof(int64_t) - s);
bytes += s;
stot += bytes;
fprintf(stderr, " %ld bytes written.\n",bytes);
fprintf(stderr, "done. %ld bytes written.\n", stot);
return 0;
}