void vectortest::test_dot()
{
float a[] = { 1, 0},
b[] = { 0, 1};
float c = vector2_dot(a, b);
ASSERT_EQUAL(c, 0);
}
// 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;
}
