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; }