C library of triangulation algorithms (for the problems of mobile robot positioning or resection)
toolbox.c
Go to the documentation of this file.
1 /**
2  * @file toolbox.c
3  * @brief ...
4  * @author Vincent Pierlot
5 */
6 
7 #include <stdio.h>
8 #include <stdlib.h>
9 #include <math.h>
10 #include <sys/time.h>
11 #include "const.h"
12 #include "toolbox.h"
13 #include "methods.h"
14 
15 #ifdef _WIN32
16 long int random(void) { return (long int) rand(); }
17 void srandom(unsigned int seed) { srand(seed); }
18 #endif
19 
20 /**
21 * @brief
22 *
23 * @param in
24 * @param out
25 * @param width
26 * @param height
27 * @param numgraylevels
28 */
29 static void map_to_grayscale_hist ( tfloat * in , tfloat * out , unsigned int width , unsigned int height , unsigned int numgraylevels );
30 
31 /**
32 * @brief
33 *
34 * @param in
35 * @param out
36 * @param width
37 * @param height
38 * @param p
39 */
40 static void map_to_grayscale_sat ( tfloat * in , tfloat * out , unsigned int width , unsigned int height , unsigned int p );
41 
42 /**
43 * @brief
44 *
45 * @param in
46 * @param out
47 * @param scale
48 * @param width
49 * @param height
50 * @param numgraylevels
51 */
52 static void map_to_grayscale_lin ( tfloat * in , tfloat * out , tfloat * scale , unsigned int width , unsigned int height , unsigned int numgraylevels );
53 
54 /**
55 * @brief
56 *
57 * @param in
58 * @param out
59 * @param scale
60 * @param width
61 * @param height
62 * @param numgraylevels
63 * @param P1
64 */
65 static void map_to_grayscale_log ( tfloat * in , tfloat * out , tfloat * scale , unsigned int width , unsigned int height , unsigned int numgraylevels , tfloat P1 );
66 
67 
68 /**
69 * Computes the angles (a1,a2,a3) measured by a robot (xr,yr) from beacons (x1,y1), (x2,y2), (x3,y3).
70 * The orientation of the robot is given by 'or', and angles are computed relatively to 'or'.
71 *
72 * @param a1
73 * @param a2
74 * @param a3
75 * @param xr
76 * @param yr
77 * @param or
78 * @param x1
79 * @param y1
80 * @param x2
81 * @param y2
82 * @param x3
83 * @param y3
84 */
85 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)
86 {
87  *a1 = atan2( (y1 - yr) , (x1 - xr) ) - or ;
88  *a2 = atan2( (y2 - yr) , (x2 - xr) ) - or ;
89  *a3 = atan2( (y3 - yr) , (x3 - xr) ) - or ;
90 }
91 
92 /**
93 * Triangulates robot position (x,y) from measured angles alpha1, alpha2, and alpha3,
94 * and beacons locations (x1,y1), (x2,y2), and (x3,y3).
95 * Uses triangulation method 'm' given in command line.
96 *
97 * @param x
98 * @param y
99 * @param alpha1
100 * @param alpha2
101 * @param alpha3
102 * @param x1
103 * @param y1
104 * @param x2
105 * @param y2
106 * @param x3
107 * @param y3
108 * @param method
109 *
110 * @return
111 */
113  tfloat alpha1, tfloat alpha2, tfloat alpha3,
114  tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3, int method)
115 {
116  switch( method )
117  {
118  case 1: return triangulationPierlot(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
119  case 2: return triangulationEsteves(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
120  case 3: return triangulationFontLlagunes(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
121  case 4: return triangulationZalama(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
122  case 5: return triangulationMcGillemTrigo(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
123  case 6: return triangulationMcGillemGeometric(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
124  case 7: return triangulationEaston(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
125  case 8: return triangulationHmam(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
126  case 9: return triangulationCohenTrigo(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
127  case 10: return triangulationCohenGeometric(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
128  case 11: return triangulationCohenGeometricOriginal(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
129  case 12: return triangulationTsukiyama(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
130  case 13: return triangulationTsukiyamaOriginal(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
131  case 14: return triangulationMadsen(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
132  case 15: return triangulationLukic(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
133  case 16: return triangulationLukicOriginal(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
134  case 17: return triangulationTienstra(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
135  case 18: return triangulationLigas(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
136  case 19: return triangulationFontLlagunes2(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
137  case 20: return triangulationKaestner(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
138  case 21: return triangulationCollins(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
139  case 22: return triangulationCassini(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
140  case 42: return triangulationPierlot2(x, y, alpha1, alpha2, alpha3, x1, y1, x2, y2, x3, y3) ;
141  default: fprintf(stderr, "triangulationMethod(): Bad method number!\n") ; return -1 ;
142  }
143 }
144 
145 /**
146 * @brief Computes the algebraic mean of the array data[] of length N.
147 *
148 * @param data[]
149 * @param N
150 *
151 * @return
152 */
153 tfloat mean( tfloat data[] , unsigned long N )
154 {
155  unsigned long i ;
156  tfloat mean = 0.0 ;
157 
158  for( i = 0 ; i < N ; ++i )
159  mean += data[i] ;
160  mean /= N ;
161 
162  return mean ;
163 }
164 
165 /**
166 * @brief Computes the variance of the array data[] of length N.
167 *
168 * @param data[]
169 * @param N
170 *
171 * @return
172 */
173 tfloat variance( tfloat data[] , unsigned long N )
174 {
175  unsigned long i ;
176  tfloat m = 0.0 , variance = 0.0 ;
177 
178  m = mean( data , N ) ;
179 
180  for( i = 0 ; i < N ; ++i )
181  variance += (data[i]-m) * (data[i]-m) ;
182  variance /= N ;
183 
184  return variance ;
185 }
186 
187 /**
188 * @brief Initializes the random number generator with current time.
189 */
190 void initRand()
191 {
192  struct timeval startTime ;
193  gettimeofday( &startTime , NULL ) ;
194  srandom( startTime.tv_usec ) ;
195 }
196 
197 /**
198 * Returns an occurence of a normally distributed random variable.
199 * Uses the Marsaglia polar method (polar version of Box-Muller transform).
200 * http://en.wikipedia.org/wiki/Box_Muller_transform
201 *
202 * @return
203 */
205 {
206  tfloat u , v , s = 0.0 ;
207 
208  while( s == 0.0 || s >= 1.0 )
209  {
210  u = ( ( (tfloat) random() / (tfloat) RAND_MAX ) * 2.0 ) - 1.0 ;
211  v = ( ( (tfloat) random() / (tfloat) RAND_MAX ) * 2.0 ) - 1.0 ;
212  s = u*u + v*v ;
213  }
214 
215  return u * sqrt( -2 * log(s) / s ) ;
216 }
217 
218 
219 /**
220 * Simulates the beacon position sensitivity of the triangulation method 'm', for measured angles alpha1, alpha2, and alpha3,
221 * and beacons locations (x1,y1), (x2,y2), and (x3,y3).
222 * 'sigma' is the standard deviation of beacon coordinates.
223 * The simulation is performed 'n' times.
224 *
225 * @param xTrue
226 * @param yTrue
227 * @param x1
228 * @param y1
229 * @param x2
230 * @param y2
231 * @param x3
232 * @param y3
233 * @param method
234 * @param sigmaAngle
235 * @param sigmaBeacon
236 * @param n
237 * @param outputType
238 * @param thresh
239 *
240 * @return
241 */
243  int method, tfloat sigmaAngle, tfloat sigmaBeacon, unsigned int n, int outputType, double thresh)
244 {
245  tfloat alpha1 ;
246  tfloat alpha2 ;
247  tfloat alpha3 ;
248  tfloat oTrue = 0.0 ;
249  tfloat xNoise = 0.0 ;
250  tfloat yNoise = 0.0 ;
251  tfloat oNoise = 0.0 ;
252  tfloat qual = 0.0 ;
253 
254  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 ;
255  tfloat *errP = ( tfloat * ) malloc( n * sizeof(tfloat) ) ; if( errP == NULL ) return -1 ;
256  tfloat *errO = ( tfloat * ) malloc( n * sizeof(tfloat) ) ; if( errO == NULL ) return -1 ;
257  tfloat *Qual = ( tfloat * ) malloc( n * sizeof(tfloat) ) ; if( Qual == NULL ) return -1 ;
258 
259  int i ;
260 
261  initRand() ;
262 
263  for ( i = 0 ; i < n ; ++i )
264  {
265  if ( sigmaAngle != 0.0 )
266  {
267  ea1 = gaussianRand() * sigmaAngle ;
268  ea2 = gaussianRand() * sigmaAngle ;
269  ea3 = gaussianRand() * sigmaAngle ;
270  }
271  if ( sigmaBeacon != 0.0 )
272  {
273  ex1 = gaussianRand() * sigmaBeacon ;
274  ey1 = gaussianRand() * sigmaBeacon ;
275  ex2 = gaussianRand() * sigmaBeacon ;
276  ey2 = gaussianRand() * sigmaBeacon ;
277  ex3 = gaussianRand() * sigmaBeacon ;
278  ey3 = gaussianRand() * sigmaBeacon ;
279  }
280 
281  get_angles(&alpha1, &alpha2, &alpha3, xTrue, yTrue, 0.0, x1+ex1, y1+ey1, x2+ex2, y2+ey2, x3+ex3, y3+ey3) ;
282  qual = triangulationMethod(&xNoise, &yNoise, alpha1+ea1, alpha2+ea2, alpha3+ea3, x1, y1, x2, y2, x3, y3, method) ;
283  qual = abs( qual ) ;
284  oNoise = atan2( (y1-yNoise) , (x1-xNoise) ) - alpha1 ;
285  /*tfloat oNoise2, oNoise3, sinMean, cosMean ;
286  oNoise2 = atan2( (y2-yNoise) , (x2-xNoise) ) - alpha2 ;
287  oNoise3 = atan2( (y3-yNoise) , (x3-xNoise) ) - alpha3 ;
288  sinMean = (sin(oNoise)+sin(oNoise2)+sin(oNoise3))/3;
289  cosMean = (cos(oNoise)+cos(oNoise2)+cos(oNoise3))/3;
290  oNoise = atan2( sinMean , cosMean ) ;*/
291 
292  errP[ i ] = sqrt( (xTrue-xNoise)*(xTrue-xNoise) + (yTrue-yNoise)*(yTrue-yNoise) ) ;
293  errO[ i ] = oTrue - oNoise ; while( errO[ i ] > PI ) errO[ i ] -= TWOPI ; while( errO[ i ] <= -PI ) errO[ i ] += TWOPI ;
294  Qual[ i ] = qual ;
295  }
296 
297  switch( outputType )
298  {
299  case 1: s = sqrt ( variance ( errP , n ) ) ; break ;
300  case 2: s = sqrt ( variance ( errO , n ) ) * RAD2DEG ; break ;
301  case 3: s = mean ( Qual , n ) ; break ;
302  default: fprintf( stderr , "triangulationSensitivity(): Bad output type!\n" ) ; return -1 ;
303  }
304 
305  free ( errP ) ;
306  free ( errO ) ;
307  free ( Qual ) ;
308 
309  if( thresh > 0.0 )
310  {
311  if( s <= thresh ) return 1.0 ;
312  else return 0.0 ;
313  }
314  else return s ;
315 
316 }
317 
318 
319 /**
320 * Computes the first order Geometric Dilution of Precision (GDOP) for triangulation,
321 * given the robot position (xr,yr), and beacons locations (x1,y1), (x2,y2), and (x3,y3).
322 *
323 * @param xr
324 * @param yr
325 * @param x1
326 * @param y1
327 * @param x2
328 * @param y2
329 * @param x3
330 * @param y3
331 *
332 * @return
333 */
335 {
336  tfloat jacobian[3][3] , det , d1 , d2 , d3 ;
337 
338  d1 = ( x1 - xr ) * ( x1 - xr ) + ( y1 - yr ) * ( y1 - yr ) ;
339  d2 = ( x2 - xr ) * ( x2 - xr ) + ( y2 - yr ) * ( y2 - yr ) ;
340  d3 = ( x3 - xr ) * ( x3 - xr ) + ( y3 - yr ) * ( y3 - yr ) ;
341 
342  jacobian[0][0] = ( y1 - yr ) / d1 ; jacobian[0][1] = ( x1 - xr ) / d1 ; jacobian[0][2] = 1.0 ;
343  jacobian[1][0] = ( y2 - yr ) / d2 ; jacobian[1][1] = ( x2 - xr ) / d2 ; jacobian[1][2] = 1.0 ;
344  jacobian[2][0] = ( y3 - yr ) / d3 ; jacobian[2][1] = ( x3 - xr ) / d3 ; jacobian[2][2] = 1.0 ;
345 
346  det = detMatrix33( jacobian ) ;
347 
348  return 1.0 / fabs( det ) ;
349 }
350 
351 /**
352 * @brief Computes the determinant of a 3x3 matrix.
353 *
354 * @param M[][3]
355 *
356 * @return
357 */
358 inline tfloat detMatrix33(tfloat M[][3])
359 {
360  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] ;
361 }
362 
363 
364 #define SWP 20 /* Scale Width Percentage */
365 /**
366 * Scales 'image[]' according to 'mode', 'Max', and 'P1'.
367 * Saves scaled image in "map.pgm" and corresponding scale in "scale.pgm"
368 *
369 * @param image[]
370 * @param width
371 * @param height
372 * @param mode
373 * @param Max
374 * @param P1
375 *
376 * @return
377 */
378 int saveMapAndScale(tfloat image[] , unsigned int width, unsigned int height, unsigned int mode, unsigned int Max, tfloat P1)
379 {
380  tfloat *map = ( tfloat * ) malloc( width * height * sizeof(tfloat) ) ; if( map == NULL ) return -1 ;
381  tfloat *scale = ( tfloat * ) malloc( (width/SWP) * height * sizeof(tfloat) ) ; if( scale == NULL ) return -1 ;
382  tfloat *map_color = ( tfloat * ) malloc( width * height * 3 * sizeof(tfloat) ) ; if( map_color == NULL ) return -1 ;
383  tfloat *scale_color = ( tfloat * ) malloc( (width/SWP) * height * 3 * sizeof(tfloat) ) ; if( scale_color == NULL ) return -1 ;
384 
385  switch( mode )
386  {
387  case 0:
388  map_to_grayscale_lin( image , map , scale , width , height , Max ) ;
389  break;
390  case 1:
391  map_to_grayscale_log( image , map , scale , width , height , Max , P1 ) ;
392  break;
393  case 2:
394  map_to_grayscale_hist( image , map , width , height , Max ) ;
395  break;
396  case 3:
397  map_to_grayscale_sat( image , map , width , height , 100 ) ;
398  map_to_grayscale_lin( map , map , scale , width , height , Max ) ;
399  break;
400  case 4:
401  map_to_grayscale_sat( image , map , width , height , 100 ) ;
402  map_to_grayscale_log( map , map , scale , width , height , Max , P1 ) ;
403  break;
404  default:
405  fprintf(stderr, "saveMapAndScale(): Bad map scaling mode!\n") ;
406  free( map ) ;
407  free( scale ) ;
408  free( map_color ) ;
409  free( scale_color ) ;
410  return -1 ;
411  }
412  writePGM( map , width , height , Max , "map.pgm" ) ;
413  writePGM( scale , width/SWP , height , Max , "scale.pgm" ) ; /* for now, there is no scale for mode 2. To Do */
414 
415  grayscale2RGB( map , map_color , width , height , Max ) ;
416  grayscale2RGB( scale , scale_color , width/SWP , height , Max ) ;
417 
418  writePPM( map_color , width , height , Max , "map.ppm" ) ;
419  writePPM( scale_color , width/SWP , height , Max , "scale.ppm" ) ; /* for now, there is no scale for mode 2. To Do */
420 
421  /* free memory */
422  free( map ) ;
423  free( scale ) ;
424  free( map_color ) ;
425  free( scale_color ) ;
426 
427  return 0 ;
428 }
429 
430 /**
431 * @brief Convert a grayscale to an RGB image.
432 *
433 * @param imageG[]
434 * @param imageRGB[]
435 * @param width
436 * @param height
437 * @param Max
438 */
439 void grayscale2RGB( tfloat imageG[] , tfloat imageRGB[] , unsigned int width , unsigned int height , unsigned int Max )
440 {
441  unsigned int i , j ;
442  tfloat v ;
443  color c ;
444 
445  for( j = 0 ; j < height ; ++j )
446  {
447  for( i = 0 ; i < width ; ++i )
448  {
449  v = imageG [ (height-j-1)*width + i ] ;
450  c = mapRGB( v , 0.0 , Max ) ;
451  imageRGB [ (height-j-1)*width*3 + i*3 + 0 ] = c.r * Max ;
452  imageRGB [ (height-j-1)*width*3 + i*3 + 1 ] = c.g * Max ;
453  imageRGB [ (height-j-1)*width*3 + i*3 + 2 ] = c.b * Max ;
454  }
455  }
456 }
457 
458 /**
459 * Return a RGB color value given a scalar v in the range [vmin,vmax]
460 * In this case each colour component ranges from 0 (no contribution) to
461 * 1 (fully saturated), modifications for other ranges is trivial.
462 * The colour is clipped at the end of the scales if v is outside
463 * the range [vmin,vmax]
464 * http://stackoverflow.com/questions/7706339/grayscale-to-red-green-blue-matlab-jet-color-scale
465 *
466 * @param v
467 * @param vmin
468 * @param vmax
469 *
470 * @return
471 */
472 color mapRGB( tfloat v , tfloat vmin , tfloat vmax )
473 {
474  color c = { 1.0 , 1.0 , 1.0 } ; /* white */
475  tfloat dv ;
476 
477  if (v < vmin) v = vmin ;
478  if (v > vmax) v = vmax ;
479  dv = vmax - vmin ;
480 
481  if ( v < ( vmin + 0.25 * dv ) ) {
482  c.r = 0 ;
483  c.g = 4 * ( v - vmin ) / dv ;
484  } else if ( v < ( vmin + 0.5 * dv ) ) {
485  c.r = 0 ;
486  c.b = 1 + 4 * ( vmin + 0.25 * dv - v ) / dv ;
487  } else if ( v < ( vmin + 0.75 * dv ) ) {
488  c.r = 4 * ( v - vmin - 0.5 * dv ) / dv ;
489  c.b = 0 ;
490  } else {
491  c.g = 1 + 4 * ( vmin + 0.75 * dv - v ) / dv ;
492  c.b = 0 ;
493  }
494 
495  return c ;
496 }
497 
498 
499 /**
500 * Writes a PGM ASCII file in 'fileName'.
501 * image[] is the array of values, width/height are the dimensions, Max is the maximum desired value in the file.
502 * http://netpbm.sourceforge.net/doc/ppm.html
503 * http://en.wikipedia.org/wiki/Netpbm_format
504 *
505 * @param image[]
506 * @param width
507 * @param height
508 * @param Max
509 * @param fileName
510 *
511 * @return
512 */
513 int writePGM( tfloat image[] , unsigned int width , unsigned int height , unsigned int Max , char* fileName )
514 {
515  unsigned int i , j ;
516  FILE *file = fopen( fileName , "w" ) ; if( file == NULL ) return -1 ;
517 
518  /* write PGM ASCII file */
519  fprintf( file , "P2\n" ) ;
520  fprintf( file , "%d\n", width ) ;
521  fprintf( file , "%d\n", height ) ;
522  fprintf( file , "%d\n", Max ) ;
523  for( j = 0 ; j < height ; ++j )
524  {
525  for( i = 0 ; i < width ; ++i )
526  {
527  fprintf( file , "%u ", ( unsigned int ) image [ (height-j-1)*width + i ] ) ;
528  }
529  fprintf( file , "\n" ) ;
530  }
531 
532  fclose( file ) ;
533 
534  return 0 ;
535 }
536 
537 /**
538 * Writes a PPM ASCII file in 'fileName'.
539 * image[] is the array of values, width/height are the dimensions, Max is the maximum desired value in the file.
540 * http://netpbm.sourceforge.net/doc/ppm.html
541 * http://en.wikipedia.org/wiki/Netpbm_format
542 *
543 * @param image[]
544 * @param width
545 * @param height
546 * @param Max
547 * @param fileName
548 *
549 * @return
550 */
551 int writePPM( tfloat image[] , unsigned int width , unsigned int height , unsigned int Max , char* fileName )
552 {
553  unsigned int i , j ;
554  FILE *file = fopen( fileName , "w" ) ; if( file == NULL ) return -1 ;
555 
556  /* write PGM ASCII file */
557  fprintf( file , "P3\n" ) ;
558  fprintf( file , "%d\n", width ) ;
559  fprintf( file , "%d\n", height ) ;
560  fprintf( file , "%d\n", Max ) ;
561  for( j = 0 ; j < height ; ++j )
562  {
563  for( i = 0 ; i < width ; ++i )
564  {
565  fprintf( file , "%u ", ( unsigned int ) image [ (height-j-1)*width*3 + i*3 ] ) ;
566  fprintf( file , "%u ", ( unsigned int ) image [ (height-j-1)*width*3 + i*3 + 1 ] ) ;
567  fprintf( file , "%u ", ( unsigned int ) image [ (height-j-1)*width*3 + i*3 + 2 ] ) ;
568  }
569  fprintf( file , "\n" ) ;
570  }
571 
572  fclose( file ) ;
573 
574  return 0 ;
575 }
576 
577 
578 /**
579 *
580 */
581 typedef struct {
582  unsigned int pos ;
583  tfloat value ;
585 
586 /**
587 * @brief
588 *
589 * @param a
590 * @param b
591 *
592 * @return
593 */
594 static int map_to_grayscale_hist_comp ( const void * a , const void * b )
595 {
598  if ( a_->value < b_->value ) return -1 ;
599  if ( a_->value > b_->value ) return 1 ;
600  return 0 ;
601 }
602 
603 /**
604 * @brief
605 *
606 * @param in
607 * @param out
608 * @param width
609 * @param height
610 * @param numgraylevels
611 */
612 static void map_to_grayscale_hist ( tfloat * in , tfloat * out , unsigned int width , unsigned int height , unsigned int numgraylevels )
613 {
614  struct_map_to_grayscale * tmp = ( struct_map_to_grayscale * ) malloc ( width * height * sizeof ( struct_map_to_grayscale ) ) ;
615  unsigned int pos ;
616  unsigned int N = (width * height) ;
617 
618  if ( tmp == NULL ) return ;
619  for ( pos = 0 ; pos < N ; ++ pos ) {
620  tmp [ pos ] .pos = pos ;
621  tmp [ pos ] .value = in [ pos ] ;
622  }
623 
624  qsort ( tmp , N , sizeof ( struct_map_to_grayscale ) , map_to_grayscale_hist_comp ) ;
625  printf( "min = %f\n" , tmp[ 0 ] .value ) ;
626  printf( "max = %f\n" , tmp[ N-1 ] .value ) ;
627 
628  for ( pos = 0 ; pos < N ; ++ pos ) {
629  out [ tmp [ pos ] .pos ] = pos * numgraylevels / N ;
630  }
631 
632  free ( tmp ) ;
633 }
634 
635 /**
636 * @brief
637 *
638 * @param in
639 * @param out
640 * @param width
641 * @param height
642 * @param p
643 */
644 static void map_to_grayscale_sat ( tfloat * in , tfloat * out , unsigned int width , unsigned int height , unsigned int p )
645 {
646  struct_map_to_grayscale * tmp = ( struct_map_to_grayscale * ) malloc ( width * height * sizeof ( struct_map_to_grayscale ) ) ;
647  unsigned int pos ;
648  unsigned int N = (width * height) ;
649  unsigned int Nmax = (width * height) - ( (width * height) / p ) ;
650  tfloat max;
651 
652  if ( tmp == NULL ) return ;
653  for ( pos = 0 ; pos < N ; ++ pos ) {
654  tmp [ pos ] .pos = pos ;
655  tmp [ pos ] .value = in [ pos ] ;
656  }
657 
658  qsort ( tmp , N , sizeof ( struct_map_to_grayscale ) , map_to_grayscale_hist_comp ) ;
659  max = tmp [ Nmax ] .value ;
660 
661  for ( pos = 0 ; pos < N ; ++ pos ) {
662  if( in [ pos ] > max ) out [ pos ] = max ;
663  else out [ pos ] = in [ pos ] ;
664  }
665 
666  free ( tmp ) ;
667 }
668 
669 /**
670 * @brief
671 *
672 * @param in
673 * @param out
674 * @param scale
675 * @param width
676 * @param height
677 * @param numgraylevels
678 */
679 static void map_to_grayscale_lin ( tfloat * in , tfloat * out , tfloat * scale , unsigned int width , unsigned int height , unsigned int numgraylevels )
680 {
681  unsigned int index , indexMin = 0 , indexMax = 0 , j ;
682  unsigned int N = (width * height) ;
683  tfloat min = in[0] , max = in[0] ;
684 
685  /* search extremum value */
686  for( index = 0 ; index < N ; ++index )
687  {
688  if( in[index] < min ) min = in[index] , indexMin = index ;
689  if( in[index] > max ) max = in[index] , indexMax = index ;
690  }
691  printf( "min = %f @ (%d,%d)\n" , min , indexMin/width , indexMin%width ) ;
692  printf( "max = %f @ (%d,%d)\n" , max , indexMax/width , indexMax%width ) ;
693 
694  /* rescale values */
695  for( index = 0 ; index < N ; ++index )
696  {
697  out[index] = ( ( in[index] - min ) / ( max - min ) ) * numgraylevels ;
698  }
699 
700  /* create scale */
701  for( index = 0 ; index < height ; ++index )
702  {
703  for( j = 0 ; j < (width/SWP) ; ++j )
704  scale[ index*(width/SWP) + j ] = index * (tfloat)numgraylevels / (tfloat)height ;
705  }
706 }
707 
708 /**
709 * @brief
710 *
711 * @param in
712 * @param out
713 * @param scale
714 * @param width
715 * @param height
716 * @param numgraylevels
717 * @param P1
718 */
719 static void map_to_grayscale_log ( tfloat * in , tfloat * out , tfloat * scale , unsigned int width , unsigned int height , unsigned int numgraylevels , tfloat P1 )
720 {
721  unsigned int index , indexMin = 0 , indexMax = 0 , j ;
722  unsigned int N = (width * height) ;
723  tfloat min = in[0] , max = in[0] ;
724 
725  /* search extremum value */
726  for( index = 0 ; index < N ; ++index )
727  {
728  if( in[index] < min ) min = in[index] , indexMin = index ;
729  if( in[index] > max ) max = in[index] , indexMax = index ;
730  }
731  printf( "min = %f @ (%d,%d)\n" , min , indexMin/width , indexMin%width ) ;
732  printf( "max = %f @ (%d,%d)\n" , max , indexMax/width , indexMax%width ) ;
733 
734  /* rescale values */
735  for( index = 0 ; index < N ; ++index )
736  {
737  out[index] = log( 1 + P1 * (( in[index] - min ) / ( max - min )) ) * (tfloat)numgraylevels / log( 1 + P1 ) ;
738  }
739 
740  /* create scale */
741  for( index = 0 ; index < height ; ++index )
742  {
743  for( j = 0 ; j < (width/SWP) ; ++j )
744  scale[ index*(width/SWP) + j ] = log( 1 + P1 * ( (tfloat)index / (tfloat)height ) ) * (tfloat)numgraylevels / log( 1 + P1 ) ;
745  }
746 }
747