50void pf_draw_statistics(pf_t *pf, rtk_fig_t *fig);
54void pf_draw_samples(pf_t *pf, rtk_fig_t *fig,
int max_samples)
59 set = pf->sets + pf->current_set;
60 max_samples = MIN(max_samples, set->sample_count);
62 for (i = 0; i < max_samples; i++)
64 pf_sample_t *sample = set->samples + i;
66 double px = sample->pose.v[0];
67 double py = sample->pose.v[1];
68 double pa = sample->pose.v[2];
72 rtk_fig_point(fig, px, py);
73 rtk_fig_arrow(fig, px, py, pa, 0.1, 0.02);
82void pf_draw_hist(pf_t *pf, rtk_fig_t *fig)
86 set = pf->sets + pf->current_set;
88 rtk_fig_color(fig, 0.0, 0.0, 1.0);
89 pf_kdtree_draw(set->kdtree, fig);
96void pf_draw_cep_stats(pf_t *pf, rtk_fig_t *fig)
101 pf_get_cep_stats(pf, &mean, &var);
104 rtk_fig_color(fig, 0, 0, 1);
105 rtk_fig_ellipse(fig, mean.v[0], mean.v[1], mean.v[2], 3 * var, 3 * var, 0);
112void pf_draw_cluster_stats(pf_t *pf, rtk_fig_t *fig)
115 pf_sample_set_t *set;
120 set = pf->sets + pf->current_set;
122 for (i = 0; i < set->cluster_count; i++)
124 pf_cluster_t *cluster = set->clusters + i;
127 mean = cluster->mean;
131 pf_matrix_unitary(&r, &d, cov);
145 double o = atan2(r.m[1][0], r.m[0][0]);
146 double d1 = 6 * sqrt(d.m[0][0]);
147 double d2 = 6 * sqrt(d.m[1][1]);
149 if (d1 > 1e-3 && d2 > 1e-3)
152 rtk_fig_ellipse(fig, mean.v[0], mean.v[1], o, d1, d2, 0);
153 rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o, d1);
154 rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o + M_PI / 2, d2);
158 rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2], 0.50, 0.10);
159 rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] + 3 * sqrt(cov.m[2][2]), 0.50, 0.10);
160 rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] - 3 * sqrt(cov.m[2][2]), 0.50, 0.10);