From 7949a178fe8d66cd11e219bae6faba7bd9b1309c Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 2 Nov 2015 11:52:22 +0900 Subject: [PATCH 1/4] package.xml : add libgsl to run_depend see https://github.com/AndreaCensi/csm/blob/master/sm/pkg-config/csm.pc.in#L7 --- package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/package.xml b/package.xml index 0b27e6bc..39eb573c 100644 --- a/package.xml +++ b/package.xml @@ -24,5 +24,6 @@ cmake catkin + libgsl From 24c18387096df94245656ba65babe8defbccd374 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 2 Nov 2015 11:53:43 +0900 Subject: [PATCH 2/4] sm/CMakeLists.txt use TARGETS instaed of PROGRAMS, this fails if you compile on different directory --- sm/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sm/CMakeLists.txt b/sm/CMakeLists.txt index 19b99510..97787c34 100644 --- a/sm/CMakeLists.txt +++ b/sm/CMakeLists.txt @@ -81,7 +81,7 @@ SUBDIRS(csm) MACRO(new_executable exec) ADD_EXECUTABLE(${exec} apps/${exec}.c) TARGET_LINK_LIBRARIES(${exec} csm-static ${csm_link_flags}) - INSTALL(PROGRAMS ${exec} DESTINATION bin) + INSTALL(TARGETS ${exec} DESTINATION bin) ENDMACRO(new_executable exec) # new_executable(sm0) From 6fe14f7d3ff0ea5961db0254b2c912aa11d1a783 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 2 Nov 2015 11:54:59 +0900 Subject: [PATCH 3/4] sm/CMakeLists.txt add -fPIC (relocation R_X86_64_32 against '.rodata' can not be used when making a shared object; recompile with -fPIC) --- sm/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sm/CMakeLists.txt b/sm/CMakeLists.txt index 97787c34..2cf939f9 100644 --- a/sm/CMakeLists.txt +++ b/sm/CMakeLists.txt @@ -56,7 +56,7 @@ MESSAGE(STATUS "csm_link_flags = ${csm_link_flags}") SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${csm_c_flags}") SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c99") -SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -ggdb -Wall") +SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC -ggdb -Wall") # for realpath From 17768c9f2c7b039832eb71c6d3d1796b3bbc7e4e Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 2 Nov 2015 11:56:23 +0900 Subject: [PATCH 4/4] add cm_icp_xy from https://github.com/ccny-ros-pkg/scan_tools/tree/indigo/laser_scan_matcher/csm/patches --- sm/csm/algos.h | 1 + sm/csm/icp/icp.c | 164 +++++++++++++++++++++++++++++++++++++++++++++++ sm/csm/icp/icp.h | 2 + 3 files changed, 167 insertions(+) diff --git a/sm/csm/algos.h b/sm/csm/algos.h index abcbd103..140420f7 100644 --- a/sm/csm/algos.h +++ b/sm/csm/algos.h @@ -159,6 +159,7 @@ struct sm_result { void sm_icp(struct sm_params*input, struct sm_result*output); +void sm_icp_xy(struct sm_params*input, struct sm_result*output); void sm_gpm(struct sm_params*input, struct sm_result*output); void sm_hsm(struct sm_params*input, struct sm_result*output); diff --git a/sm/csm/icp/icp.c b/sm/csm/icp/icp.c index 4472ea27..8c5cb523 100644 --- a/sm/csm/icp/icp.c +++ b/sm/csm/icp/icp.c @@ -185,3 +185,167 @@ void sm_icp(struct sm_params*params, struct sm_result*res) { if(JJ) jj_context_exit(); } + +void sm_icp_xy(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 */ + + 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)); + 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(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(x_new); + gsl_vector_free(x_old); + gsl_vector_free(best_x); + } +/* + egsl_pop_named("sm_icp"); + + if(JJ) jj_context_exit(); +*/ +} diff --git a/sm/csm/icp/icp.h b/sm/csm/icp/icp.h index 298bfcd6..8dd597ae 100644 --- a/sm/csm/icp/icp.h +++ b/sm/csm/icp/icp.h @@ -8,6 +8,8 @@ /** This sets the stage. */ void sm_icp(struct sm_params*params, struct sm_result*res); +void sm_icp_xy(struct sm_params*params, struct sm_result*res); + /** This is the meat */ int icp_loop(struct sm_params*params, const double*q0, double*x_new, double*total_error, int*nvalid, int*iterations);