// Draw the cluster statistics
void pf_draw_cluster_stats(pf_t *pf, rtk_fig_t *fig)
{
int i;
pf_cluster_t *cluster;
pf_sample_set_t *set;
pf_vector_t mean;
pf_matrix_t cov;
pf_matrix_t r, d;
double weight, o, d1, d2;
set = pf->sets + pf->current_set;
for (i = 0; i < set->cluster_count; i++)
{
cluster = set->clusters + i;
weight = cluster->weight;
mean = cluster->mean;
cov = cluster->cov;
// Compute unitary representation S = R D R^T
pf_matrix_unitary(&r, &d, cov);
/* Debugging
printf("mean = \n");
pf_vector_fprintf(mean, stdout, "%e");
printf("cov = \n");
pf_matrix_fprintf(cov, stdout, "%e");
printf("r = \n");
pf_matrix_fprintf(r, stdout, "%e");
printf("d = \n");
pf_matrix_fprintf(d, stdout, "%e");
*/
// Compute the orientation of the error ellipse (first eigenvector)
o = atan2(r.m[1][0], r.m[0][0]);
d1 = 6 * sqrt(d.m[0][0]);
d2 = 6 * sqrt(d.m[1][1]);
if (d1 > 1e-3 && d2 > 1e-3)
{
// Draw the error ellipse
rtk_fig_ellipse(fig, mean.v[0], mean.v[1], o, d1, d2, 0);
rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o, d1);
rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o + M_PI / 2, d2);
}
// Draw a direction indicator
rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2], 0.50, 0.10);
rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] + 3 * sqrt(cov.m[2][2]), 0.50, 0.10);
rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] - 3 * sqrt(cov.m[2][2]), 0.50, 0.10);
}
return;
}
int main(int argc, char **argv)
{
rtk_app_t *app;
rtk_canvas_t *canvas;
rtk_fig_t *fig1, *fig2, *fig3;
int i;
app = rtk_app_create();
canvas = rtk_canvas_create(app);
rtk_app_start(app);
fig1 = rtk_fig_create(canvas, NULL);
rtk_fig_rectangle(fig1, 0, 0, 0, 1.0, 0.5);
rtk_fig_ellipse(fig1, 0, 0, 0, 1.0, 0.5);
rtk_fig_arrow(fig1, 0, 0, M_PI / 4, 1.0, 0.2);
fig2 = rtk_fig_create(canvas, fig1);
rtk_fig_origin(fig2, 1, 0, 0);
rtk_fig_scale(fig2, 0.5);
rtk_fig_rectangle(fig2, 0, 0, 0, 1.0, 0.5);
rtk_fig_ellipse(fig2, 0, 0, 0, 1.0, 0.5);
rtk_fig_arrow(fig2, 0, 0, M_PI / 4, 1.0, 0.2);
fig3 = rtk_fig_create(canvas, NULL);
i = 0;
while (!rtk_app_quit(app))
{
//rtk_fig_origin(fig1, 0, 0, i * 0.012);
rtk_fig_clear(fig3);
rtk_fig_origin(fig3, sin(i * 0.07), -1.0, 0);
rtk_fig_rectangle(fig3, 0, 0, i * 0.1, 0.5, 1.0);
rtk_fig_arrow(fig3, 0, 0, -i * 0.1, 0.8, 0.20);
rtk_fig_text(fig3, 0.0, 0.2, 0, "some text");
i++;
usleep(20000);
}
rtk_app_stop(app);
rtk_canvas_export(canvas, "test.fig");
//rtk_canvas_destroy(canvas);
rtk_app_destroy(app);
return 0;
}
// Draw the sample set
void pf_draw_samples(pf_t *pf, rtk_fig_t *fig, int max_samples)
{
int i;
double px, py, pa;
pf_sample_set_t *set;
pf_sample_t *sample;
set = pf->sets + pf->current_set;
max_samples = MIN(max_samples, set->sample_count);
for (i = 0; i < max_samples; i++)
{
sample = set->samples + i;
px = sample->pose.v[0];
py = sample->pose.v[1];
pa = sample->pose.v[2];
//printf("%f %f\n", px, py);
rtk_fig_point(fig, px, py);
rtk_fig_arrow(fig, px, py, pa, 0.1, 0.02);
//rtk_fig_rectangle(fig, px, py, 0, 0.1, 0.1, 0);
}
return;
}
