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
50 void pf_draw_statistics(pf_t *pf, rtk_fig_t *fig);
51 
52 
53 // Draw the sample set
54 void pf_draw_samples(pf_t *pf, rtk_fig_t *fig, int max_samples)
55 {
56  int i;
57  double px, py, pa;
58  pf_sample_set_t *set;
59  pf_sample_t *sample;
60 
61  set = pf->sets + pf->current_set;
62  max_samples = MIN(max_samples, set->sample_count);
63 
64  for (i = 0; i < max_samples; i++)
65  {
66  sample = set->samples + i;
67 
68  px = sample->pose.v[0];
69  py = sample->pose.v[1];
70  pa = sample->pose.v[2];
71 
72  //printf("%f %f\n", px, py);
73 
74  rtk_fig_point(fig, px, py);
75  rtk_fig_arrow(fig, px, py, pa, 0.1, 0.02);
76  //rtk_fig_rectangle(fig, px, py, 0, 0.1, 0.1, 0);
77  }
78 
79  return;
80 }
81 
82 
83 // Draw the hitogram (kd tree)
84 void pf_draw_hist(pf_t *pf, rtk_fig_t *fig)
85 {
86  pf_sample_set_t *set;
87 
88  set = pf->sets + pf->current_set;
89 
90  rtk_fig_color(fig, 0.0, 0.0, 1.0);
91  pf_kdtree_draw(set->kdtree, fig);
92 
93  return;
94 }
95 
96 
97 // Draw the CEP statistics
98 void pf_draw_cep_stats(pf_t *pf, rtk_fig_t *fig)
99 {
100  pf_vector_t mean;
101  double var;
102 
103  pf_get_cep_stats(pf, &mean, &var);
104  var = sqrt(var);
105 
106  rtk_fig_color(fig, 0, 0, 1);
107  rtk_fig_ellipse(fig, mean.v[0], mean.v[1], mean.v[2], 3 * var, 3 * var, 0);
108 
109  return;
110 }
111 
112 
113 // Draw the cluster statistics
114 void pf_draw_cluster_stats(pf_t *pf, rtk_fig_t *fig)
115 {
116  int i;
117  pf_cluster_t *cluster;
118  pf_sample_set_t *set;
119  pf_vector_t mean;
120  pf_matrix_t cov;
121  pf_matrix_t r, d;
122  double weight, o, d1, d2;
123 
124  set = pf->sets + pf->current_set;
125 
126  for (i = 0; i < set->cluster_count; i++)
127  {
128  cluster = set->clusters + i;
129 
130  weight = cluster->weight;
131  mean = cluster->mean;
132  cov = cluster->cov;
133 
134  // Compute unitary representation S = R D R^T
135  pf_matrix_unitary(&r, &d, cov);
136 
137  /* Debugging
138  printf("mean = \n");
139  pf_vector_fprintf(mean, stdout, "%e");
140  printf("cov = \n");
141  pf_matrix_fprintf(cov, stdout, "%e");
142  printf("r = \n");
143  pf_matrix_fprintf(r, stdout, "%e");
144  printf("d = \n");
145  pf_matrix_fprintf(d, stdout, "%e");
146  */
147 
148  // Compute the orientation of the error ellipse (first eigenvector)
149  o = atan2(r.m[1][0], r.m[0][0]);
150  d1 = 6 * sqrt(d.m[0][0]);
151  d2 = 6 * sqrt(d.m[1][1]);
152 
153  if (d1 > 1e-3 && d2 > 1e-3)
154  {
155  // Draw the error ellipse
156  rtk_fig_ellipse(fig, mean.v[0], mean.v[1], o, d1, d2, 0);
157  rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o, d1);
158  rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o + M_PI / 2, d2);
159  }
160 
161  // Draw a direction indicator
162  rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2], 0.50, 0.10);
163  rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] + 3 * sqrt(cov.m[2][2]), 0.50, 0.10);
164  rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] - 3 * sqrt(cov.m[2][2]), 0.50, 0.10);
165  }
166 
167  return;
168 }
169 
170 /// @endcond
171 
172 #endif