void sm_icp(struct sm_params*params, struct sm_result*res) {
res->valid = 0;
LDP laser_ref = params->laser_ref;
LDP laser_sens = params->laser_sens;
if(!ld_valid_fields(laser_ref) ||
!ld_valid_fields(laser_sens)) {
return;
}
sm_debug("sm_icp: laser_sens has %d/%d; laser_ref has %d/%d rays valid\n",
count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays,
count_equal(laser_ref->valid, laser_ref->nrays, 1), laser_ref->nrays);
/** Mark as invalid the rays outside of (min_reading, max_reading] */
ld_invalid_if_outside(laser_ref, params->min_reading, params->max_reading);
ld_invalid_if_outside(laser_sens, params->min_reading, params->max_reading);
sm_debug("sm_icp: laser_sens has %d/%d; laser_ref has %d/%d rays valid (after removing outside interval [%f, %f])\n",
count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays,
count_equal(laser_ref->valid, laser_ref->nrays, 1), laser_ref->nrays,
params->min_reading, params->max_reading);
if(JJ) jj_context_enter("sm_icp");
egsl_push_named("sm_icp");
if(params->use_corr_tricks || params->debug_verify_tricks)
ld_create_jump_tables(laser_ref);
ld_compute_cartesian(laser_ref);
ld_compute_cartesian(laser_sens);
if(params->do_alpha_test) {
ld_simple_clustering(laser_ref, params->clustering_threshold);
ld_compute_orientation(laser_ref, params->orientation_neighbourhood, params->sigma);
ld_simple_clustering(laser_sens, params->clustering_threshold);
ld_compute_orientation(laser_sens, params->orientation_neighbourhood, params->sigma);
}
if(JJ) jj_add("laser_ref", ld_to_json(laser_ref));
if(JJ) jj_add("laser_sens", ld_to_json(laser_sens));
gsl_vector * x_new = gsl_vector_alloc(3);
gsl_vector * x_old = vector_from_array(3, params->first_guess);
if(params->do_visibility_test) {
sm_debug("laser_ref:\n");
visibilityTest(laser_ref, x_old);
sm_debug("laser_sens:\n");
gsl_vector * minus_x_old = gsl_vector_alloc(3);
ominus(x_old,minus_x_old);
visibilityTest(laser_sens, minus_x_old);
gsl_vector_free(minus_x_old);
}
double error;
int iterations;
int nvalid;
if(!icp_loop(params, x_old->data, x_new->data, &error, &nvalid, &iterations)) {
sm_error("icp: ICP failed for some reason. \n");
res->valid = 0;
res->iterations = iterations;
res->nvalid = 0;
} else {
/* It was succesfull */
int restarted = 0;
double best_error = error;
gsl_vector * best_x = gsl_vector_alloc(3);
gsl_vector_memcpy(best_x, x_new);
if(params->restart &&
(error/nvalid)>(params->restart_threshold_mean_error) ) {
sm_debug("Restarting: %f > %f \n",(error/nvalid),(params->restart_threshold_mean_error));
restarted = 1;
double dt = params->restart_dt;
double dth = params->restart_dtheta;
sm_debug("icp_loop: dt = %f dtheta= %f deg\n",dt,rad2deg(dth));
double perturb[6][3] = {
{dt,0,0}, {-dt,0,0},
{0,dt,0}, {0,-dt,0},
{0,0,dth}, {0,0,-dth}
};
int a; for(a=0;a<6;a++){
sm_debug("-- Restarting with perturbation #%d\n", a);
struct sm_params my_params = *params;
gsl_vector * start = gsl_vector_alloc(3);
gvs(start, 0, gvg(x_new,0)+perturb[a][0]);
gvs(start, 1, gvg(x_new,1)+perturb[a][1]);
gvs(start, 2, gvg(x_new,2)+perturb[a][2]);
gsl_vector * x_a = gsl_vector_alloc(3);
double my_error; int my_valid; int my_iterations;
if(!icp_loop(&my_params, start->data, x_a->data, &my_error, &my_valid, &my_iterations)){
sm_error("Error during restart #%d/%d. \n", a, 6);
break;
}
iterations+=my_iterations;
if(my_error < best_error) {
sm_debug("--Perturbation #%d resulted in error %f < %f\n", a,my_error,best_error);
gsl_vector_memcpy(best_x, x_a);
best_error = my_error;
}
gsl_vector_free(x_a); gsl_vector_free(start);
}
}
/* At last, we did it. */
res->valid = 1;
vector_to_array(best_x, res->x);
sm_debug("icp: final x = %s \n", gsl_friendly_pose(best_x));
if (restarted) { // recompute correspondences in case of restarts
ld_compute_world_coords(laser_sens, res->x);
if(params->use_corr_tricks)
find_correspondences_tricks(params);
else
find_correspondences(params);
}
if(params->do_compute_covariance) {
val cov0_x, dx_dy1, dx_dy2;
compute_covariance_exact(
laser_ref, laser_sens, best_x,
&cov0_x, &dx_dy1, &dx_dy2);
val cov_x = sc(square(params->sigma), cov0_x);
/* egsl_v2da(cov_x, res->cov_x); */
res->cov_x_m = egsl_v2gslm(cov_x);
res->dx_dy1_m = egsl_v2gslm(dx_dy1);
res->dx_dy2_m = egsl_v2gslm(dx_dy2);
if(0) {
egsl_print("cov0_x", cov0_x);
egsl_print_spectrum("cov0_x", cov0_x);
val fim = ld_fisher0(laser_ref);
val ifim = inv(fim);
egsl_print("fim", fim);
egsl_print_spectrum("ifim", ifim);
}
}
res->error = best_error;
res->iterations = iterations;
res->nvalid = nvalid;
gsl_vector_free(best_x);
}
gsl_vector_free(x_new);
gsl_vector_free(x_old);
egsl_pop_named("sm_icp");
if(JJ) jj_context_exit();
}
void ld_cluster_curv(LDP ld) {
int min_cluster_size = 10;
double sigma = 0.005;
int orientation_neighbours = 4;
int npeaks = 5;
double near_peak_threshold = 0.4;
if(JJ) jj_context_enter("ld_cluster_curv");
int n = ld->nrays;
if(JJ) jj_add_int_array("a00valid", ld->valid, n);
if(JJ) jj_add_double_array("a01theta", ld->theta, n);
if(JJ) jj_add_double_array("a02readings", ld->readings, n);
ld_simple_clustering(ld, sigma*5);
/* int i=0; for(i=0;i<n;i++)
ld->cluster[i] = ld->valid[i] ? 1 : -1;*/
if(JJ) jj_add_int_array("a04cluster", ld->cluster, n);
ld_remove_small_clusters(ld, min_cluster_size);
ld_mark_cluster_as_invalid(ld, -1);
if(JJ) jj_add_int_array("a06cluster", ld->cluster, n);
double filter[10] = {.5, .4, .3, .2, .2, .2, .2, .2, .2, .2};
double deriv_filter[7] = {0, .6, .3, .2, .2, .2, .1};
double smooth_alpha[n];
double deriv_alpha[n];
int p;
if(JJ) jj_loop_enter("it");
for(p=0;p<npeaks;p++) { if(JJ) jj_loop_iteration();
if(JJ) jj_add_int_array("cluster", ld->cluster, n);
ld_compute_orientation(ld, orientation_neighbours, sigma);
int i;
for(i=0;i<ld->nrays;i++)
if(!ld->alpha_valid[i])
ld->cluster[i] = -1;
if(JJ) jj_add_double_array("alpha", ld->alpha, n);
cluster_convolve(ld->cluster, ld->alpha, n, smooth_alpha, filter, 10, 0);
if(JJ) jj_add_int_array("alpha_valid", ld->alpha_valid, n);
if(JJ) jj_add_double_array("smooth_alpha", smooth_alpha, n);
cluster_convolve(ld->cluster, smooth_alpha, n, deriv_alpha, deriv_filter, 7, 1);
if(JJ) jj_add_double_array("deriv_alpha", deriv_alpha, n);
array_abs(deriv_alpha, n);
int peak = cluster_find_max(ld->cluster, deriv_alpha, n);
if(JJ) jj_add_int("peak", peak);
int peak_cluster = ld->cluster[peak];
int up = peak; double threshold = near_peak_threshold * deriv_alpha[peak];
while(up<n-1 && (ld->cluster[up]==peak_cluster) && deriv_alpha[up+1] > threshold) up++;
int down = peak;
while(down>1 && (ld->cluster[up]==peak_cluster) && deriv_alpha[down-1] > threshold) down--;
int j;
for(j=down;j<=up;j++) {
ld->cluster[j] = -1;
ld->valid[j] = 0;
ld->readings[j] = NAN;
}
int next_cluster = ld_max_cluster_id(ld) + 1;
for(j = up+1; j<ld->nrays; j++) {
if(ld->cluster[j] == peak_cluster)
ld->cluster[j] = next_cluster;
}
}
if(JJ) jj_loop_exit();
if(JJ) jj_context_exit();
}
