diff --git a/package.xml b/package.xml
index 0b27e6bcad6f5287b9e6fd0216299abe0fb1a1c5..39eb573c7757b75fd44507437336888aa780fd2c 100644
--- a/package.xml
+++ b/package.xml
@@ -24,5 +24,6 @@
<buildtool_depend>cmake</buildtool_depend>
<run_depend>catkin</run_depend>
+ <run_depend>libgsl</run_depend>
<export />
</package>
diff --git a/sm/CMakeLists.txt b/sm/CMakeLists.txt
index 19b9951070eac861a8bcfdcca59e74ee92909cf5..2cf939f964b9f79f7e6642eca720215de1ea40ba 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
@@ -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)
diff --git a/sm/csm/algos.h b/sm/csm/algos.h
index abcbd1039ef937000bfbbe1a60e51e217afd47aa..140420f727d9f7bdf9d5ddbe44091bd852091ab3 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 4472ea2792afd6bd7c0b7d71f7d807c3ef864737..8c5cb52343e9889d050996248c363ce67b78d384 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 298bfcd6977e360b4d87b0e4138ebbdd29703a54..8dd597ae52eb72088dd2c6e445950be78c0eb290 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);