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