pf_draw.c
Go to the documentation of this file.
1 /*
2  * Player - One Hell of a Robot Server
3  * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
4  * gerkey@usc.edu kaspers@robotics.usc.edu
5  *
6  * This library is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU Lesser General Public
8  * License as published by the Free Software Foundation; either
9  * version 2.1 of the License, or (at your option) any later version.
10  *
11  * This library is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14  * Lesser General Public License for more details.
15  *
16  * You should have received a copy of the GNU Lesser General Public
17  * License along with this library; if not, write to the Free Software
18  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19  *
20  */
21 /**************************************************************************
22  * Desc: Particle filter; drawing routines
23  * Author: Andrew Howard
24  * Date: 10 Dec 2002
25  * CVS: $Id: pf_draw.c 7057 2008-10-02 00:44:06Z gbiggs $
26  *************************************************************************/
27 
28 #ifdef INCLUDE_RTKGUI
29 
30 #include <assert.h>
31 #include <math.h>
32 #include <stdlib.h>
33 
34 
35 #include "rtk.h"
36 
37 #include "pf.h"
38 #include "pf_pdf.h"
39 #include "pf_kdtree.h"
40 
41 
42 // Draw the statistics
43 void pf_draw_statistics(pf_t *pf, rtk_fig_t *fig);
44 
45 
46 // Draw the sample set
47 void pf_draw_samples(pf_t *pf, rtk_fig_t *fig, int max_samples)
48 {
49  int i;
50  double px, py, pa;
51  pf_sample_set_t *set;
52  pf_sample_t *sample;
53 
54  set = pf->sets + pf->current_set;
55  max_samples = MIN(max_samples, set->sample_count);
56 
57  for (i = 0; i < max_samples; i++)
58  {
59  sample = set->samples + i;
60 
61  px = sample->pose.v[0];
62  py = sample->pose.v[1];
63  pa = sample->pose.v[2];
64 
65  //printf("%f %f\n", px, py);
66 
67  rtk_fig_point(fig, px, py);
68  rtk_fig_arrow(fig, px, py, pa, 0.1, 0.02);
69  //rtk_fig_rectangle(fig, px, py, 0, 0.1, 0.1, 0);
70  }
71 
72  return;
73 }
74 
75 
76 // Draw the hitogram (kd tree)
77 void pf_draw_hist(pf_t *pf, rtk_fig_t *fig)
78 {
79  pf_sample_set_t *set;
80 
81  set = pf->sets + pf->current_set;
82 
83  rtk_fig_color(fig, 0.0, 0.0, 1.0);
84  pf_kdtree_draw(set->kdtree, fig);
85 
86  return;
87 }
88 
89 
90 // Draw the CEP statistics
91 void pf_draw_cep_stats(pf_t *pf, rtk_fig_t *fig)
92 {
93  pf_vector_t mean;
94  double var;
95 
96  pf_get_cep_stats(pf, &mean, &var);
97  var = sqrt(var);
98 
99  rtk_fig_color(fig, 0, 0, 1);
100  rtk_fig_ellipse(fig, mean.v[0], mean.v[1], mean.v[2], 3 * var, 3 * var, 0);
101 
102  return;
103 }
104 
105 
106 // Draw the cluster statistics
107 void pf_draw_cluster_stats(pf_t *pf, rtk_fig_t *fig)
108 {
109  int i;
110  pf_cluster_t *cluster;
111  pf_sample_set_t *set;
112  pf_vector_t mean;
113  pf_matrix_t cov;
114  pf_matrix_t r, d;
115  double weight, o, d1, d2;
116 
117  set = pf->sets + pf->current_set;
118 
119  for (i = 0; i < set->cluster_count; i++)
120  {
121  cluster = set->clusters + i;
122 
123  weight = cluster->weight;
124  mean = cluster->mean;
125  cov = cluster->cov;
126 
127  // Compute unitary representation S = R D R^T
128  pf_matrix_unitary(&r, &d, cov);
129 
130  /* Debugging
131  printf("mean = \n");
132  pf_vector_fprintf(mean, stdout, "%e");
133  printf("cov = \n");
134  pf_matrix_fprintf(cov, stdout, "%e");
135  printf("r = \n");
136  pf_matrix_fprintf(r, stdout, "%e");
137  printf("d = \n");
138  pf_matrix_fprintf(d, stdout, "%e");
139  */
140 
141  // Compute the orientation of the error ellipse (first eigenvector)
142  o = atan2(r.m[1][0], r.m[0][0]);
143  d1 = 6 * sqrt(d.m[0][0]);
144  d2 = 6 * sqrt(d.m[1][1]);
145 
146  if (d1 > 1e-3 && d2 > 1e-3)
147  {
148  // Draw the error ellipse
149  rtk_fig_ellipse(fig, mean.v[0], mean.v[1], o, d1, d2, 0);
150  rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o, d1);
151  rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o + M_PI / 2, d2);
152  }
153 
154  // Draw a direction indicator
155  rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2], 0.50, 0.10);
156  rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] + 3 * sqrt(cov.m[2][2]), 0.50, 0.10);
157  rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] - 3 * sqrt(cov.m[2][2]), 0.50, 0.10);
158  }
159 
160  return;
161 }
162 
163 #endif
d
double v[3]
Definition: pf_vector.h:40
Definition: pf.h:111
pf_matrix_t cov
Definition: pf.h:81
void pf_draw_cluster_stats(pf_t *pf, struct _rtk_fig_t *fig)
void pf_draw_cep_stats(pf_t *pf, struct _rtk_fig_t *fig)
void pf_draw_samples(pf_t *pf, struct _rtk_fig_t *fig, int max_samples)
ROSCPP_DECL void set(const std::string &key, const XmlRpc::XmlRpcValue &v)
double weight
Definition: pf.h:77
int current_set
Definition: pf.h:121
Definition: pf.h:59
double m[3][3]
Definition: pf_vector.h:47
pf_vector_t pose
Definition: pf.h:62
pf_vector_t mean
Definition: pf.h:80
void pf_draw_hist(pf_t *pf, struct _rtk_fig_t *fig)
void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a)
Definition: pf_vector.c:222
pf_sample_set_t sets[2]
Definition: pf.h:122
void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var)
Definition: pf.c:605


amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:44:09