Fawkes API  Fawkes Development Version
pf_draw.c
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