static Rboolean any_nan_list(SEXP x) {
const R_xlen_t nx = xlength(x);
for (R_xlen_t i = 0; i < nx; i++) {
if (any_nan(VECTOR_ELT(x, i)))
return TRUE;
}
return FALSE;
}
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 main(int argc, const char*argv[]) {
sm_set_program_name(argv[0]);
struct sm_params params;
struct sm_result result;
struct option* ops = options_allocate(100);
options_string(ops, "in", &p.file_in, "stdin", "Input file ");
options_string(ops, "out", &p.file_out, "stdout", "Output file ");
options_string(ops, "out_stats", &p.file_out_stats, "", "Output file (stats) ");
options_string(ops, "file_jj", &p.file_jj, "",
"File for journaling -- if left empty, journal not open.");
options_int(ops, "algo", &p.algo, 0, "Which algorithm to use (0:(pl)ICP 1:gpm-stripped 2:HSM) ");
options_int(ops, "debug", &p.debug, 0, "Shows debug information");
options_int(ops, "recover_from_error", &p.recover_from_error, 0, "If true, tries to recover from an ICP matching error");
p.format = 0;
/* options_int(ops, "format", &p.format, 0,
"Output format (0: log in JSON format, 1: log in Carmen format (not implemented))");*/
sm_options(¶ms, ops);
if(!options_parse_args(ops, argc, argv)) {
fprintf(stderr, "\n\nUsage:\n");
options_print_help(ops, stderr);
return -1;
}
sm_debug_write(p.debug);
/* Open input and output files */
FILE * file_in = open_file_for_reading(p.file_in);
if(!file_in) return -1;
FILE * file_out = open_file_for_writing(p.file_out);
if(!file_out) return -1;
if(strcmp(p.file_jj, "")) {
FILE * jj = open_file_for_writing(p.file_jj);
if(!jj) return -1;
jj_set_stream(jj);
}
FILE * file_out_stats = 0;
if(strcmp(p.file_out_stats, "")) {
file_out_stats = open_file_for_writing(p.file_out_stats);
if(!file_out_stats) return -1;
}
/* Read first scan */
LDP laser_ref;
if(!(laser_ref = ld_read_smart(file_in))) {
sm_error("Could not read first scan.\n");
return -1;
}
if(!ld_valid_fields(laser_ref)) {
sm_error("Invalid laser data in first scan.\n");
return -2;
}
/* For the first scan, set estimate = odometry */
copy_d(laser_ref->odometry, 3, laser_ref->estimate);
spit(laser_ref, file_out);
int count=-1;
LDP laser_sens;
while( (laser_sens = ld_read_smart(file_in)) ) {
count++;
if(!ld_valid_fields(laser_sens)) {
sm_error("Invalid laser data in (#%d in file).\n", count);
return -(count+2);
}
params.laser_ref = laser_ref;
params.laser_sens = laser_sens;
/* Set first guess as the difference in odometry */
if( any_nan(params.laser_ref->odometry,3) ||
any_nan(params.laser_sens->odometry,3) ) {
sm_error("The 'odometry' field is set to NaN so I don't know how to get an initial guess. I usually use the difference in the odometry fields to obtain the initial guess.\n");
sm_error(" laser_ref->odometry = %s \n", friendly_pose(params.laser_ref->odometry) );
sm_error(" laser_sens->odometry = %s \n", friendly_pose(params.laser_sens->odometry) );
sm_error(" I will quit it here. \n");
return -3;
}
double odometry[3];
pose_diff_d(laser_sens->odometry, laser_ref->odometry, odometry);
double ominus_laser[3], temp[3];
ominus_d(params.laser, ominus_laser);
oplus_d(ominus_laser, odometry, temp);
oplus_d(temp, params.laser, params.first_guess);
/* Do the actual work */
switch(p.algo) {
case(0):
sm_icp(¶ms, &result); break;
case(1):
sm_gpm(¶ms, &result); break;
case(2):
sm_hsm(¶ms, &result); break;
default:
sm_error("Unknown algorithm to run: %d.\n",p.algo);
return -1;
}
if(!result.valid){
if(p.recover_from_error) {
sm_info("One ICP matching failed. Because you passed -recover_from_error, I will try to recover."
" Note, however, that this might not be good in some cases. \n");
sm_info("The recover is that the displacement is set to 0. No result stats is output. \n");
/* For the first scan, set estimate = odometry */
copy_d(laser_ref->estimate, 3, laser_sens->estimate);
ld_free(laser_ref); laser_ref = laser_sens;
} else {
sm_error("One ICP matching failed. Because I process recursively, I will stop here.\n");
sm_error("Use the option -recover_from_error if you want to try to recover.\n");
ld_free(laser_ref);
return 2;
}
} else {
/* Add the result to the previous estimate */
oplus_d(laser_ref->estimate, result.x, laser_sens->estimate);
/* Write the corrected log */
spit(laser_sens, file_out);
/* Write the statistics (if required) */
if(file_out_stats) {
JO jo = result_to_json(¶ms, &result);
fputs(jo_to_string(jo), file_out_stats);
fputs("\n", file_out_stats);
jo_free(jo);
}
ld_free(laser_ref); laser_ref = laser_sens;
}
}
ld_free(laser_ref);
return 0;
}
SEXP attribute_hidden c_any_nan(SEXP x) {
return ScalarLogical(any_nan(x));
}
