void hsm_find_peaks_circ(int n, const double*f, double min_angle_deg, int unidir, int max_peaks,
int*peaks, int* npeaks)
{
sm_log_push("hsm_find_peaks_circ");
assert(max_peaks>0);
/* Find all local maxima for the function */
int maxima[n], nmaxima;
hsm_find_local_maxima_circ(n, f, maxima, &nmaxima);
sm_debug("Found %d of %d are local maxima.\n", nmaxima, n);
/* Sort based on value */
qsort_descending(maxima, (size_t) nmaxima, f);
*npeaks = 0;
sm_log_push("For each maximum");
/* Only retain a subset of these */
for(int m=0;m<nmaxima;m++) {
/* Here's a candidate maximum */
int candidate = maxima[m];
double candidate_angle = candidate * (2*M_PI/n);
/* Check that is not too close to the already accepted maxima */
int acceptable = 1;
for(int a=0;a<*npeaks;a++) {
int other = peaks[a];
double other_angle = other * (2*M_PI/n);
if(hsm_is_angle_between_smaller_than_deg(candidate_angle,other_angle,min_angle_deg)) {
acceptable = 0; break;
}
/* If unidir, check also +M_PI */
if(unidir)
if(hsm_is_angle_between_smaller_than_deg(candidate_angle+M_PI,other_angle,min_angle_deg)) {
acceptable = 0; break;
}
}
sm_debug("%saccepting candidate %d; lag = %d value = %f\n",
acceptable?"":"not ", m, maxima[m], f[maxima[m]]);
if(acceptable) {
peaks[*npeaks] = candidate;
(*npeaks) ++;
}
if(*npeaks>=max_peaks) break;
}
sm_log_pop();
sm_debug("found %d (max %d) maxima.\n", *npeaks, max_peaks);
sm_log_pop();
}
/**
If multiple points in laser_sens match to the same point in laser_ref,
only the nearest one wins.
Uses: laser_sens->corr, laser_sens->p
Modifies: laser_sens->corr
*/
void kill_outliers_double(struct sm_params*params) {
double threshold = 3; /* TODO: add as configurable */
LDP laser_ref = params->laser_ref;
LDP laser_sens = params->laser_sens;
double dist2_i[laser_sens->nrays];
double dist2_j[laser_ref->nrays];
int j; for(j=0;j<laser_ref->nrays;j++)
dist2_j[j]= 1000000;
int i;
for(i=0;i<laser_sens->nrays;i++) {
if(!ld_valid_corr(laser_sens, i)) continue;
int j1 = laser_sens->corr[i].j1;
dist2_i[i] = laser_sens->corr[i].dist2_j1;
dist2_j[j1] = GSL_MIN(dist2_j[j1], dist2_i[i]);
}
int nkilled = 0;
for(i=0;i<laser_sens->nrays;i++) {
if(!ld_valid_corr(laser_sens, i)) continue;
int j1 = laser_sens->corr[i].j1;
if(dist2_i[i] > (threshold*threshold)*dist2_j[j1]) {
laser_sens->corr[i].valid=0;
nkilled ++;
}
}
sm_debug("\tkill_outliers_double: killed %d correspondences\n",nkilled);
}
void hsm_find_peaks_linear(int n, const double*f, double min_dist, int max_peaks,
int*peaks, int* npeaks)
{
sm_log_push("hsm_find_peaks_linear");
assert(max_peaks>0);
/* Find all local maxima for the function */
int maxima[n], nmaxima;
hsm_find_local_maxima_linear(n,f,maxima,&nmaxima);
sm_debug("Found %d of %d are local maxima.\n", nmaxima, n);
/* Sort based on value */
qsort_descending(maxima, (size_t) nmaxima, f);
*npeaks = 0;
sm_log_push("for each maximum");
/* Only retain a subset of these */
for(int m=0;m<nmaxima;m++) {
/* Here's a candidate maximum */
int candidate = maxima[m];
/* Check that is not too close to the already accepted maxima */
int acceptable = 1;
for(int a=0;a<*npeaks;a++) {
int other = peaks[a];
if(abs(other-candidate) < min_dist) {
acceptable = 0; break;
}
}
sm_debug("%s accepting candidate %d; lag = %d value = %f\n",
acceptable?"":"not", m, maxima[m], f[maxima[m]]);
if(acceptable) {
peaks[*npeaks] = candidate;
(*npeaks) ++;
}
if(*npeaks >= max_peaks) break;
}
sm_log_pop("");
sm_debug("Found %d (max %d) maxima.\n", *npeaks, max_peaks);
sm_log_pop();
}
int main(int argc, const char * argv[]) {
sm_set_program_name(argv[0]);
const char*input_filename;
const char*output_pattern_op;
struct option* ops = options_allocate(3);
options_string(ops, "in", &input_filename, "stdin", "input file (JSON)");
options_string(ops, "out", &output_pattern_op, "./ld_split^02d.json", "output pattern; printf() pattern, but write '^' instead of '%'");
if(!options_parse_args(ops, argc, argv)) {
fprintf(stderr, "%s : splits a JSON file into many files."
"\n\nOptions:\n", argv[0]);
options_print_help(ops, stderr);
return -1;
}
/* Substitute "$" with "%" */
char output_pattern[256];
strcpy(output_pattern, output_pattern_op);
char *f = output_pattern;
while(*f) {
if(*f=='^') *f='%';
f++;
}
fputs(output_pattern, stderr);
FILE * input_stream = open_file_for_reading(input_filename);
int count = 0;
JO jo;
while( (jo = json_read_stream(input_stream)) ) {
char filename[1000];
sprintf(filename, output_pattern, count);
if(!count) {
}
sm_debug("Writing to file (%s) %s\n", output_pattern, filename);
FILE * f = open_file_for_writing(filename);
if(!f) return -1;
fputs(json_object_to_json_string(jo), f);
jo_free(jo);
fclose(f);
count++;
}
return 0;
}
/** expects cartesian valid */
void visibilityTest(LDP laser_ref, const gsl_vector*u) {
double theta_from_u[laser_ref->nrays];
int j;
for(j=0;j<laser_ref->nrays;j++) {
if(!ld_valid_ray(laser_ref,j)) continue;
theta_from_u[j] =
atan2(gvg(u,1)-laser_ref->points[j].p[1],
gvg(u,0)-laser_ref->points[j].p[0]);
}
sm_debug("\tvisibility: Found outliers: ");
int invalid = 0;
for(j=1;j<laser_ref->nrays;j++) {
if(!ld_valid_ray(laser_ref,j)||!ld_valid_ray(laser_ref,j-1)) continue;
if(theta_from_u[j]<theta_from_u[j-1]) {
laser_ref->valid[j] = 0;
invalid ++;
sm_debug("%d ",j);
}
}
sm_debug("\n");
}
/** Read next FLASER line in file, or NULL on error */
int ld_read_next_laser_carmen(FILE*file, LDP*ld) {
*ld = 0;
#define MAX_LINE_LENGTH 10000
char line[MAX_LINE_LENGTH];
while(fgets(line, MAX_LINE_LENGTH-1, file)) {
if(0 != strncmp(line, carmen_prefix, strlen(carmen_prefix))) {
sm_debug("Skipping line: \n-> %s\n", line);
continue;
}
*ld = ld_from_carmen_string(line);
if(!*ld) {
printf("Malformed line? \n-> '%s'", line);
return 0;
} else {
return 1;
}
}
return 1;
}
/**
Trims the corrispondences using an adaptive algorithm
Assumes cartesian coordinates computed. (points and points_w)
So, to disable this:
outliers_maxPerc = 1
outliers_adaptive_order = 1
*/
void kill_outliers_trim(struct sm_params*params, double*total_error) {
if(JJ) jj_context_enter("kill_outliers_trim");
LDP laser_ref = params->laser_ref;
LDP laser_sens = params->laser_sens;
/* dist2, indexed by k, contains the error for the k-th correspondence */
int k = 0;
double dist2[laser_sens->nrays];
int i;
double dist[laser_sens->nrays];
/* for each point in laser_sens */
for(i=0;i<laser_sens->nrays;i++) {
/* which has a valid correspondence */
if(!ld_valid_corr(laser_sens, i)) { dist[i]=NAN; continue; }
double *p_i_w = laser_sens->points_w[i].p;
int j1 = laser_sens->corr[i].j1;
int j2 = laser_sens->corr[i].j2;
/* Compute the distance to the corresponding segment */
dist[i]= dist_to_segment_d(
laser_ref->points[j1].p, laser_ref->points[j2].p, p_i_w);
dist2[k] = dist[i];
k++;
}
if(JJ) jj_add_int("num_valid_before", k);
if(JJ) jj_add_double_array("dist_points", dist2, laser_sens->nrays);
if(JJ) jj_add_double_array("dist_corr_unsorted", dist2, k);
#if 0
double dist2_copy[k]; for(i=0;i<k;i++) dist2_copy[i] = dist2[i];
#endif
/* two errors limits are defined: */
/* In any case, we don't want more than outliers_maxPerc% */
int order = (int)floor(k*(params->outliers_maxPerc));
order = GSL_MAX(0, GSL_MIN(order, k-1));
/* The dists for the correspondence are sorted
in ascending order */
quicksort(dist2, 0, k-1);
double error_limit1 = dist2[order];
if(JJ) jj_add_double_array("dist_corr_sorted", dist2, k);
/* Then we take a order statics (o*K) */
/* And we say that the error must be less than alpha*dist(o*K) */
int order2 = (int)floor(k*params->outliers_adaptive_order);
order2 = GSL_MAX(0, GSL_MIN(order2, k-1));
double error_limit2 = params->outliers_adaptive_mult*dist2[order2];
double error_limit = GSL_MIN(error_limit1, error_limit2);
#if 0
double error_limit1_ho = hoare_selection(dist2_copy, 0, k-1, order);
double error_limit2_ho = error_limit2;
if((error_limit1_ho != error_limit1) || (error_limit2_ho != error_limit2)) {
printf("%f == %f %f == %f\n",
error_limit1_ho, error_limit1, error_limit2_ho, error_limit2);
}
#endif
if(JJ) jj_add_double_array("dist_corr_sorted", dist2, k);
if(JJ) jj_add_double("error_limit_max_perc", error_limit1);
if(JJ) jj_add_double("error_limit_adaptive", error_limit2);
if(JJ) jj_add_double("error_limit", error_limit);
sm_debug("\ticp_outliers: maxPerc %f error_limit: fix %f adaptive %f \n",
params->outliers_maxPerc,error_limit1,error_limit2);
*total_error = 0;
int nvalid = 0;
for(i=0;i<laser_sens->nrays;i++) {
if(!ld_valid_corr(laser_sens, i)) continue;
if(dist[i] > error_limit) {
laser_sens->corr[i].valid = 0;
laser_sens->corr[i].j1 = -1;
laser_sens->corr[i].j2 = -1;
} else {
nvalid++;
*total_error += dist[i];
}
}
sm_debug("\ticp_outliers: valid %d/%d (limit: %f) mean error = %f \n",nvalid,k,error_limit,
*total_error/nvalid);
if(JJ) jj_add_int("num_valid_after", nvalid);
if(JJ) jj_add_double("total_error", *total_error);
if(JJ) jj_add_double("mean_error", *total_error / nvalid);
if(JJ) jj_context_exit();
}
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();
}
int compute_next_estimate(struct sm_params*params,
const double x_old[3], double x_new[3])
{
LDP laser_ref = params->laser_ref;
LDP laser_sens = params->laser_sens;
struct gpc_corr c[laser_sens->nrays];
int i; int k=0;
for(i=0;i<laser_sens->nrays;i++) {
if(!laser_sens->valid[i])
continue;
if(!ld_valid_corr(laser_sens,i))
continue;
int j1 = laser_sens->corr[i].j1;
int j2 = laser_sens->corr[i].j2;
c[k].valid = 1;
if(laser_sens->corr[i].type == corr_pl) {
c[k].p[0] = laser_sens->points[i].p[0];
c[k].p[1] = laser_sens->points[i].p[1];
c[k].q[0] = laser_ref->points[j1].p[0];
c[k].q[1] = laser_ref->points[j1].p[1];
/** TODO: here we could use the estimated alpha */
double diff[2];
diff[0] = laser_ref->points[j1].p[0]-laser_ref->points[j2].p[0];
diff[1] = laser_ref->points[j1].p[1]-laser_ref->points[j2].p[1];
double one_on_norm = 1 / sqrt(diff[0]*diff[0]+diff[1]*diff[1]);
double normal[2];
normal[0] = +diff[1] * one_on_norm;
normal[1] = -diff[0] * one_on_norm;
double cos_alpha = normal[0];
double sin_alpha = normal[1];
c[k].C[0][0] = cos_alpha*cos_alpha;
c[k].C[1][0] =
c[k].C[0][1] = cos_alpha*sin_alpha;
c[k].C[1][1] = sin_alpha*sin_alpha;
/* sm_debug("k=%d, i=%d sens_phi: %fdeg, j1=%d j2=%d, alpha_seg=%f, cos=%f sin=%f \n", k,i,
rad2deg(laser_sens->theta[i]), j1,j2, atan2(sin_alpha,cos_alpha), cos_alpha,sin_alpha);*/
#if 0
/* Note: it seems that because of numerical errors this matrix might be
not semidef positive. */
double det = c[k].C[0][0] * c[k].C[1][1] - c[k].C[0][1] * c[k].C[1][0];
double trace = c[k].C[0][0] + c[k].C[1][1];
int semidef = (det >= 0) && (trace>0);
if(!semidef) {
/* printf("%d: Adjusting correspondence weights\n",i);*/
double eps = -det;
c[k].C[0][0] += 2*sqrt(eps);
c[k].C[1][1] += 2*sqrt(eps);
}
#endif
} else {
c[k].p[0] = laser_sens->points[i].p[0];
c[k].p[1] = laser_sens->points[i].p[1];
projection_on_segment_d(
laser_ref->points[j1].p,
laser_ref->points[j2].p,
laser_sens->points_w[i].p,
c[k].q);
/* Identity matrix */
c[k].C[0][0] = 1;
c[k].C[1][0] = 0;
c[k].C[0][1] = 0;
c[k].C[1][1] = 1;
}
double factor = 1;
/* Scale the correspondence weight by a factor concerning the
information in this reading. */
if(params->use_ml_weights) {
int have_alpha = 0;
double alpha = 0;
if(!is_nan(laser_ref->true_alpha[j1])) {
alpha = laser_ref->true_alpha[j1];
have_alpha = 1;
} else if(laser_ref->alpha_valid[j1]) {
alpha = laser_ref->alpha[j1];;
have_alpha = 1;
} else have_alpha = 0;
if(have_alpha) {
double pose_theta = x_old[2];
/** Incidence of the ray
Note that alpha is relative to the first scan (not the world)
and that pose_theta is the angle of the second scan with
respect to the first, hence it's ok. */
double beta = alpha - (pose_theta + laser_sens->theta[i]);
factor = 1 / square(cos(beta));
} else {
static int warned_before = 0;
if(!warned_before) {
sm_error("Param use_ml_weights was active, but not valid alpha[] or true_alpha[]."
"Perhaps, if this is a single ray not having alpha, you should mark it as inactive.\n");
sm_error("Writing laser_ref: \n");
ld_write_as_json(laser_ref, stderr);
warned_before = 1;
}
}
}
/* Weight the points by the sigma in laser_sens */
if(params->use_sigma_weights) {
if(!is_nan(laser_sens->readings_sigma[i])) {
factor *= 1 / square(laser_sens->readings_sigma[i]);
} else {
static int warned_before = 0;
if(!warned_before) {
sm_error("Param use_sigma_weights was active, but the field readings_sigma[] was not filled in.\n");
sm_error("Writing laser_sens: \n");
ld_write_as_json(laser_sens, stderr);
}
}
}
c[k].C[0][0] *= factor;
c[k].C[1][0] *= factor;
c[k].C[0][1] *= factor;
c[k].C[1][1] *= factor;
k++;
}
/* TODO: use prior for odometry */
double std = 0.11;
const double inv_cov_x0[9] =
{1/(std*std), 0, 0,
0, 1/(std*std), 0,
0, 0, 0};
int ok = gpc_solve(k, c, 0, inv_cov_x0, x_new);
if(!ok) {
sm_error("gpc_solve_valid failed\n");
return 0;
}
double old_error = gpc_total_error(c, k, x_old);
double new_error = gpc_total_error(c, k, x_new);
sm_debug("\tcompute_next_estimate: old error: %f x_old= %s \n", old_error, friendly_pose(x_old));
sm_debug("\tcompute_next_estimate: new error: %f x_new= %s \n", new_error, friendly_pose(x_new));
sm_debug("\tcompute_next_estimate: new error - old_error: %g \n", new_error-old_error);
double epsilon = 0.000001;
if(new_error > old_error + epsilon) {
sm_error("\tcompute_next_estimate: something's fishy here! Old error: %lf new error: %lf x_old %lf %lf %lf x_new %lf %lf %lf\n",old_error,new_error,x_old[0],x_old[1],x_old[2],x_new[0],x_new[1],x_new[2]);
}
return 1;
}
int icp_loop(struct sm_params*params, const double*q0, double*x_new,
double*total_error, int*valid, int*iterations) {
if(any_nan(q0,3)) {
sm_error("icp_loop: Initial pose contains nan: %s\n", friendly_pose(q0));
return 0;
}
LDP laser_sens = params->laser_sens;
double x_old[3], delta[3], delta_old[3] = {0,0,0};
copy_d(q0, 3, x_old);
unsigned int hashes[params->max_iterations];
int iteration;
sm_debug("icp: starting at q0 = %s \n", friendly_pose(x_old));
if(JJ) jj_loop_enter("iterations");
int all_is_okay = 1;
for(iteration=0; iteration<params->max_iterations;iteration++) {
if(JJ) jj_loop_iteration();
if(JJ) jj_add_double_array("x_old", x_old, 3);
egsl_push_named("icp_loop iteration");
sm_debug("== icp_loop: starting iteration. %d \n", iteration);
/** Compute laser_sens's points in laser_ref's coordinates
by roto-translating by x_old */
ld_compute_world_coords(laser_sens, x_old);
/** Find correspondences (the naif or smart way) */
if(params->use_corr_tricks)
find_correspondences_tricks(params);
else
find_correspondences(params);
/** If debug_verify_tricks, make sure that find_correspondences_tricks()
and find_correspondences() return the same answer */
if(params->debug_verify_tricks)
debug_correspondences(params);
/* If not many correspondences, bail out */
int num_corr = ld_num_valid_correspondences(laser_sens);
double fail_perc = 0.05;
if(num_corr < fail_perc * laser_sens->nrays) { /* TODO: arbitrary */
sm_error(" : before trimming, only %d correspondences.\n",num_corr);
all_is_okay = 0;
egsl_pop_named("icp_loop iteration"); /* loop context */
break;
}
if(JJ) jj_add("corr0", corr_to_json(laser_sens->corr, laser_sens->nrays));
/* Kill some correspondences (using dubious algorithm) */
if(params->outliers_remove_doubles)
kill_outliers_double(params);
int num_corr2 = ld_num_valid_correspondences(laser_sens);
if(JJ) jj_add("corr1", corr_to_json(laser_sens->corr, laser_sens->nrays));
double error=0;
/* Trim correspondences */
kill_outliers_trim(params, &error);
int num_corr_after = ld_num_valid_correspondences(laser_sens);
if(JJ) {
jj_add("corr2", corr_to_json(laser_sens->corr, laser_sens->nrays));
jj_add_int("num_corr0", num_corr);
jj_add_int("num_corr1", num_corr2);
jj_add_int("num_corr2", num_corr_after);
}
*total_error = error;
*valid = num_corr_after;
sm_debug(" icp_loop: total error: %f valid %d mean = %f\n", *total_error, *valid, *total_error/ *valid);
/* If not many correspondences, bail out */
if(num_corr_after < fail_perc * laser_sens->nrays){
sm_error(" icp_loop: failed: after trimming, only %d correspondences.\n",num_corr_after);
all_is_okay = 0;
egsl_pop_named("icp_loop iteration"); /* loop context */
break;
}
/* Compute next estimate based on the correspondences */
if(!compute_next_estimate(params, x_old, x_new)) {
sm_error(" icp_loop: Cannot compute next estimate.\n");
all_is_okay = 0;
egsl_pop_named("icp_loop iteration");
break;
}
pose_diff_d(x_new, x_old, delta);
{
sm_debug(" icp_loop: killing. laser_sens has %d/%d rays valid, %d corr found -> %d after double cut -> %d after adaptive cut \n", count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays, num_corr, num_corr2, num_corr_after);
if(JJ) {
jj_add_double_array("x_new", x_new, 3);
jj_add_double_array("delta", delta, 3);
}
}
/** Checks for oscillations */
hashes[iteration] = ld_corr_hash(laser_sens);
{
sm_debug(" icp_loop: it. %d hash=%d nvalid=%d mean error = %f, x_new= %s\n",
iteration, hashes[iteration], *valid, *total_error/ *valid,
friendly_pose(x_new));
}
/** PLICP terminates in a finite number of steps! */
if(params->use_point_to_line_distance) {
int loop_detected = 0; /* TODO: make function */
int a; for(a=iteration-1;a>=0;a--) {
if(hashes[a]==hashes[iteration]) {
sm_debug("icpc: oscillation detected (cycle length = %d)\n", iteration-a);
loop_detected = 1;
break;
}
}
if(loop_detected) {
egsl_pop_named("icp_loop iteration");
break;
}
}
/* This termination criterium is useless when using
the point-to-line-distance; however, we put it here because
one can choose to use the point-to-point distance. */
if(termination_criterion(params, delta)) {
egsl_pop_named("icp_loop iteration");
break;
}
copy_d(x_new, 3, x_old);
copy_d(delta, 3, delta_old);
egsl_pop_named("icp_loop iteration");
}
if(JJ) jj_loop_exit();
*iterations = iteration+1;
return all_is_okay;
}
int log2pdf(log2pdf_params *p) {
/** First of all, we read the entire map into memory */
FILE *input_file = open_file_for_reading(p->input_filename);
if(!input_file) return 0;
LDP*scans; int nscans;
if(!ld_read_some_scans_distance(input_file, &scans, &nscans,
p->use_reference, p->distance_xy, deg2rad(p->distance_th_deg) ) ){
sm_error("Could not read map from file '%s'.\n", p->input_filename);
return 0;
}
if(nscans == 0) {
sm_error("I could not read any scan from file '%s'.\n", p->input_filename);
return 0;
}
sm_debug("Read map: %d scans in total.\n", nscans);
/** Let's find the bounding box for the map */
double bb_min[2], bb_max[2];
double offset[3] = {0,0,0};
lda_get_bounding_box(scans, nscans, bb_min, bb_max, offset, p->use_reference, p->laser.horizon);
bb_min[0] -= p->padding;
bb_min[1] -= p->padding;
bb_max[0] += p->padding;
bb_max[1] += p->padding;
sm_debug("Bounding box: %f %f -- %f %f.\n", bb_min[0], bb_min[1],
bb_max[0], bb_max[1]);
/* Create PDF surface and setup paper size and transformations */
int max_width_points = p->dimension;
int max_height_points = p->dimension;
cairo_surface_t *surface;
cairo_t *cr;
if(!create_pdf_surface(p->output_filename, max_width_points, max_height_points,
bb_min, bb_max, &surface, &cr)) return 0;
/* Draw pose path */
if(p->pose_path.draw) {
cairo_save(cr);
cr_set_style(cr, &(p->pose_path));
cr_lda_draw_pose_path(cr, scans, nscans, p->use_reference);
if(nscans > 0 && p->laser.pose.draw) {
cairo_set_source_rgb(cr, 0.3, 0.0, 1.0);
double *pose0 = ld_get_reference_pose(scans[0], p->use_reference);
cairo_arc(cr, pose0[0], pose0[1], p->start_pose_width, 0.0, 2*M_PI);
cairo_fill(cr);
}
cairo_restore(cr);
}
/* Draw map */
int k; for(k=0;k<nscans;k++) {
LDP ld = scans[k];
double *pose = ld_get_reference_pose(ld, p->use_reference);
if(!pose) continue;
double offset[3] = {0,0, deg2rad(p->offset_theta_deg) };
double world_pose[3];
oplus_d(offset, pose, world_pose);
cairo_save(cr);
cr_set_reference(cr, world_pose);
cr_ld_draw(cr, ld, &(p->laser));
cairo_restore(cr);
}
cairo_show_page (cr);
cairo_destroy (cr);
cairo_surface_destroy (surface);
return 1;
}
void hsm_match(struct hsm_params*p, hsm_buffer b1, hsm_buffer b2) {
sm_log_push("hsm_match");
/* Let's measure the time */
clock_t hsm_match_start = clock();
assert(b1->num_angular_cells == b2->num_angular_cells);
assert(p->max_translation > 0);
assert(b1->linear_cell_size > 0);
b1->num_valid_results = 0;
/* Compute cross-correlation of spectra */
hsm_circular_cross_corr_stupid(b1->num_angular_cells, b2->hs, b1->hs, b1->hs_cross_corr);
/* Find peaks in cross-correlation */
int peaks[p->num_angular_hypotheses], npeaks;
hsm_find_peaks_circ(b1->num_angular_cells, b1->hs_cross_corr, p->angular_hyp_min_distance_deg, 0, p->num_angular_hypotheses, peaks, &npeaks);
sm_debug("Found %d peaks (max %d) in cross correlation.\n", npeaks, p->num_angular_hypotheses);
if(npeaks == 0) {
sm_error("Cross correlation of spectra has 0 peaks.\n");
sm_log_pop();
return;
}
sm_log_push("loop on theta hypotheses");
/* lag e' quanto 2 si sposta a destra rispetto a 1 */
for(int np=0;np<npeaks;np++) {
int lag = peaks[np];
double theta_hypothesis = lag * (2*M_PI/b1->num_angular_cells);
sm_debug("Theta hyp#%d: lag %d, angle %fdeg\n", np, lag, rad2deg(theta_hypothesis));
/* Superimpose the two spectra */
double mult[b1->num_angular_cells];
for(int r=0;r<b1->num_angular_cells;r++)
mult[r] = b1->hs[r] * b2->hs[pos_mod(r-lag, b1->num_angular_cells)];
/* Find directions where both are intense */
int directions[p->xc_ndirections], ndirections;
hsm_find_peaks_circ(b1->num_angular_cells, b1->hs_cross_corr, p->xc_directions_min_distance_deg, 1, p->xc_ndirections, directions, &ndirections);
if(ndirections<2) {
sm_error("Too few directions.\n");
}
struct {
/* Direction of cross correlation */
double angle;
int nhypotheses;
struct {
double delta;
double value;
} hypotheses[p->linear_xc_max_npeaks];
} dirs[ndirections];
sm_debug("Using %d (max %d) correlations directions.\n", ndirections, p->xc_ndirections);
int max_lag = (int) ceil(p->max_translation / b1->linear_cell_size);
int min_lag = -max_lag;
sm_debug("Max lag: %d cells (max t: %f, cell size: %f)\n",
max_lag, p->max_translation, b1->linear_cell_size);
sm_log_push("loop on xc direction");
/* For each correlation direction */
for(int cd=0;cd<ndirections;cd++) {
dirs[cd].angle = theta_hypothesis + (directions[cd]) * (2*M_PI/b1->num_angular_cells);
printf(" cd %d angle = %d deg\n", cd, (int) rad2deg(dirs[cd].angle));
/* Do correlation */
int lags [2*max_lag + 1];
double xcorr [2*max_lag + 1];
int i1 = pos_mod(directions[cd] , b1->num_angular_cells);
int i2 = pos_mod(directions[cd] + lag , b1->num_angular_cells);
double *f1 = b1->ht[i1];
double *f2 = b2->ht[i2];
hsm_linear_cross_corr_stupid(
b2->num_linear_cells,f2,
b1->num_linear_cells,f1,
xcorr, lags, min_lag, max_lag);
/* Find peaks of cross-correlation */
int linear_peaks[p->linear_xc_max_npeaks], linear_npeaks;
hsm_find_peaks_linear(
2*max_lag + 1, xcorr, p->linear_xc_peaks_min_distance/b1->linear_cell_size,
p->linear_xc_max_npeaks, linear_peaks, &linear_npeaks);
sm_debug("theta hyp #%d: Found %d (max %d) peaks for correlation.\n",
cd, linear_npeaks, p->linear_xc_max_npeaks);
dirs[cd].nhypotheses = linear_npeaks;
sm_log_push("Considering each peak of linear xc");
for(int lp=0;lp<linear_npeaks;lp++) {
int linear_xc_lag = lags[linear_peaks[lp]];
double value = xcorr[linear_peaks[lp]];
double linear_xc_lag_m = linear_xc_lag * b1->linear_cell_size;
sm_debug("lag: %d delta: %f value: %f \n", linear_xc_lag, linear_xc_lag_m, value);
dirs[cd].hypotheses[lp].delta = linear_xc_lag_m;
dirs[cd].hypotheses[lp].value = value;
}
sm_log_pop();
if(p->debug_true_x_valid) {
double true_delta = cos(dirs[cd].angle) * p->debug_true_x[0] +
sin(dirs[cd].angle) * p->debug_true_x[1];
sm_debug("true_x delta = %f \n", true_delta );
}
} /* xc direction */
sm_log_pop();
sm_debug("Now doing all combinations. How many are there?\n");
int possible_choices[ndirections];
int num_combinations = 1;
for(int cd=0;cd<ndirections;cd++) {
possible_choices[cd] = dirs[cd].nhypotheses;
num_combinations *= dirs[cd].nhypotheses;
}
sm_debug("Total: %d combinations\n", num_combinations);
sm_log_push("For each combination..");
for(int comb=0;comb<num_combinations;comb++) {
int choices[ndirections];
hsm_generate_combinations(ndirections, possible_choices, comb, choices);
/* Linear least squares */
double M[2][2]={{0,0},{0,0}}; double Z[2]={0,0};
/* heuristic quality value */
double sum_values = 0;
for(int cd=0;cd<ndirections;cd++) {
double angle = dirs[cd].angle;
double c = cos(angle), s = sin(angle);
double w = dirs[cd].hypotheses[choices[cd]].value;
double y = dirs[cd].hypotheses[choices[cd]].delta;
M[0][0] += c * c * w;
M[1][0] += c * s * w;
M[0][1] += c * s * w;
M[1][1] += s * s * w;
Z[0] += w * c * y;
Z[1] += w * s * y;
sum_values += w;
}
double det = M[0][0]*M[1][1]-M[0][1]*M[1][0];
double Minv[2][2];
Minv[0][0] = M[1][1] * (1/det);
Minv[1][1] = M[0][0] * (1/det);
Minv[0][1] = -M[0][1] * (1/det);
Minv[1][0] = -M[1][0] * (1/det);
double t[2] = {
Minv[0][0]*Z[0] + Minv[0][1]*Z[1],
Minv[1][0]*Z[0] + Minv[1][1]*Z[1]};
/* copy result in results slot */
int k = b1->num_valid_results;
b1->results[k][0] = t[0];
b1->results[k][1] = t[1];
b1->results[k][2] = theta_hypothesis;
b1->results_quality[k] = sum_values;
b1->num_valid_results++;
}
sm_log_pop();
} /* theta hypothesis */
sm_log_pop();
/* for(int i=0;i<b1->num_valid_results;i++) {
printf("#%d %.0fdeg %.1fm %.1fm quality %f \n",i,
rad2deg(b1->results[i][2]),
b1->results[i][0],
b1->results[i][1],
b1->results_quality[i]);
}*/
/* Sorting based on values */
int indexes[b1->num_valid_results];
for(int i=0;i<b1->num_valid_results;i++)
indexes[i] = i;
qsort_descending(indexes, (size_t) b1->num_valid_results, b1->results_quality);
/* copy in the correct order*/
double*results_tmp[b1->num_valid_results];
double results_quality_tmp[b1->num_valid_results];
for(int i=0;i<b1->num_valid_results;i++) {
results_tmp[i] = b1->results[i];
results_quality_tmp[i] = b1->results_quality[i];
}
for(int i=0;i<b1->num_valid_results;i++) {
b1->results[i] = results_tmp[indexes[i]];
b1->results_quality[i] = results_quality_tmp[indexes[i]];
}
for(int i=0;i<b1->num_valid_results;i++) {
char near[256]="";
double *x = b1->results[i];
if(p->debug_true_x_valid) {
double err_th = rad2deg(fabs(angleDiff(p->debug_true_x[2],x[2])));
double err_m = hypot(p->debug_true_x[0]-x[0],
p->debug_true_x[1]-x[1]);
const char * ast = (i == 0) && (err_th > 2) ? " ***** " : "";
sprintf(near, "th err %4d err_m %5f %s",(int)err_th ,err_m,ast);
}
if(i<10)
printf("after #%d %3.1fm %.1fm %3.0fdeg quality %5.0f \t%s\n",i,
x[0],
x[1], rad2deg(x[2]), b1->results_quality[i], near);
}
/* How long did it take? */
clock_t hsm_match_stop = clock();
int ticks = hsm_match_stop-hsm_match_start;
double ctime = ((double)ticks) / CLOCKS_PER_SEC;
sm_debug("Time: %f sec (%d ticks)\n", ctime, ticks);
sm_log_pop();
}
