示例#1
0
void Paw::prepare_to_move(float position[3])
{
	coords.m_prepare_coords = {position[coord_x], position[coord_y], position[coord_z]};

	m_servo_angles = compute_angles({position[coord_x], position[coord_y], position[coord_z]});
	//use Paw_math_model to get servos angles
	determine_servos_paw_time();

	m_error_detection->set_paw(*this);
}
示例#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;
}	
示例#3
0
void glRouteWithFeet::create_from_multi( glMultiRoute& mMulti )
{
    glRoute::create_from_multi( mMulti );
    compute_angles         ( );
    generate_steps_vertices( );
}