bool check_boundary_point(Point const& point) const
{
init();
if (! m_has_boundary)
return false;
std::size_t count = count_equal(m_endpoints.begin(), m_endpoints.end(), point);
return count % 2 != 0; // odd count -> boundary
}
void
test()
{
int ia[] = {0, 1, 2, 3, 4, 5, 0, 1, 2, 3, 4, 0, 1, 2, 3, 0, 1, 2, 0, 1, 0};
const unsigned sa = sizeof(ia)/sizeof(ia[0]);
int b[] = {0};
count_equal::count = 0;
assert(std::find_end(Iter1(ia), Iter1(ia+sa), Iter2(b), Iter2(b+1), count_equal()) == Iter1(ia+sa-1));
assert(count_equal::count <= 1*(sa-1+1));
int c[] = {0, 1};
count_equal::count = 0;
assert(std::find_end(Iter1(ia), Iter1(ia+sa), Iter2(c), Iter2(c+2), count_equal()) == Iter1(ia+18));
assert(count_equal::count <= 2*(sa-2+1));
int d[] = {0, 1, 2};
count_equal::count = 0;
assert(std::find_end(Iter1(ia), Iter1(ia+sa), Iter2(d), Iter2(d+3), count_equal()) == Iter1(ia+15));
assert(count_equal::count <= 3*(sa-3+1));
int e[] = {0, 1, 2, 3};
count_equal::count = 0;
assert(std::find_end(Iter1(ia), Iter1(ia+sa), Iter2(e), Iter2(e+4), count_equal()) == Iter1(ia+11));
assert(count_equal::count <= 4*(sa-4+1));
int f[] = {0, 1, 2, 3, 4};
count_equal::count = 0;
assert(std::find_end(Iter1(ia), Iter1(ia+sa), Iter2(f), Iter2(f+5), count_equal()) == Iter1(ia+6));
assert(count_equal::count <= 5*(sa-5+1));
int g[] = {0, 1, 2, 3, 4, 5};
count_equal::count = 0;
assert(std::find_end(Iter1(ia), Iter1(ia+sa), Iter2(g), Iter2(g+6), count_equal()) == Iter1(ia));
assert(count_equal::count <= 6*(sa-6+1));
int h[] = {0, 1, 2, 3, 4, 5, 6};
count_equal::count = 0;
assert(std::find_end(Iter1(ia), Iter1(ia+sa), Iter2(h), Iter2(h+7), count_equal()) == Iter1(ia+sa));
assert(count_equal::count <= 7*(sa-7+1));
count_equal::count = 0;
assert(std::find_end(Iter1(ia), Iter1(ia+sa), Iter2(b), Iter2(b), count_equal()) == Iter1(ia+sa));
assert(count_equal::count <= 0);
count_equal::count = 0;
assert(std::find_end(Iter1(ia), Iter1(ia), Iter2(b), Iter2(b+1), count_equal()) == Iter1(ia));
assert(count_equal::count <= 0);
}
void
test()
{
int ia[] = {0, 1, 2, 3, 4, 5};
const unsigned sa = sizeof(ia)/sizeof(ia[0]);
count_equal::count = 0;
assert(std::search_n(Iter(ia), Iter(ia+sa), 0, 0, count_equal()) == Iter(ia));
assert(count_equal::count <= sa);
count_equal::count = 0;
assert(std::search_n(Iter(ia), Iter(ia+sa), 1, 0, count_equal()) == Iter(ia+0));
assert(count_equal::count <= sa);
count_equal::count = 0;
assert(std::search_n(Iter(ia), Iter(ia+sa), 2, 0, count_equal()) == Iter(ia+sa));
assert(count_equal::count <= sa);
count_equal::count = 0;
assert(std::search_n(Iter(ia), Iter(ia+sa), sa, 0, count_equal()) == Iter(ia+sa));
assert(count_equal::count <= sa);
count_equal::count = 0;
assert(std::search_n(Iter(ia), Iter(ia+sa), 0, 3, count_equal()) == Iter(ia));
assert(count_equal::count <= sa);
count_equal::count = 0;
assert(std::search_n(Iter(ia), Iter(ia+sa), 1, 3, count_equal()) == Iter(ia+3));
assert(count_equal::count <= sa);
count_equal::count = 0;
assert(std::search_n(Iter(ia), Iter(ia+sa), 2, 3, count_equal()) == Iter(ia+sa));
assert(count_equal::count <= sa);
count_equal::count = 0;
assert(std::search_n(Iter(ia), Iter(ia+sa), sa, 3, count_equal()) == Iter(ia+sa));
assert(count_equal::count <= sa);
count_equal::count = 0;
assert(std::search_n(Iter(ia), Iter(ia+sa), 0, 5, count_equal()) == Iter(ia));
assert(count_equal::count <= sa);
count_equal::count = 0;
assert(std::search_n(Iter(ia), Iter(ia+sa), 1, 5, count_equal()) == Iter(ia+5));
assert(count_equal::count <= sa);
count_equal::count = 0;
assert(std::search_n(Iter(ia), Iter(ia+sa), 2, 5, count_equal()) == Iter(ia+sa));
assert(count_equal::count <= sa);
count_equal::count = 0;
assert(std::search_n(Iter(ia), Iter(ia+sa), sa, 5, count_equal()) == Iter(ia+sa));
assert(count_equal::count <= sa);
count_equal::count = 0;
int ib[] = {0, 0, 1, 1, 2, 2};
const unsigned sb = sizeof(ib)/sizeof(ib[0]);
assert(std::search_n(Iter(ib), Iter(ib+sb), 0, 0, count_equal()) == Iter(ib));
assert(count_equal::count <= sb);
count_equal::count = 0;
assert(std::search_n(Iter(ib), Iter(ib+sb), 1, 0, count_equal()) == Iter(ib+0));
assert(count_equal::count <= sb);
count_equal::count = 0;
assert(std::search_n(Iter(ib), Iter(ib+sb), 2, 0, count_equal()) == Iter(ib+0));
assert(count_equal::count <= sb);
count_equal::count = 0;
assert(std::search_n(Iter(ib), Iter(ib+sb), 3, 0, count_equal()) == Iter(ib+sb));
assert(count_equal::count <= sb);
count_equal::count = 0;
assert(std::search_n(Iter(ib), Iter(ib+sb), sb, 0, count_equal()) == Iter(ib+sb));
assert(count_equal::count <= sb);
count_equal::count = 0;
assert(std::search_n(Iter(ib), Iter(ib+sb), 0, 1, count_equal()) == Iter(ib));
assert(count_equal::count <= sb);
count_equal::count = 0;
assert(std::search_n(Iter(ib), Iter(ib+sb), 1, 1, count_equal()) == Iter(ib+2));
assert(count_equal::count <= sb);
count_equal::count = 0;
assert(std::search_n(Iter(ib), Iter(ib+sb), 2, 1, count_equal()) == Iter(ib+2));
assert(count_equal::count <= sb);
count_equal::count = 0;
assert(std::search_n(Iter(ib), Iter(ib+sb), 3, 1, count_equal()) == Iter(ib+sb));
assert(count_equal::count <= sb);
count_equal::count = 0;
assert(std::search_n(Iter(ib), Iter(ib+sb), sb, 1, count_equal()) == Iter(ib+sb));
assert(count_equal::count <= sb);
count_equal::count = 0;
assert(std::search_n(Iter(ib), Iter(ib+sb), 0, 2, count_equal()) == Iter(ib));
assert(count_equal::count <= sb);
count_equal::count = 0;
assert(std::search_n(Iter(ib), Iter(ib+sb), 1, 2, count_equal()) == Iter(ib+4));
assert(count_equal::count <= sb);
count_equal::count = 0;
assert(std::search_n(Iter(ib), Iter(ib+sb), 2, 2, count_equal()) == Iter(ib+4));
assert(count_equal::count <= sb);
count_equal::count = 0;
assert(std::search_n(Iter(ib), Iter(ib+sb), 3, 2, count_equal()) == Iter(ib+sb));
assert(count_equal::count <= sb);
count_equal::count = 0;
assert(std::search_n(Iter(ib), Iter(ib+sb), sb, 2, count_equal()) == Iter(ib+sb));
assert(count_equal::count <= sb);
count_equal::count = 0;
int ic[] = {0, 0, 0};
const unsigned sc = sizeof(ic)/sizeof(ic[0]);
assert(std::search_n(Iter(ic), Iter(ic+sc), 0, 0, count_equal()) == Iter(ic));
assert(count_equal::count <= sc);
count_equal::count = 0;
assert(std::search_n(Iter(ic), Iter(ic+sc), 1, 0, count_equal()) == Iter(ic));
assert(count_equal::count <= sc);
count_equal::count = 0;
assert(std::search_n(Iter(ic), Iter(ic+sc), 2, 0, count_equal()) == Iter(ic));
assert(count_equal::count <= sc);
count_equal::count = 0;
assert(std::search_n(Iter(ic), Iter(ic+sc), 3, 0, count_equal()) == Iter(ic));
assert(count_equal::count <= sc);
count_equal::count = 0;
assert(std::search_n(Iter(ic), Iter(ic+sc), 4, 0, count_equal()) == Iter(ic+sc));
assert(count_equal::count <= sc);
count_equal::count = 0;
// Check that we properly convert the size argument to an integral.
std::search_n(Iter(ic), Iter(ic+sc), UserDefinedIntegral<unsigned>(4), 0, count_equal());
count_equal::count = 0;
}
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 test_iter() {
const int ia[] = {0};
const unsigned sa = sizeof(ia)/sizeof(ia[0]);
int ja[sa] = {-1};
count_equal::count = 0;
ranges::unique_copy_result<InIter, OutIter> r =
ranges::unique_copy(InIter(ia), Sent(ia+sa), OutIter(ja), count_equal());
CHECK(base(r.in) == ia + sa);
CHECK(base(r.out) == ja + sa);
CHECK(ja[0] == 0);
CHECK(count_equal::count == sa - 1);
const int ib[] = {0, 1};
const unsigned sb = sizeof(ib)/sizeof(ib[0]);
int jb[sb] = {-1};
count_equal::count = 0;
r = ranges::unique_copy(InIter(ib), Sent(ib+sb), OutIter(jb), count_equal());
CHECK(base(r.in) == ib + sb);
CHECK(base(r.out) == jb + sb);
CHECK(jb[0] == 0);
CHECK(jb[1] == 1);
CHECK(count_equal::count == sb - 1);
const int ic[] = {0, 0};
const unsigned sc = sizeof(ic)/sizeof(ic[0]);
int jc[sc] = {-1};
count_equal::count = 0;
r = ranges::unique_copy(InIter(ic), Sent(ic+sc), OutIter(jc), count_equal());
CHECK(base(r.in) == ic + sc);
CHECK(base(r.out) == jc + 1);
CHECK(jc[0] == 0);
CHECK(count_equal::count == sc - 1);
const int id[] = {0, 0, 1};
const unsigned sd = sizeof(id)/sizeof(id[0]);
int jd[sd] = {-1};
count_equal::count = 0;
r = ranges::unique_copy(InIter(id), Sent(id+sd), OutIter(jd), count_equal());
CHECK(base(r.in) == id + sd);
CHECK(base(r.out) == jd + 2);
CHECK(jd[0] == 0);
CHECK(jd[1] == 1);
CHECK(count_equal::count == sd - 1);
const int ie[] = {0, 0, 1, 0};
const unsigned se = sizeof(ie)/sizeof(ie[0]);
int je[se] = {-1};
count_equal::count = 0;
r = ranges::unique_copy(InIter(ie), Sent(ie+se), OutIter(je), count_equal());
CHECK(base(r.in) == ie + se);
CHECK(base(r.out) == je + 3);
CHECK(je[0] == 0);
CHECK(je[1] == 1);
CHECK(je[2] == 0);
CHECK(count_equal::count == se - 1);
const int ig[] = {0, 0, 1, 1};
const unsigned sg = sizeof(ig)/sizeof(ig[0]);
int jg[sg] = {-1};
count_equal::count = 0;
r = ranges::unique_copy(InIter(ig), Sent(ig+sg), OutIter(jg), count_equal());
CHECK(base(r.in) == ig + sg);
CHECK(base(r.out) == jg + 2);
CHECK(jg[0] == 0);
CHECK(jg[1] == 1);
CHECK(count_equal::count == sg - 1);
const int ih[] = {0, 1, 1};
const unsigned sh = sizeof(ih)/sizeof(ih[0]);
int jh[sh] = {-1};
count_equal::count = 0;
r = ranges::unique_copy(InIter(ih), Sent(ih+sh), OutIter(jh), count_equal());
CHECK(base(r.in) == ih + sh);
CHECK(base(r.out) == jh + 2);
CHECK(jh[0] == 0);
CHECK(jh[1] == 1);
CHECK(count_equal::count == sh - 1);
const int ii[] = {0, 1, 1, 1, 2, 2, 2};
const unsigned si = sizeof(ii)/sizeof(ii[0]);
int ji[si] = {-1};
count_equal::count = 0;
r = ranges::unique_copy(InIter(ii), Sent(ii+si), OutIter(ji), count_equal());
CHECK(base(r.in) == ii + si);
CHECK(base(r.out) == ji + 3);
CHECK(ji[0] == 0);
CHECK(ji[1] == 1);
CHECK(ji[2] == 2);
CHECK(count_equal::count == si - 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;
}
