pf_vector.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: Vector functions
00023  * Author: Andrew Howard
00024  * Date: 10 Dec 2002
00025  * CVS: $Id: pf_vector.c 6345 2008-04-17 01:36:39Z gerkey $
00026  *************************************************************************/
00027 
00028 #include <math.h>
00029 //#include <gsl/gsl_matrix.h>
00030 //#include <gsl/gsl_eigen.h>
00031 //#include <gsl/gsl_linalg.h>
00032 
00033 #include "nav2d_localizer/pf_vector.h"
00034 #include "nav2d_localizer/eig3.h"
00035 
00036 
00037 // Return a zero vector
00038 pf_vector_t pf_vector_zero()
00039 {
00040   pf_vector_t c;
00041 
00042   c.v[0] = 0.0;
00043   c.v[1] = 0.0;
00044   c.v[2] = 0.0;
00045   
00046   return c;
00047 }
00048 
00049 
00050 // Check for NAN or INF in any component
00051 int pf_vector_finite(pf_vector_t a)
00052 {
00053   int i;
00054   
00055   for (i = 0; i < 3; i++)
00056     if (!finite(a.v[i]))
00057       return 0;
00058   
00059   return 1;
00060 }
00061 
00062 
00063 // Print a vector
00064 void pf_vector_fprintf(pf_vector_t a, FILE *file, const char *fmt)
00065 {
00066   int i;
00067 
00068   for (i = 0; i < 3; i++)
00069   {
00070     fprintf(file, fmt, a.v[i]);
00071     fprintf(file, " ");
00072   }
00073   fprintf(file, "\n");
00074 
00075   return;     
00076 }
00077 
00078 
00079 // Simple vector addition
00080 pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b)
00081 {
00082   pf_vector_t c;
00083 
00084   c.v[0] = a.v[0] + b.v[0];
00085   c.v[1] = a.v[1] + b.v[1];
00086   c.v[2] = a.v[2] + b.v[2];
00087   
00088   return c;
00089 }
00090 
00091 
00092 // Simple vector subtraction
00093 pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b)
00094 {
00095   pf_vector_t c;
00096 
00097   c.v[0] = a.v[0] - b.v[0];
00098   c.v[1] = a.v[1] - b.v[1];
00099   c.v[2] = a.v[2] - b.v[2];
00100   
00101   return c;
00102 }
00103 
00104 
00105 // Transform from local to global coords (a + b)
00106 pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b)
00107 {
00108   pf_vector_t c;
00109 
00110   c.v[0] = b.v[0] + a.v[0] * cos(b.v[2]) - a.v[1] * sin(b.v[2]);
00111   c.v[1] = b.v[1] + a.v[0] * sin(b.v[2]) + a.v[1] * cos(b.v[2]);
00112   c.v[2] = b.v[2] + a.v[2];
00113   c.v[2] = atan2(sin(c.v[2]), cos(c.v[2]));
00114   
00115   return c;
00116 }
00117 
00118 
00119 // Transform from global to local coords (a - b)
00120 pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b)
00121 {
00122   pf_vector_t c;
00123 
00124   c.v[0] = +(a.v[0] - b.v[0]) * cos(b.v[2]) + (a.v[1] - b.v[1]) * sin(b.v[2]);
00125   c.v[1] = -(a.v[0] - b.v[0]) * sin(b.v[2]) + (a.v[1] - b.v[1]) * cos(b.v[2]);
00126   c.v[2] = a.v[2] - b.v[2];
00127   c.v[2] = atan2(sin(c.v[2]), cos(c.v[2]));
00128   
00129   return c;
00130 }
00131 
00132 
00133 // Return a zero matrix
00134 pf_matrix_t pf_matrix_zero()
00135 {
00136   int i, j;
00137   pf_matrix_t c;
00138 
00139   for (i = 0; i < 3; i++)
00140     for (j = 0; j < 3; j++)
00141       c.m[i][j] = 0.0;
00142         
00143   return c;
00144 }
00145 
00146 
00147 // Check for NAN or INF in any component
00148 int pf_matrix_finite(pf_matrix_t a)
00149 {
00150   int i, j;
00151   
00152   for (i = 0; i < 3; i++)
00153     for (j = 0; j < 3; j++)
00154       if (!finite(a.m[i][j]))
00155         return 0;
00156   
00157   return 1;
00158 }
00159 
00160 
00161 // Print a matrix
00162 void pf_matrix_fprintf(pf_matrix_t a, FILE *file, const char *fmt)
00163 {
00164   int i, j;
00165 
00166   for (i = 0; i < 3; i++)
00167   {
00168     for (j = 0; j < 3; j++)
00169     {
00170       fprintf(file, fmt, a.m[i][j]);
00171       fprintf(file, " ");
00172     }
00173     fprintf(file, "\n");
00174   }
00175   return;     
00176 }
00177 
00178 
00179 /*
00180 // Compute the matrix inverse
00181 pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det)
00182 {
00183   double lndet;
00184   int signum;
00185   gsl_permutation *p;
00186   gsl_matrix_view A, Ai;
00187 
00188   pf_matrix_t ai;
00189 
00190   A = gsl_matrix_view_array((double*) a.m, 3, 3);
00191   Ai = gsl_matrix_view_array((double*) ai.m, 3, 3);
00192   
00193   // Do LU decomposition
00194   p = gsl_permutation_alloc(3);
00195   gsl_linalg_LU_decomp(&A.matrix, p, &signum);
00196 
00197   // Check for underflow
00198   lndet = gsl_linalg_LU_lndet(&A.matrix);
00199   if (lndet < -1000)
00200   {
00201     //printf("underflow in matrix inverse lndet = %f", lndet);
00202     gsl_matrix_set_zero(&Ai.matrix);
00203   }
00204   else
00205   {
00206     // Compute inverse
00207     gsl_linalg_LU_invert(&A.matrix, p, &Ai.matrix);
00208   }
00209 
00210   gsl_permutation_free(p);
00211 
00212   if (det)
00213     *det = exp(lndet);
00214 
00215   return ai;
00216 }
00217 */
00218 
00219 
00220 // Decompose a covariance matrix [a] into a rotation matrix [r] and a diagonal
00221 // matrix [d] such that a = r d r^T.
00222 void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a)
00223 {
00224   int i, j;
00225   /*
00226   gsl_matrix *aa;
00227   gsl_vector *eval;
00228   gsl_matrix *evec;
00229   gsl_eigen_symmv_workspace *w;
00230 
00231   aa = gsl_matrix_alloc(3, 3);
00232   eval = gsl_vector_alloc(3);
00233   evec = gsl_matrix_alloc(3, 3);
00234   */
00235 
00236   double aa[3][3];
00237   double eval[3];
00238   double evec[3][3];
00239 
00240   for (i = 0; i < 3; i++)
00241   {
00242     for (j = 0; j < 3; j++)
00243     {
00244       //gsl_matrix_set(aa, i, j, a.m[i][j]);
00245       aa[i][j] = a.m[i][j];
00246     }
00247   }
00248 
00249   // Compute eigenvectors/values
00250   /*
00251   w = gsl_eigen_symmv_alloc(3);
00252   gsl_eigen_symmv(aa, eval, evec, w);
00253   gsl_eigen_symmv_free(w);
00254   */
00255 
00256   eigen_decomposition(aa,evec,eval);
00257 
00258   *d = pf_matrix_zero();
00259   for (i = 0; i < 3; i++)
00260   {
00261     //d->m[i][i] = gsl_vector_get(eval, i);
00262     d->m[i][i] = eval[i];
00263     for (j = 0; j < 3; j++)
00264     {
00265       //r->m[i][j] = gsl_matrix_get(evec, i, j);
00266       r->m[i][j] = evec[i][j];
00267     }
00268   }
00269   
00270   //gsl_matrix_free(evec);
00271   //gsl_vector_free(eval);
00272   //gsl_matrix_free(aa);
00273   
00274   return;
00275 }
00276 


nav2d_localizer
Author(s): Sebastian Kasperski
autogenerated on Thu Aug 27 2015 14:07:21