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