$search
00001 /* 00002 * Player - One Hell of a Robot Server 00003 * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 00004 * gerkey@usc.edu kaspers@robotics.usc.edu 00005 * 00006 * This library is free software; you can redistribute it and/or 00007 * modify it under the terms of the GNU Lesser General Public 00008 * License as published by the Free Software Foundation; either 00009 * version 2.1 of the License, or (at your option) any later version. 00010 * 00011 * This library is distributed in the hope that it will be useful, 00012 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00014 * Lesser General Public License for more details. 00015 * 00016 * You should have received a copy of the GNU Lesser General Public 00017 * License along with this library; if not, write to the Free Software 00018 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00019 * 00020 */ 00021 /************************************************************************** 00022 * Desc: Particle filter; drawing routines 00023 * Author: Andrew Howard 00024 * Date: 10 Dec 2002 00025 * CVS: $Id: pf_draw.c 7057 2008-10-02 00:44:06Z gbiggs $ 00026 *************************************************************************/ 00027 00028 #ifdef INCLUDE_RTKGUI 00029 00030 #include <assert.h> 00031 #include <math.h> 00032 #include <stdlib.h> 00033 00034 00035 #include "rtk.h" 00036 00037 #include "pf.h" 00038 #include "pf_pdf.h" 00039 #include "pf_kdtree.h" 00040 00041 00042 // Draw the statistics 00043 void pf_draw_statistics(pf_t *pf, rtk_fig_t *fig); 00044 00045 00046 // Draw the sample set 00047 void pf_draw_samples(pf_t *pf, rtk_fig_t *fig, int max_samples) 00048 { 00049 int i; 00050 double px, py, pa; 00051 pf_sample_set_t *set; 00052 pf_sample_t *sample; 00053 00054 set = pf->sets + pf->current_set; 00055 max_samples = MIN(max_samples, set->sample_count); 00056 00057 for (i = 0; i < max_samples; i++) 00058 { 00059 sample = set->samples + i; 00060 00061 px = sample->pose.v[0]; 00062 py = sample->pose.v[1]; 00063 pa = sample->pose.v[2]; 00064 00065 //printf("%f %f\n", px, py); 00066 00067 rtk_fig_point(fig, px, py); 00068 rtk_fig_arrow(fig, px, py, pa, 0.1, 0.02); 00069 //rtk_fig_rectangle(fig, px, py, 0, 0.1, 0.1, 0); 00070 } 00071 00072 return; 00073 } 00074 00075 00076 // Draw the hitogram (kd tree) 00077 void pf_draw_hist(pf_t *pf, rtk_fig_t *fig) 00078 { 00079 pf_sample_set_t *set; 00080 00081 set = pf->sets + pf->current_set; 00082 00083 rtk_fig_color(fig, 0.0, 0.0, 1.0); 00084 pf_kdtree_draw(set->kdtree, fig); 00085 00086 return; 00087 } 00088 00089 00090 // Draw the CEP statistics 00091 void pf_draw_cep_stats(pf_t *pf, rtk_fig_t *fig) 00092 { 00093 pf_vector_t mean; 00094 double var; 00095 00096 pf_get_cep_stats(pf, &mean, &var); 00097 var = sqrt(var); 00098 00099 rtk_fig_color(fig, 0, 0, 1); 00100 rtk_fig_ellipse(fig, mean.v[0], mean.v[1], mean.v[2], 3 * var, 3 * var, 0); 00101 00102 return; 00103 } 00104 00105 00106 // Draw the cluster statistics 00107 void pf_draw_cluster_stats(pf_t *pf, rtk_fig_t *fig) 00108 { 00109 int i; 00110 pf_cluster_t *cluster; 00111 pf_sample_set_t *set; 00112 pf_vector_t mean; 00113 pf_matrix_t cov; 00114 pf_matrix_t r, d; 00115 double weight, o, d1, d2; 00116 00117 set = pf->sets + pf->current_set; 00118 00119 for (i = 0; i < set->cluster_count; i++) 00120 { 00121 cluster = set->clusters + i; 00122 00123 weight = cluster->weight; 00124 mean = cluster->mean; 00125 cov = cluster->cov; 00126 00127 // Compute unitary representation S = R D R^T 00128 pf_matrix_unitary(&r, &d, cov); 00129 00130 /* Debugging 00131 printf("mean = \n"); 00132 pf_vector_fprintf(mean, stdout, "%e"); 00133 printf("cov = \n"); 00134 pf_matrix_fprintf(cov, stdout, "%e"); 00135 printf("r = \n"); 00136 pf_matrix_fprintf(r, stdout, "%e"); 00137 printf("d = \n"); 00138 pf_matrix_fprintf(d, stdout, "%e"); 00139 */ 00140 00141 // Compute the orientation of the error ellipse (first eigenvector) 00142 o = atan2(r.m[1][0], r.m[0][0]); 00143 d1 = 6 * sqrt(d.m[0][0]); 00144 d2 = 6 * sqrt(d.m[1][1]); 00145 00146 if (d1 > 1e-3 && d2 > 1e-3) 00147 { 00148 // Draw the error ellipse 00149 rtk_fig_ellipse(fig, mean.v[0], mean.v[1], o, d1, d2, 0); 00150 rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o, d1); 00151 rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o + M_PI / 2, d2); 00152 } 00153 00154 // Draw a direction indicator 00155 rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2], 0.50, 0.10); 00156 rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] + 3 * sqrt(cov.m[2][2]), 0.50, 0.10); 00157 rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] - 3 * sqrt(cov.m[2][2]), 0.50, 0.10); 00158 } 00159 00160 return; 00161 } 00162 00163 #endif