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(); }