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);
}
/********************************************************************************
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;
}
void glRouteWithFeet::create_from_multi( glMultiRoute& mMulti )
{
glRoute::create_from_multi( mMulti );
compute_angles ( );
generate_steps_vertices( );
}