Ejemplo n.º 1
0
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());
}
Ejemplo n.º 2
0
/********************************************************************************
 	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;
}
Ejemplo n.º 4
0
/*
 *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;
}
Ejemplo n.º 5
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");
}
Ejemplo n.º 6
0
/**
 *  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 );
}
Ejemplo n.º 7
0
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;
}