C library of triangulation algorithms (for the problems of mobile robot positioning or resection)
triangulation.c
Go to the documentation of this file.
1 /**
2 * @file triangulation.c
3 * @author Vincent Pierlot [ vpierlot@gmail.com ]
4 * @date April 2013
5 * @version: 0.7
6 *
7 * Three Object Triangulation algorithms implementation, simulation and benchmarking
8 * compile with: make
9 * run with: ./triangulation [options]
10 */
11 
12 #include <stdio.h>
13 #include <stdlib.h>
14 #include <unistd.h>
15 #include <math.h>
16 #include <time.h>
17 
18 #include "const.h"
19 #include "toolbox.h"
20 
21 
22 #define AUTHOR "Vincent Pierlot"
23 #define EMAIL "vpierlot@gmail.com"
24 #define VERSION "0.7"
25 #define DEFAULT_PROGNAME "triangulation"
26 
27 
28 int test0 (tfloat xr, tfloat yr, tfloat or, tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3);
29 int test1 (tfloat alpha1, tfloat alpha2, tfloat alpha3, tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3, int m);
30 int test2 (tfloat xr, tfloat yr, tfloat or, tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3, int m);
31 int test3 (tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3, int m,
32  tfloat xstart, tfloat xend, tfloat ystart, tfloat yend, unsigned int n);
33 int test4 (tfloat xr, tfloat yr, tfloat or, tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3,
34  int m, tfloat sigmaAngle, tfloat sigmaBeacon, unsigned int n, int O);
35 int test5 (tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3, int m, tfloat sigmaAngle, tfloat sigmaBeacon, unsigned int n,
36  tfloat xstart, tfloat xend , tfloat ystart , tfloat yend, tfloat step, unsigned int map, int O, double thresh);
37 
38 void usage(const char *progName);
39 
40 
41 /**
42 * @brief Print usage informations.
43 *
44 * Print usage informations,
45 * the only argument is <progName> and is typically argv[0]
46 *
47 * @param progName
48 */
49 void usage( const char *progName )
50 {
51  fprintf(stderr ,
52  DEFAULT_PROGNAME " " VERSION " by " AUTHOR " [" EMAIL "]\n"
53  "Three Object Triangulation algorithms implementation, simulation and benchmarking.\n"
54  "http://www2.ulg.ac.be/telecom/triangulation/\n"
55  "http://hdl.handle.net/2268/89435\n"
56  "Usage: %s [options]\n"
57  "[options]:\n"
58  " -t int Test to execute (default=5):\n"
59  " 0: gets angles from robot pose (x;y;o). Displays angles (a;b;c).\n"
60  " 1: triangulates from measured angles (a;b;c). Displays robot pose (x;y;o).\n"
61  " 2: gets angles from robot pose (x;y;o) and triangulates with measured angles. Displays angles and pose.\n"
62  " 3: runs triangulation method n times from random positions in the grid [-x,X]*[y,Y]. Displays execution time.\n"
63  " 4: runs triangulation sensitivity analysis from robot position (x;y). (see -O for output).\n"
64  " 5: runs triangulation sensitivity analysis for each point in the grid [-x,X]*[y,Y]. (see -O for output).\n"
65  " Writes PGM images \"map.pgm\" and \"scale.pgm\" in current directory. Displays step, min and max values.\n"
66  " -m int Triangulation method to use (default=1). For all tests.\n"
67  " 1: Pierlot (ToTal) 2: Esteves 3: FontLlagunes (T) 4: Zalama\n"
68  " 5: McGillem (T) 6: McGillem (G) 7: Easton 8: Hmam\n"
69  " 9: Cohen (T) 10: Cohen (G) 11: Cohen (G)* 12: Tsukiyama\n"
70  " 13: Tsukiyama* 14: Madsen 15: Lukic 16: Lukic*\n"
71  " 17: Tienstra 18: Ligas 19: FontLlagunes (G) 20: Kaestner\n"
72  " 21: Collins 22: Cassini\n"
73  " (*) original method from the paper (typically slower than my implementation)\n"
74  " (G) Geometric solution\n"
75  " (T) Trigonometric solution\n"
76  " -x float X robot position. For tests 0, 2, 4. (default=0.0)\n"
77  " X min value of test grid. For tests 3, 5. (default=-2.0)\n"
78  " -y float Y robot position. For tests 0, 2, 4. (default=0.0)\n"
79  " Y min value of test grid. For tests 3, 5. (default=-2.0)\n"
80  " -o float Robot orientation relative to X axis [deg]. For tests 0, 2, 4. (default=0.0)\n"
81  " -a float alpha1 angle [deg]. For test 1. (default=+90)\n"
82  " -b float alpha2 angle [deg]. For test 1. (default=-150)\n"
83  " -c float alpha3 angle [deg]. For test 1. (default=-30)\n"
84  " -1 float X1 beacon1 position. For all tests. (default=0.0)\n"
85  " -2 float Y1 beacon1 position. For all tests. (default=+1.0)\n"
86  " -3 float X2 beacon2 position. For all tests. (default=-0.866)\n"
87  " -4 float Y2 beacon2 position. For all tests. (default=-0.5)\n"
88  " -5 float X3 beacon3 position. For all tests. (default=+0.866)\n"
89  " -6 float Y3 beacon3 position. For all tests. (default=-0.5)\n"
90  " -O int Output of the sensitivity analysis. For tests 4, 5. (default=1)\n"
91  " 1: std deviation of position error, 2: std deviation of orientation error, 3: return value of method.\n"
92  " -X float X max value of test grid. For tests 3, 5. (default=+2.0)\n"
93  " -Y float Y max value of test grid. For tests 3, 5. (default=+2.0)\n"
94  " -p float Discretization step of the test grid. For test 5. (default=0.02)\n"
95  " -A float Standard deviation of the gaussian noise applied to angles in [deg]. For tests 4, 5. (default=0.1)\n"
96  " -B float Standard deviation of the gaussian noise applied to beacons coordinates. For tests 4, 5. (default=0.0)\n"
97  " -n int Number of tests for the execution time and the sensitivity analysis. For tests 3, 4, 5. (default=100)\n"
98  " -M int Map used to display results from the sensitivity analysis. For test 5. (default=2)\n"
99  " 0: Linear.\n"
100  " 1: Logarithmic.\n"
101  " 2: Histogram equalized.\n"
102  " 3: Linear after removing outliers (1 percent of the highest values).\n"
103  " 4: Logarithmic after removing outliers (1 percent of the highest values).\n"
104  " -T float Threshold for the sensitivity analysis output (must be >= 0 to be active). For test 5. (default=-1)\n"
105  " -h Displays this help and exit.\n"
106  "\n"
107  , progName ) ;
108  exit( -1 ) ;
109 }
110 
111 
112 int main ( int argc , char *argv[] )
113 {
114  /* default testing mode */
115  int t = 5 ;
116 
117  /* default triangulation method */
118  int m = 1 ;
119 
120  /* default robot position and orientation */
121  tfloat xr = 0 ;
122  tfloat yr = 0 ;
123  tfloat or = 0 ;
124 
125  /* default angles (from default robot position and orientation, and beacon locations) */
126  tfloat a1 = 90.0 * DEG2RAD ;
127  tfloat a2 = -150.0 * DEG2RAD ;
128  tfloat a3 = -30.0 * DEG2RAD ;
129 
130  /* default beacon locations (regular triangle) */
131  tfloat x1 = 0.0 ;
132  tfloat y1 = +1.0 ;
133  tfloat x2 = -sqrt(3.0)/2 ;
134  tfloat y2 = -0.5 ;
135  tfloat x3 = +sqrt(3.0)/2 ;
136  tfloat y3 = -0.5 ;
137 
138  /* default std dev for sensitivity analysis (in [deg] for angles) */
139  tfloat sigmaAngle = 0.1 * DEG2RAD ;
140  tfloat sigmaBeacon = 0.0 ;
141 
142  /* default bounds, number of points in image edge, number of tests for sensitivity analysis */
143  tfloat xstart = -2.0 ;
144  tfloat xend = +2.0 ;
145  tfloat ystart = -2.0 ;
146  tfloat yend = +2.0 ;
147  tfloat step = 0.02 ;
148  tfloat thresh = -1.0 ;
149  unsigned int n = 100 ;
150 
151  /* default map mode for the sensitivity analysis */
152  unsigned int map = 2 ;
153 
154  /* default output for the sensitivity analysis */
155  int O = 1 ;
156 
157  int optionChar;
158  char optString[] = "t:m:x:y:o:A:B:a:b:c:p:n:O:1:2:3:4:5:6:X:Y:M:T:h" ;
159 
160  /* Parse Command line */
161  while( ( optionChar = getopt(argc, argv, optString) ) != -1 )
162  {
163  switch( optionChar )
164  {
165  case '?' : return -1 ; break ; /* erroneous option or missing argument */
166  case 't' : t = atoi( optarg ) ; break ;
167  case 'm' : m = atoi( optarg ) ; break ;
168  case 'x' : xr = atof( optarg ) ; xstart = xr ; break ;
169  case 'y' : yr = atof( optarg ) ; ystart = yr ; break ;
170  case 'o' : or = atof( optarg )*DEG2RAD ; break ;
171  case 'A' : sigmaAngle = atof( optarg )*DEG2RAD ; break ;
172  case 'B' : sigmaBeacon = atof( optarg ) ; break ;
173  case 'a' : a1 = atof( optarg )*DEG2RAD ; break ;
174  case 'b' : a2 = atof( optarg )*DEG2RAD ; break ;
175  case 'c' : a3 = atof( optarg )*DEG2RAD ; break ;
176  case 'p' : step = atof( optarg ) ; break ;
177  case 'n' : n = atoi( optarg ) ; break ;
178  case 'O' : O = atoi( optarg ) ; break ;
179  case '1' : x1 = atof( optarg ) ; break ;
180  case '2' : y1 = atof( optarg ) ; break ;
181  case '3' : x2 = atof( optarg ) ; break ;
182  case '4' : y2 = atof( optarg ) ; break ;
183  case '5' : x3 = atof( optarg ) ; break ;
184  case '6' : y3 = atof( optarg ) ; break ;
185  case 'X' : xend = atof( optarg ) ; break ;
186  case 'Y' : yend = atof( optarg ) ; break ;
187  case 'M' : map = atoi( optarg ) ; break ;
188  case 'T' : thresh = atof( optarg ) ; break ;
189  case 'h' : usage( argv[0] ) ; return 0 ;
190  default : ;
191  }
192  }
193 
194  switch( t )
195  {
196  case 0: return test0( xr , yr , or , x1 , y1 , x2 , y2 , x3 , y3 ) ;
197  case 1: return test1( a1 , a2 , a3 , x1 , y1 , x2 , y2 , x3 , y3 , m ) ;
198  case 2: return test2( xr , yr , or , x1 , y1 , x2 , y2 , x3 , y3 , m ) ;
199  case 3: return test3( x1 , y1 , x2 , y2 , x3 , y3 , m , xstart , xend , ystart , yend , n ) ;
200  case 4: return test4( xr , yr , or , x1 , y1 , x2 , y2 , x3 , y3 , m , sigmaAngle , sigmaBeacon , n , O ) ;
201  case 5: return test5( x1 , y1 , x2 , y2 , x3 , y3 , m , sigmaAngle , sigmaBeacon , n , xstart , xend , ystart , yend , step , map , O , thresh ) ;
202  default: fprintf( stderr , "main(): Bad test number!\n" ) ; return -1 ;
203  }
204 
205 }
206 
207 
208 /*
209  *
210 */
211 int test0 (tfloat xr, tfloat yr, tfloat or, tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3)
212 {
213  tfloat alpha1 = 0.0 ;
214  tfloat alpha2 = 0.0 ;
215  tfloat alpha3 = 0.0 ;
216 
217  get_angles( &alpha1 , &alpha2 , &alpha3 , xr , yr , or , x1 , y1 , x2 , y2 , x3 , y3 ) ;
218 
219  printf( "\nMeasured angles:\n" ) ;
220  printf( "a = %f [deg]\n" , alpha1*RAD2DEG ) ;
221  printf( "b = %f [deg]\n" , alpha2*RAD2DEG ) ;
222  printf( "c = %f [deg]\n\n" , alpha3*RAD2DEG ) ;
223 
224  return 0 ;
225 }
226 
227 
228 /*
229  *
230 */
231 int test1 (tfloat alpha1, tfloat alpha2, tfloat alpha3, tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3, int m)
232 {
233  tfloat x = 0.0 ;
234  tfloat y = 0.0 ;
235  tfloat o = 0.0 ;
236 
237  tfloat Q = 0.0 ;
238 
239  Q = triangulationMethod( &x , &y , alpha1 , alpha2 , alpha3 , x1 , y1 , x2 , y2 , x3 , y3 , m ) ;
240  o = atan2( (y1-y) , (x1-x) ) - alpha1 ;
241 
242  printf( "\nComputed position:\n" ) ;
243  printf( "x = %f\n", x ) ;
244  printf( "y = %f\n", y ) ;
245  printf( "o = %f [deg]\n\n" , o*RAD2DEG ) ;
246  printf( "Error measure = %f\n\n" , Q) ;
247 
248  return 0 ;
249 }
250 
251 
252 /*
253  *
254 */
255 int test2 (tfloat xr, tfloat yr, tfloat or, tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3, int m)
256 {
257  tfloat x = 0 ;
258  tfloat y = 0 ;
259  tfloat o = 0 ;
260 
261  tfloat alpha1 = 0 ;
262  tfloat alpha2 = 0 ;
263  tfloat alpha3 = 0 ;
264 
265  tfloat Q = 0 ;
266 
267  get_angles( &alpha1 , &alpha2 , &alpha3 , xr , yr , or , x1 , y1 , x2 , y2 , x3 , y3 ) ;
268 
269  Q = triangulationMethod( &x , &y , alpha1 , alpha2 , alpha3 , x1 , y1 , x2 , y2 , x3 , y3 , m ) ;
270  o = atan2( (y1-y) , (x1-x) ) - alpha1 ;
271 
272  printf( "\nMeasured angles:\n" ) ;
273  printf( "a = %f [deg]\n" , alpha1*RAD2DEG ) ;
274  printf( "b = %f [deg]\n" , alpha2*RAD2DEG ) ;
275  printf( "c = %f [deg]\n" , alpha3*RAD2DEG ) ;
276  printf( "\nComputed position:\n") ;
277  printf( "x = %f\n" , x ) ;
278  printf( "y = %f\n" , y ) ;
279  printf( "o = %f [deg]\n\n" , o*RAD2DEG ) ;
280  printf( "Error measure = %f\n\n" , Q ) ;
281 
282  return 0 ;
283 }
284 
285 
286 /*
287  *
288 */
289 int test3 ( tfloat x1 , tfloat y1 , tfloat x2 , tfloat y2 , tfloat x3 , tfloat y3 , int m ,
290  tfloat xstart, tfloat xend , tfloat ystart , tfloat yend , unsigned int n )
291 {
292  tfloat xi = 0.0 ;
293  tfloat yi = 0.0 ;
294 
295  tfloat *alpha1M = ( tfloat * ) malloc( n * sizeof(tfloat) ) ; if( alpha1M == NULL ) return -1 ;
296  tfloat *alpha2M = ( tfloat * ) malloc( n * sizeof(tfloat) ) ; if( alpha2M == NULL ) return -1 ;
297  tfloat *alpha3M = ( tfloat * ) malloc( n * sizeof(tfloat) ) ; if( alpha3M == NULL ) return -1 ;
298 
299  int i , j ;
300  double t , tmin = 0.0 ;
301 
302  initRand() ;
303  srandom( 0 ) ;
304 
305  for( i = 0 ; i < n ; ++i )
306  {
307  xi = ( ( (tfloat) random() / (tfloat) RAND_MAX ) * ( xend - xstart ) ) + xstart ;
308  yi = ( ( (tfloat) random() / (tfloat) RAND_MAX ) * ( yend - ystart ) ) + ystart ;
309  get_angles( &alpha1M[i] , &alpha2M[i] , &alpha3M[i] , xi , yi , 0.0 , x1 , y1 , x2 , y2 , x3 , y3 ) ;
310  }
311 
312  for( j = 0 ; j < 10 ; ++j )
313  {
314 
315 #ifdef _WIN32 /* also valid for Linux but not precise */
316  clock_t startClock = clock() ;
317 #else
318  struct timespec startTime , endTime ;
319  clock_gettime( CLOCK_PROCESS_CPUTIME_ID , &startTime ) ;
320 #endif
321 
322  for( i = 0 ; i < n ; ++i )
323  {
324  triangulationMethod( &xi , &yi , alpha1M[i] , alpha2M[i] , alpha3M[i] , x1 , y1 , x2 , y2 , x3 , y3 , m ) ;
325  }
326 
327 #ifdef _WIN32 /* also valid for Linux but not precise */
328  clock_t endClock = clock() ;
329  t = ( (double) (endClock - startClock) ) / CLOCKS_PER_SEC ;
330 #else
331  clock_gettime( CLOCK_PROCESS_CPUTIME_ID , &endTime ) ;
332  t = ( (double) ((endTime.tv_sec-startTime.tv_sec)+(endTime.tv_nsec-startTime.tv_nsec)/1000000000.0)) ;
333  /*printf( "Duration = %10.5f [s]\n" , t ) ;*/
334 #endif
335 
336  if ( j == 0 ) tmin = t ;
337  if ( t < tmin ) tmin = t ;
338  }
339 
340  printf( "Duration = %10.5f [s]\n" , tmin ) ;
341 
342  free( alpha1M ) ;
343  free( alpha2M ) ;
344  free( alpha3M ) ;
345 
346  return 0 ;
347 }
348 
349 
350 /*
351  *
352 */
353 int test4 (tfloat xr, tfloat yr, tfloat or, tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3,
354  int m, tfloat sigmaAngle, tfloat sigmaBeacon, unsigned int n, int O)
355 {
356  tfloat alpha1 = 0.0 ;
357  tfloat alpha2 = 0.0 ;
358  tfloat alpha3 = 0.0 ;
359 
360  tfloat s = 0.0 ;
361 
362  get_angles( &alpha1 , &alpha2 , &alpha3 , xr , yr , or , x1 , y1 , x2 , y2 , x3 , y3 ) ;
363 
364  s = triangulationSensitivity( xr , yr , x1 , y1 , x2 , y2 , x3 , y3 , m , sigmaAngle , sigmaBeacon , n , O , -1 ) ;
365 
366  printf( "\nMeasured angles:\n" ) ;
367  printf( "a = %f [deg]\n" , alpha1*RAD2DEG ) ;
368  printf( "b = %f [deg]\n" , alpha2*RAD2DEG ) ;
369  printf( "c = %f [deg]\n" , alpha3*RAD2DEG ) ;
370 
371  switch( O )
372  {
373  case 1: printf( "\nPosition error standard deviation: %f\n" , s ) ; break ;
374  case 2: printf( "\nOrientation error standard deviation: %f [deg]\n" , s ) ; break ; /* already converted in degree */
375  case 3: printf( "\nMean error measure: %f\n" , s ) ; break ;
376  default: fprintf( stderr , "test4(): Bad output type!\n" ) ; return -1 ;
377  }
378 
379  return 0 ;
380 }
381 
382 
383 /*
384  *
385 */
386 int test5 ( tfloat x1 , tfloat y1 , tfloat x2 , tfloat y2 , tfloat x3 , tfloat y3 , int m , tfloat sigmaAngle , tfloat sigmaBeacon , unsigned int n ,
387  tfloat xstart , tfloat xend , tfloat ystart , tfloat yend , tfloat step , unsigned int map , int O , double thresh )
388 {
389  tfloat xi = 0.0 , yi = 0.0 ;
390 
391  unsigned int p = round( ( xend - xstart ) / step ) + 1 ;
392  unsigned int q = round( ( yend - ystart ) / step ) + 1 ;
393 
394  tfloat *errorPM = ( tfloat * ) malloc( p * q * sizeof(tfloat) ) ; if( errorPM == NULL ) return -1 ;
395 
396  int i , j ;
397 
398  printf( "Angles standard deviation = %f [deg]\n" , sigmaAngle*RAD2DEG ) ;
399  printf( "Beacons standard deviation = %f\n" , sigmaBeacon ) ;
400  printf( "grid X limits = %f to %f\n" , xstart , xend ) ;
401  printf( "grid Y limits = %f to %f\n" , ystart , yend ) ;
402  printf( "grid step = %f\n" , step ) ;
403 
404  for( yi = ystart, j = 0 ; j < q ; j++ )
405  {
406  for( xi = xstart, i = 0 ; i < p ; i++ )
407  {
408  switch ( m )
409  {
410  default :
411  errorPM[j*p + i] = triangulationSensitivity( xi , yi , x1 , y1 , x2 , y2 , x3 , y3 , m , sigmaAngle , sigmaBeacon , n , O , thresh ) ;
412  break ;
413  case 101 :
414  errorPM[j*p + i] = triangulationGDOP( xi , yi , x1 , y1 , x2 , y2 , x3 , y3 ) ;
415  break;
416  }
417  xi += step ;
418  }
419  yi += step ;
420  }
421 
422  saveMapAndScale( errorPM , p , q , map , 255 , 10 ) ;
423 
424  free( errorPM ) ;
425 
426  return 0 ;
427 }
428 
get_angles
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)
Definition: toolbox.c:85
initRand
void initRand()
Initializes the random number generator with current time.
Definition: toolbox.c:190
usage
void usage(const char *progName)
Print usage informations.
Definition: triangulation.c:49
triangulationSensitivity
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)
Definition: toolbox.c:242
tfloat
double tfloat
Defines the type for float/double.
Definition: const.h:38
triangulationMethod
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)
Definition: toolbox.c:112
saveMapAndScale
int saveMapAndScale(tfloat image[], unsigned int width, unsigned int height, unsigned int mode, unsigned int Max, tfloat P1)
Definition: toolbox.c:378
triangulationGDOP
tfloat triangulationGDOP(tfloat xr, tfloat yr, tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3)
Definition: toolbox.c:334
Generated on Wed Feb 22 2017 14:25:19 for C library of triangulation algorithms (for the problems of mobile robot positioning or resection) by   doxygen 1.8.11