Fawkes API Fawkes Development Version
pf_draw.c
1/***************************************************************************
2 * pf_draw.c: Particle filter; drawing routines
3 *
4 * Created: Thu May 24 18:39:07 2012
5 * Copyright 2000 Brian Gerkey
6 * 2000 Kasper Stoy
7 * 2012 Tim Niemueller [www.niemueller.de]
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23/* From:
24 * Player - One Hell of a Robot Server (LGPL)
25 * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
26 * gerkey@usc.edu kaspers@robotics.usc.edu
27 */
28/**************************************************************************
29 * Desc: Particle filter; drawing routines
30 * Author: Andrew Howard
31 * Date: 10 Dec 2002
32 *************************************************************************/
33
34#ifdef INCLUDE_RTKGUI
35
36#include <assert.h>
37#include <math.h>
38#include <stdlib.h>
39
40
41#include "rtk.h"
42
43#include "pf.h"
44#include "pf_pdf.h"
45#include "pf_kdtree.h"
46
47/// @cond EXTERNAL
48
49// Draw the statistics
50void pf_draw_statistics(pf_t *pf, rtk_fig_t *fig);
51
52
53// Draw the sample set
54void pf_draw_samples(pf_t *pf, rtk_fig_t *fig, int max_samples)
55{
56 int i;
57 pf_sample_set_t *set;
58
59 set = pf->sets + pf->current_set;
60 max_samples = MIN(max_samples, set->sample_count);
61
62 for (i = 0; i < max_samples; i++)
63 {
64 pf_sample_t *sample = set->samples + i;
65
66 double px = sample->pose.v[0];
67 double py = sample->pose.v[1];
68 double pa = sample->pose.v[2];
69
70 //printf("%f %f\n", px, py);
71
72 rtk_fig_point(fig, px, py);
73 rtk_fig_arrow(fig, px, py, pa, 0.1, 0.02);
74 //rtk_fig_rectangle(fig, px, py, 0, 0.1, 0.1, 0);
75 }
76
77 return;
78}
79
80
81// Draw the hitogram (kd tree)
82void pf_draw_hist(pf_t *pf, rtk_fig_t *fig)
83{
84 pf_sample_set_t *set;
85
86 set = pf->sets + pf->current_set;
87
88 rtk_fig_color(fig, 0.0, 0.0, 1.0);
89 pf_kdtree_draw(set->kdtree, fig);
90
91 return;
92}
93
94
95// Draw the CEP statistics
96void pf_draw_cep_stats(pf_t *pf, rtk_fig_t *fig)
97{
98 pf_vector_t mean;
99 double var;
100
101 pf_get_cep_stats(pf, &mean, &var);
102 var = sqrt(var);
103
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);
106
107 return;
108}
109
110
111// Draw the cluster statistics
112void pf_draw_cluster_stats(pf_t *pf, rtk_fig_t *fig)
113{
114 int i;
115 pf_sample_set_t *set;
116 pf_vector_t mean;
117 pf_matrix_t cov;
118 pf_matrix_t r, d;
119
120 set = pf->sets + pf->current_set;
121
122 for (i = 0; i < set->cluster_count; i++)
123 {
124 pf_cluster_t *cluster = set->clusters + i;
125
126 //weight = cluster->weight;
127 mean = cluster->mean;
128 cov = cluster->cov;
129
130 // Compute unitary representation S = R D R^T
131 pf_matrix_unitary(&r, &d, cov);
132
133 /* Debugging
134 printf("mean = \n");
135 pf_vector_fprintf(mean, stdout, "%e");
136 printf("cov = \n");
137 pf_matrix_fprintf(cov, stdout, "%e");
138 printf("r = \n");
139 pf_matrix_fprintf(r, stdout, "%e");
140 printf("d = \n");
141 pf_matrix_fprintf(d, stdout, "%e");
142 */
143
144 // Compute the orientation of the error ellipse (first eigenvector)
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]);
148
149 if (d1 > 1e-3 && d2 > 1e-3)
150 {
151 // Draw the error ellipse
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);
155 }
156
157 // Draw a direction indicator
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);
161 }
162
163 return;
164}
165
166/// @endcond
167
168#endif