toolbox.h File Reference

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures
struct	color

Functions
tfloat	detMatrix33 (tfloat M[][3])
	Computes the determinant of a 3x3 matrix. More...
tfloat	gaussianRand (void)
void	get_angles (tfloat *a1, tfloat *a2, tfloat *a3, tfloat xr, tfloat yr, tfloat or, tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3)
void	grayscale2RGB (tfloat imageG[], tfloat imageRGB[], unsigned int width, unsigned int height, unsigned int Max)
	Convert a grayscale to an RGB image. More...
void	initRand (void)
	Initializes the random number generator with current time.
color	mapRGB (double v, double vmin, double vmax)
tfloat	mean (tfloat data[], unsigned long N)
	Computes the algebraic mean of the array data[] of length N. More...
int	saveMapAndScale (tfloat image[], unsigned int Width, unsigned int Height, unsigned int Mode, unsigned int Max, tfloat P1)
tfloat	triangulationGDOP (tfloat xr, tfloat yr, tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3)
tfloat	triangulationMethod (tfloat *x, tfloat *y, tfloat alpha1, tfloat alpha2, tfloat alpha3, tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3, int method)
tfloat	triangulationSensitivity (tfloat xTrue, tfloat yTrue, tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3, int method, tfloat sigmaAngle, tfloat sigmaBeacon, unsigned int n, int outputType, double thresh)
tfloat	variance (tfloat data[], unsigned long N)
	Computes the variance of the array data[] of length N. More...
int	writePGM (tfloat image[], unsigned int width, unsigned int height, unsigned int Max, char *fileName)
int	writePPM (tfloat image[], unsigned int width, unsigned int height, unsigned int Max, char *fileName)

Function Documentation

tfloat detMatrix33 ( tfloat  M[][3] )

inline

Computes the determinant of a 3x3 matrix.

Parameters

M[][3]

Returns

Definition at line 358 of file toolbox.c.

Referenced by triangulationGDOP().

{
  return M[0][0]*M[1][1]*M[2][2] + M[0][1]*M[1][2]*M[2][0] + M[1][0]*M[2][1]*M[0][2] - M[0][2]*M[1][1]*M[2][0] - M[0][1]*M[1][0]*M[2][2] - M[1][2]*M[2][1]*M[0][0] ;
}

tfloat gaussianRand

(

void

)

Returns an occurence of a normally distributed random variable. Uses the Marsaglia polar method (polar version of Box-Muller transform). http://en.wikipedia.org/wiki/Box_Muller_transform

Returns

Definition at line 204 of file toolbox.c.

Referenced by triangulationSensitivity().

{
  tfloat u , v , s = 0.0 ;
  
  while( s == 0.0 || s >= 1.0 )
  {
    u = ( ( (tfloat) random() / (tfloat) RAND_MAX ) * 2.0 ) - 1.0 ;
    v = ( ( (tfloat) random() / (tfloat) RAND_MAX ) * 2.0 ) - 1.0 ;
    s = u*u + v*v ;
  }
  
  return u * sqrt( -2 * log(s) / s ) ;
}

void get_angles	(	tfloat *	a1,
		tfloat *	a2,
		tfloat *	a3,
		tfloat	xr,
		tfloat	yr,
		tfloat	or,
		tfloat	x1,
		tfloat	y1,
		tfloat	x2,
		tfloat	y2,
		tfloat	x3,
		tfloat	y3
	)

Computes the angles (a1,a2,a3) measured by a robot (xr,yr) from beacons (x1,y1), (x2,y2), (x3,y3). The orientation of the robot is given by 'or', and angles are computed relatively to 'or'.

Parameters

a1
a2
a3
xr
yr
or
x1
y1
x2
y2
x3
y3

Definition at line 85 of file toolbox.c.

Referenced by triangulationSensitivity(), and usage().

{
  *a1 = atan2( (y1 - yr) , (x1 - xr) ) - or ;
  *a2 = atan2( (y2 - yr) , (x2 - xr) ) - or ;
  *a3 = atan2( (y3 - yr) , (x3 - xr) ) - or ;
}

void grayscale2RGB	(	tfloat	imageG[],
		tfloat	imageRGB[],
		unsigned int	width,
		unsigned int	height,
		unsigned int	Max
	)

Convert a grayscale to an RGB image.

Parameters

imageG[]
imageRGB[]
width
height
Max

Definition at line 439 of file toolbox.c.

References mapRGB().

Referenced by saveMapAndScale().

{
    unsigned int i , j ;
    tfloat v ;
    color c ;

    for( j = 0 ; j < height ; ++j )
    {
      for( i = 0 ; i < width ; ++i )
      {
        v = imageG [ (height-j-1)*width + i ] ;
        c = mapRGB( v , 0.0 , Max ) ;
        imageRGB [ (height-j-1)*width*3 + i*3 + 0 ] = c.r * Max ;
        imageRGB [ (height-j-1)*width*3 + i*3 + 1 ] = c.g * Max ;
        imageRGB [ (height-j-1)*width*3 + i*3 + 2 ] = c.b * Max ;
      }
    }
}

Here is the call graph for this function:

color mapRGB	(	tfloat	v,
		tfloat	vmin,
		tfloat	vmax
	)

Return a RGB color value given a scalar v in the range [vmin,vmax] In this case each colour component ranges from 0 (no contribution) to 1 (fully saturated), modifications for other ranges is trivial. The colour is clipped at the end of the scales if v is outside the range [vmin,vmax] http://stackoverflow.com/questions/7706339/grayscale-to-red-green-blue-matlab-jet-color-scale

Parameters

v
vmin
vmax

Returns

Definition at line 472 of file toolbox.c.

Referenced by grayscale2RGB().

{
   color c = { 1.0 , 1.0 , 1.0 } ; /* white */
   tfloat dv ;

   if (v < vmin) v = vmin ;
   if (v > vmax) v = vmax ;
   dv = vmax - vmin ;

   if ( v < ( vmin + 0.25 * dv ) ) {
      c.r = 0 ;
      c.g = 4 * ( v - vmin ) / dv ;
   } else if ( v < ( vmin + 0.5 * dv ) ) {
      c.r = 0 ;
      c.b = 1 + 4 * ( vmin + 0.25 * dv - v ) / dv ;
   } else if ( v < ( vmin + 0.75 * dv ) ) {
      c.r = 4 * ( v - vmin - 0.5 * dv ) / dv ;
      c.b = 0 ;
   } else {
      c.g = 1 + 4 * ( vmin + 0.75 * dv - v ) / dv ;
      c.b = 0 ;
   }

   return c ;
}

tfloat mean	(	tfloat	data[],
		unsigned long	N
	)

Computes the algebraic mean of the array data[] of length N.

Parameters

data[]
N

Returns

Definition at line 153 of file toolbox.c.

References mean().

Referenced by mean(), triangulationSensitivity(), and variance().

{
  unsigned long i ;
  tfloat mean = 0.0 ;
  
  for( i = 0 ; i < N ; ++i )
    mean += data[i] ;
  mean /= N ;
  
  return mean ;
}

Here is the call graph for this function:

int saveMapAndScale	(	tfloat	image[],
		unsigned int	width,
		unsigned int	height,
		unsigned int	mode,
		unsigned int	Max,
		tfloat	P1
	)

Scales 'image[]' according to 'mode', 'Max', and 'P1'. Saves scaled image in "map.pgm" and corresponding scale in "scale.pgm"

Parameters

image[]
width
height
mode
Max
P1

Returns

Definition at line 378 of file toolbox.c.

References grayscale2RGB(), map_to_grayscale_hist(), map_to_grayscale_lin(), map_to_grayscale_log(), map_to_grayscale_sat(), writePGM(), and writePPM().

Referenced by usage().

{
    tfloat *map = ( tfloat * ) malloc( width * height * sizeof(tfloat) ) ; if( map == NULL ) return -1 ;
    tfloat *scale = ( tfloat * ) malloc( (width/SWP) * height * sizeof(tfloat) ) ; if( scale == NULL ) return -1 ;
    tfloat *map_color = ( tfloat * ) malloc( width * height * 3 * sizeof(tfloat) ) ; if( map_color == NULL ) return -1 ;
    tfloat *scale_color = ( tfloat * ) malloc( (width/SWP) * height * 3 * sizeof(tfloat) ) ; if( scale_color == NULL ) return -1 ;
    
    switch( mode )
    {
      case 0:
        map_to_grayscale_lin( image , map , scale , width , height , Max ) ;
        break;
      case 1:
        map_to_grayscale_log( image , map , scale , width , height , Max , P1 ) ;
        break;
      case 2:
        map_to_grayscale_hist( image , map , width , height , Max ) ;
        break;
      case 3:
        map_to_grayscale_sat( image , map , width , height , 100 ) ;
        map_to_grayscale_lin( map , map , scale , width , height , Max ) ;
        break;
      case 4:
        map_to_grayscale_sat( image , map , width , height , 100 ) ;
        map_to_grayscale_log( map , map , scale , width , height , Max , P1 ) ;
        break;
      default:
        fprintf(stderr, "saveMapAndScale(): Bad map scaling mode!\n") ;
        free( map ) ;
        free( scale ) ;
        free( map_color ) ;
        free( scale_color ) ;
        return -1 ;
    }
    writePGM( map , width , height , Max , "map.pgm" ) ;
    writePGM( scale , width/SWP , height , Max , "scale.pgm" ) ; /* for now, there is no scale for mode 2. To Do */
    
    grayscale2RGB( map , map_color , width , height , Max ) ;
    grayscale2RGB( scale , scale_color , width/SWP , height , Max ) ;
    
    writePPM( map_color , width , height , Max , "map.ppm" ) ;
    writePPM( scale_color , width/SWP , height , Max , "scale.ppm" ) ; /* for now, there is no scale for mode 2. To Do */
    
    /* free memory */
    free( map ) ;
    free( scale ) ;
    free( map_color ) ;
    free( scale_color ) ;
    
    return 0 ;
}

Here is the call graph for this function:

tfloat triangulationGDOP	(	tfloat	xr,
		tfloat	yr,
		tfloat	x1,
		tfloat	y1,
		tfloat	x2,
		tfloat	y2,
		tfloat	x3,
		tfloat	y3
	)

Computes the first order Geometric Dilution of Precision (GDOP) for triangulation, given the robot position (xr,yr), and beacons locations (x1,y1), (x2,y2), and (x3,y3).

Parameters

xr
yr
x1
y1
x2
y2
x3
y3

Returns

Definition at line 334 of file toolbox.c.

References detMatrix33().

Referenced by usage().

{
    tfloat jacobian[3][3] , det , d1 , d2 , d3 ;
    
    d1 = ( x1 - xr ) * ( x1 - xr ) + ( y1 - yr ) * ( y1 - yr ) ;
    d2 = ( x2 - xr ) * ( x2 - xr ) + ( y2 - yr ) * ( y2 - yr ) ;
    d3 = ( x3 - xr ) * ( x3 - xr ) + ( y3 - yr ) * ( y3 - yr ) ;
    
    jacobian[0][0] = ( y1 - yr ) / d1 ; jacobian[0][1] = ( x1 - xr ) / d1 ; jacobian[0][2] = 1.0 ;
    jacobian[1][0] = ( y2 - yr ) / d2 ; jacobian[1][1] = ( x2 - xr ) / d2 ; jacobian[1][2] = 1.0 ;
    jacobian[2][0] = ( y3 - yr ) / d3 ; jacobian[2][1] = ( x3 - xr ) / d3 ; jacobian[2][2] = 1.0 ;
    
    det = detMatrix33( jacobian ) ;
    
    return 1.0 / fabs( det ) ;
}

Here is the call graph for this function:

tfloat triangulationMethod	(	tfloat *	x,
		tfloat *	y,
		tfloat	alpha1,
		tfloat	alpha2,
		tfloat	alpha3,
		tfloat	x1,
		tfloat	y1,
		tfloat	x2,
		tfloat	y2,
		tfloat	x3,
		tfloat	y3,
		int	method
	)

Triangulates robot position (x,y) from measured angles alpha1, alpha2, and alpha3, and beacons locations (x1,y1), (x2,y2), and (x3,y3). Uses triangulation method 'm' given in command line.

Parameters

x
y
alpha1
alpha2
alpha3
x1
y1
x2
y2
x3
y3
method

Returns

Definition at line 112 of file toolbox.c.

References triangulationPierlot().

Referenced by triangulationSensitivity(), and usage().

{
  switch( method )
  {
    case 1:  return triangulationPierlot(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 2:  return triangulationEsteves(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 3:  return triangulationFontLlagunes(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 4:  return triangulationZalama(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 5:  return triangulationMcGillemTrigo(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 6:  return triangulationMcGillemGeometric(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 7:  return triangulationEaston(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 8:  return triangulationHmam(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 9:  return triangulationCohenTrigo(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 10: return triangulationCohenGeometric(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 11: return triangulationCohenGeometricOriginal(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 12: return triangulationTsukiyama(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 13: return triangulationTsukiyamaOriginal(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 14: return triangulationMadsen(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 15: return triangulationLukic(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 16: return triangulationLukicOriginal(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 17: return triangulationTienstra(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 18: return triangulationLigas(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 19: return triangulationFontLlagunes2(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 20: return triangulationKaestner(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 21: return triangulationCollins(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 22: return triangulationCassini(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    case 42: return triangulationPierlot2(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
    default: fprintf(stderr, "triangulationMethod(): Bad method number!\n") ; return -1 ;
  }
}

Here is the call graph for this function:

tfloat triangulationSensitivity	(	tfloat	xTrue,
		tfloat	yTrue,
		tfloat	x1,
		tfloat	y1,
		tfloat	x2,
		tfloat	y2,
		tfloat	x3,
		tfloat	y3,
		int	method,
		tfloat	sigmaAngle,
		tfloat	sigmaBeacon,
		unsigned int	n,
		int	outputType,
		double	thresh
	)

Simulates the beacon position sensitivity of the triangulation method 'm', for measured angles alpha1, alpha2, and alpha3, and beacons locations (x1,y1), (x2,y2), and (x3,y3). 'sigma' is the standard deviation of beacon coordinates. The simulation is performed 'n' times.

Parameters

xTrue
yTrue
x1
y1
x2
y2
x3
y3
method
sigmaAngle
sigmaBeacon
n
outputType
thresh

Returns

Definition at line 242 of file toolbox.c.

References gaussianRand(), get_angles(), initRand(), mean(), PI, triangulationMethod(), and variance().

Referenced by usage().

{
  tfloat alpha1 ;
    tfloat alpha2 ;
    tfloat alpha3 ;
    tfloat oTrue = 0.0 ;
  tfloat xNoise = 0.0 ;
    tfloat yNoise = 0.0 ;
    tfloat oNoise = 0.0 ;
    tfloat qual = 0.0 ;
    
    tfloat ea1=0.0, ea2=0.0, ea3=0.0, ex1=0.0, ey1=0.0, ex2=0.0, ey2=0.0, ex3=0.0, ey3=0.0, s ;
    tfloat *errP = ( tfloat * ) malloc( n * sizeof(tfloat) ) ; if( errP == NULL ) return -1 ;
    tfloat *errO = ( tfloat * ) malloc( n * sizeof(tfloat) ) ; if( errO == NULL ) return -1 ;
    tfloat *Qual = ( tfloat * ) malloc( n * sizeof(tfloat) ) ; if( Qual == NULL ) return -1 ;
    
    int i ;
    
  initRand() ;
    
    for ( i = 0 ; i < n ; ++i )
    {
      if ( sigmaAngle != 0.0 )
      {
        ea1 = gaussianRand() * sigmaAngle ;
        ea2 = gaussianRand() * sigmaAngle ;
        ea3 = gaussianRand() * sigmaAngle ;
      }
      if ( sigmaBeacon != 0.0 )
      {
        ex1 = gaussianRand() * sigmaBeacon ;
        ey1 = gaussianRand() * sigmaBeacon ;
        ex2 = gaussianRand() * sigmaBeacon ;
        ey2 = gaussianRand() * sigmaBeacon ;
        ex3 = gaussianRand() * sigmaBeacon ;
        ey3 = gaussianRand() * sigmaBeacon ;
      }
      
      get_angles(&alpha1, &alpha2, &alpha3, xTrue, yTrue, 0.0, x1+ex1, y1+ey1, x2+ex2, y2+ey2, x3+ex3, y3+ey3) ;
      qual = triangulationMethod(&xNoise, &yNoise, alpha1+ea1, alpha2+ea2, alpha3+ea3, x1, y1, x2, y2, x3, y3, method) ;
      qual = abs( qual ) ;
      oNoise  = atan2( (y1-yNoise) , (x1-xNoise) ) - alpha1 ;
      /*tfloat oNoise2, oNoise3, sinMean, cosMean ;
      oNoise2 = atan2( (y2-yNoise) , (x2-xNoise) ) - alpha2 ;
      oNoise3 = atan2( (y3-yNoise) , (x3-xNoise) ) - alpha3 ;
      sinMean = (sin(oNoise)+sin(oNoise2)+sin(oNoise3))/3;
      cosMean = (cos(oNoise)+cos(oNoise2)+cos(oNoise3))/3;
      oNoise  = atan2( sinMean , cosMean ) ;*/
      
      errP[ i ] = sqrt( (xTrue-xNoise)*(xTrue-xNoise) + (yTrue-yNoise)*(yTrue-yNoise) ) ;
      errO[ i ] = oTrue - oNoise ; while( errO[ i ] > PI ) errO[ i ] -= TWOPI ; while( errO[ i ] <= -PI ) errO[ i ] += TWOPI ;
      Qual[ i ] = qual ;
    }
    
  switch( outputType )
  {
    case 1: s = sqrt ( variance ( errP , n ) ) ; break ;
    case 2: s = sqrt ( variance ( errO , n ) ) * RAD2DEG ; break ;
    case 3: s = mean ( Qual , n ) ; break ;
    default: fprintf( stderr , "triangulationSensitivity(): Bad output type!\n" ) ; return -1 ;
  }
    
    free ( errP ) ;
    free ( errO ) ;
    free ( Qual ) ;
    
  if( thresh > 0.0 )
  {
      if( s <= thresh ) return 1.0 ;
      else return 0.0 ;
    }
    else return s ;

}

Here is the call graph for this function:

tfloat variance	(	tfloat	data[],
		unsigned long	N
	)

Computes the variance of the array data[] of length N.

Parameters

data[]
N

Returns

Definition at line 173 of file toolbox.c.

References mean(), and variance().

Referenced by triangulationSensitivity(), and variance().

{
  unsigned long i ;
  tfloat m = 0.0 , variance = 0.0 ;
  
  m = mean( data , N ) ;
  
  for( i = 0 ; i < N ; ++i )
    variance += (data[i]-m) * (data[i]-m) ;
  variance /= N ;
  
  return variance ;
}

Here is the call graph for this function:

int writePGM	(	tfloat	image[],
		unsigned int	width,
		unsigned int	height,
		unsigned int	Max,
		char *	fileName
	)

Writes a PGM ASCII file in 'fileName'. image[] is the array of values, width/height are the dimensions, Max is the maximum desired value in the file. http://netpbm.sourceforge.net/doc/ppm.html http://en.wikipedia.org/wiki/Netpbm_format

Parameters

image[]
width
height
Max
fileName

Returns

Definition at line 513 of file toolbox.c.

Referenced by saveMapAndScale().

{
    unsigned int i , j ;
  FILE *file = fopen( fileName , "w" ) ; if( file == NULL ) return -1 ;

    /* write PGM ASCII file */
    fprintf( file , "P2\n" ) ;
    fprintf( file , "%d\n", width ) ;
    fprintf( file , "%d\n", height ) ;
    fprintf( file , "%d\n", Max ) ;
    for( j = 0 ; j < height ; ++j )
    {
      for( i = 0 ; i < width ; ++i )
      {
        fprintf( file , "%u ", ( unsigned int ) image [ (height-j-1)*width + i ] ) ;
      }
      fprintf( file , "\n" ) ;
    }
    
    fclose( file ) ;
    
    return 0 ;
}

int writePPM	(	tfloat	image[],
		unsigned int	width,
		unsigned int	height,
		unsigned int	Max,
		char *	fileName
	)

Writes a PPM ASCII file in 'fileName'. image[] is the array of values, width/height are the dimensions, Max is the maximum desired value in the file. http://netpbm.sourceforge.net/doc/ppm.html http://en.wikipedia.org/wiki/Netpbm_format

Parameters

image[]
width
height
Max
fileName

Returns

Definition at line 551 of file toolbox.c.

Referenced by saveMapAndScale().

{
    unsigned int i , j ;
  FILE *file = fopen( fileName , "w" ) ; if( file == NULL ) return -1 ;

    /* write PGM ASCII file */
    fprintf( file , "P3\n" ) ;
    fprintf( file , "%d\n", width ) ;
    fprintf( file , "%d\n", height ) ;
    fprintf( file , "%d\n", Max ) ;
    for( j = 0 ; j < height ; ++j )
    {
      for( i = 0 ; i < width ; ++i )
      {
        fprintf( file , "%u ", ( unsigned int ) image [ (height-j-1)*width*3 + i*3 ] ) ;
        fprintf( file , "%u ", ( unsigned int ) image [ (height-j-1)*width*3 + i*3 + 1 ] ) ;
        fprintf( file , "%u  ", ( unsigned int ) image [ (height-j-1)*width*3 + i*3 + 2 ] ) ;
      }
      fprintf( file , "\n" ) ;
    }
    
    fclose( file ) ;
    
    return 0 ;
}