pf_draw.c
Go to the documentation of this file.
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


amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:48