// Draw a scan void lodo_draw_scan(lodo_t *self, lodo_scan_t *scan) { #ifdef GLUT_FOUND int i; vector2_t p; /* glColor3f(0, 0, 0.5); glBegin(GL_LINE_LOOP); for (i = 0; i < self->num_ranges; i++) { p.x = scan->ranges[i] * cos(i * self->range_step + self->range_start); p.y = scan->ranges[i] * sin(i * self->range_step + self->range_start); glVertex3f(p.x, p.y, 0.0); } glEnd(); */ glColor3f(0, 0, 0.5); glBegin(GL_POINTS); for (i = 0; i < self->num_ranges; i++) { if (scan->ranges[i] > self->range_max) continue; p.x = scan->ranges[i] * cos(i * self->range_step + self->range_start); p.y = scan->ranges[i] * sin(i * self->range_step + self->range_start); p = pose2_add_pos(p, self->laser_pose); glVertex3f(p.x, p.y, 0.0); } glEnd(); return; #endif }
// Project bounds into the grid void rmap_match_prepare(rmap_t *self, rmap_scan_t *scan) { int i; int ni, nj; vector2_t p, q; rmap_cell_t *cell; rmap_item_t *item; // Update the global pose of each boundary point and project into // grid for (i = 0; i < scan->num_hits; i++) { p = scan->hits[i]; q = pose2_add_pos(p, scan->pose); ni = RMAP_GRIDX(self, q.x); nj = RMAP_GRIDY(self, q.y); if (RMAP_GRID_VALID(self, ni, nj)) { cell = self->grid + RMAP_GRID_INDEX(self, ni, nj); assert(self->num_items < self->max_items); item = self->items + self->num_items++; item->scan = scan; item->local = p; item->global = q; item->next = NULL; if (cell->last != NULL) cell->last->next = item; if (cell->first == NULL) cell->first = item; cell->last = item; } } return; }
// Error function for fitting void rmap_fit_fdf(const gsl_vector *x, rmap_t *self, double *f, gsl_vector *g) { int i, n; rmap_constraint_t *con; pose2_t sa, sb; vector2_t pa, pb, qa, qb; vector2_t diff; vector2_t qa_sax, qa_say, qa_sar; vector2_t qb_sbx, qb_sby, qb_sbr; double u; double u_sax, u_say, u_sar; double u_sbx, u_sby, u_sbr; if (f) *f = 0; if (g) gsl_vector_set_zero(g); for (i = 0; i < self->num_cons; i++) { con = self->cons + i; // Suck the scan pose from the current vector sa.pos.x = gsl_vector_get(x, con->scan_a->index * 3 + 0); sa.pos.y = gsl_vector_get(x, con->scan_a->index * 3 + 1); sa.rot = gsl_vector_get(x, con->scan_a->index * 3 + 2); sb.pos.x = gsl_vector_get(x, con->scan_b->index * 3 + 0); sb.pos.y = gsl_vector_get(x, con->scan_b->index * 3 + 1); sb.rot = gsl_vector_get(x, con->scan_b->index * 3 + 2); // Local points pa = con->local_a; pb = con->local_b; // Global points qa = pose2_add_pos(pa, sa); qb = pose2_add_pos(pb, sb); // Compute difference term diff = vector2_sub(qa, qb); // Compute error term u = 0.5 * (vector2_dot(diff, diff) - self->max_dist * self->max_dist); if (u > 0.0) { u = 0.0; diff.x = 0; diff.y = 0; } // Derivatives qa_sax = vector2_set(+1, 0); qa_say = vector2_set(0, +1); qa_sar = vector2_set(-pa.x * sin(sa.rot) - pa.y * cos(sa.rot), +pa.x * cos(sa.rot) - pa.y * sin(sa.rot)); qb_sbx = vector2_set(-1, 0); qb_sby = vector2_set(0, -1); qb_sbr = vector2_set(+pb.x * sin(sb.rot) + pb.y * cos(sb.rot), -pb.x * cos(sb.rot) + pb.y * sin(sb.rot)); u_sax = vector2_dot(diff, qa_sax); u_say = vector2_dot(diff, qa_say); u_sar = vector2_dot(diff, qa_sar); u_sbx = vector2_dot(diff, qb_sbx); u_sby = vector2_dot(diff, qb_sby); u_sbr = vector2_dot(diff, qb_sbr); if (f) { *f += u; } if (g) { n = con->scan_a->index * 3 + 0; gsl_vector_set(g, n, gsl_vector_get(g, n) + u_sax); n = con->scan_a->index * 3 + 1; gsl_vector_set(g, n, gsl_vector_get(g, n) + u_say); n = con->scan_a->index * 3 + 2; gsl_vector_set(g, n, gsl_vector_get(g, n) + u_sar); n = con->scan_b->index * 3 + 0; gsl_vector_set(g, n, gsl_vector_get(g, n) + u_sbx); n = con->scan_b->index * 3 + 1; gsl_vector_set(g, n, gsl_vector_get(g, n) + u_sby); n = con->scan_b->index * 3 + 2; gsl_vector_set(g, n, gsl_vector_get(g, n) + u_sbr); } } return; }
// Match points for a single scan void rmap_match_scan(rmap_t *self, rmap_scan_t *scan_a) { int i; int ni, nj, di, dj, mi, mj; double d; rmap_constraint_t *con; rmap_constraint_t **cons; rmap_cell_t *cell; vector2_t pa, pb; vector2_t qa, qb; rmap_item_t *item; rmap_scan_t *scan_b; // Workspace for constraint map printf("num-scans = %d\n",self->num_scans); cons = new rmap_constraint_t *[self->num_scans]; // Match hits to boundaries for (i = 0; i < scan_a->num_hits; i += self->range_interval) { pa = scan_a->hits[i]; qa = pose2_add_pos(pa, scan_a->pose); ni = RMAP_GRIDX(self, qa.x); nj = RMAP_GRIDY(self, qa.y); // Reset the map from scan index to constraint pointer memset(cons, 0, self->num_scans * sizeof(cons[0])); // Look in the grid for the nearest boundary point; we have to // check all the cells in the vicinity of the hit point. for (dj = -1; dj <= +1; dj++) { for (di = -1; di <= +1; di++) { mi = ni + di; mj = nj + dj; if (RMAP_GRID_VALID(self, mi, mj)) { cell = self->grid + RMAP_GRID_INDEX(self, mi, mj); for (item = cell->first; item != NULL; item = item->next) { scan_b = item->scan; if (scan_b == scan_a) continue; pb = item->local; qb = item->global; d = vector2_mag(vector2_sub(qa, qb)); con = cons[scan_b->index]; if (con == NULL) { assert(self->num_cons < self->max_cons); con = self->cons + self->num_cons++; cons[scan_b->index] = con; con->scan_a = scan_a; con->scan_b = scan_b; con->local_a = pa; con->dist = DBL_MAX; } if (d < con->dist) { assert(con->scan_b == scan_b); con->local_b = pb; con->dist = d; } } } } } } delete [] cons; return; }
// Find correction double lodo_correct(lodo_t *self) { int i, ni, erc; vector2_t q; double r; double interval; double offset, err; double best_offset, best_err, best_outliers; gsl_function func; // Pre-compute some values used in the test function for (i = 0; i < self->num_ranges; i++) { // Get range relative to laser r = self->scan.ranges[i]; if (r > self->range_max) { self->scan_points[i].ni = -1; continue; } // Compute cartesian points relative to robot q.x = r * cos(i * self->range_step + self->range_start); q.y = r * sin(i * self->range_step + self->range_start); //printf("ni=%d\n",ni); q = pose2_add_pos(q, self->laser_pose); // Compute range relative to robot r = vector2_mag(q); ni = LODO_LTOR(r); //printf ("ni = %d\tself->pef_num_ranges = %d\n", ni, self->pef_num_ranges); if (ni < 0 || ni > self->pef_num_ranges) ni = -1; self->scan_points[i].ni = ni; } best_offset = 0.0; best_err = DBL_MAX; // Initialize the minimizer func.function = (double (*) (double, void*)) lodo_correct_func; func.params = self; erc = gsl_min_fminimizer_set(self->mini, &func, 0.0, -self->fit_interval, +self->fit_interval); // If the minimizer failes, revert to exhaustive search if (erc != GSL_SUCCESS) { //printf("brute force\n\n"); best_err = DBL_MAX; for (i = -100; i <= 100; i++) { offset = i * self->fit_interval / 100.0; err = lodo_test_offset(self, offset, NULL); //printf("%d %f %f\n", i, offset, err); if (err < best_err) { best_err = err; best_offset = offset; } } //printf("\n\n"); } else { for (i = 0; i < 10; i++) // HACK { erc = gsl_min_fminimizer_iterate(self->mini); if (erc == GSL_EBADFUNC) assert(0); if (erc == GSL_FAILURE) break; // Test for convergence interval = gsl_min_fminimizer_x_upper(self->mini); interval -= gsl_min_fminimizer_x_lower(self->mini); if (interval < 0.01 * M_PI / 180) // HACK break; } best_offset = gsl_min_fminimizer_x_minimum(self->mini); best_err = gsl_min_fminimizer_f_minimum(self->mini); } // Re-do the test to get the outlier count lodo_test_offset(self, best_offset, &best_outliers); // Check it his is a good fit. if (best_err > self->fit_err_thresh) { self->fit_valid = 0; self->fit_add = 0; self->fit_correct = 0; return 0; } self->fit_valid = 1; // See if should add this scan to the map. The logic is this: if we // have a good fit, but there are lots of points that are "outliers" // (i.e., large-ish error value), then these points should be added // to the map. Works for both turning in place and "bursting" // through doorways. if (fabs(best_offset) < 5 * M_PI / 180) // HACK { if (best_outliers > self->fit_outlier_frac) self->fit_add = 1; else self->fit_add = 0; } // Correct the scan pose self->fit_correct = best_offset; self->scan.cpose.rot = pose2_add_rot(self->fit_correct, self->scan.cpose); return 0; }