Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 204 | chris | 1 | /******************************************************* |
| 2 | * |
||
| 3 | * Author: Hirokazu Kato |
||
| 4 | * |
||
| 5 | * kato@sys.im.hiroshima-cu.ac.jp |
||
| 6 | * |
||
| 7 | * Revision: 2.1 |
||
| 8 | * Date: 99/07/16 |
||
| 9 | * |
||
| 10 | *******************************************************/ |
||
| 11 | |||
| 12 | #include <stdio.h> |
||
| 13 | #include <math.h> |
||
| 14 | #include <AR/param.h> |
||
| 15 | |||
| 16 | #define PD_LOOP 3 |
||
| 17 | |||
| 18 | int arParamObserv2Ideal( const double dist_factor[4], const double ox, const double oy, |
||
| 19 | double *ix, double *iy ) |
||
| 20 | { |
||
| 21 | double z02, z0, p, q, z, px, py; |
||
| 22 | int i; |
||
| 23 | |||
| 24 | px = ox - dist_factor[0]; |
||
| 25 | py = oy - dist_factor[1]; |
||
| 26 | p = dist_factor[2]/100000000.0; |
||
| 27 | z02 = px*px+ py*py; |
||
| 28 | q = z0 = sqrt(px*px+ py*py); |
||
| 29 | |||
| 30 | for( i = 1; ; i++ ) { |
||
| 31 | if( z0 != 0.0 ) { |
||
| 32 | z = z0 - ((1.0 - p*z02)*z0 - q) / (1.0 - 3.0*p*z02); |
||
| 33 | px = px * z / z0; |
||
| 34 | py = py * z / z0; |
||
| 35 | } |
||
| 36 | else { |
||
| 37 | px = 0.0; |
||
| 38 | py = 0.0; |
||
| 39 | break; |
||
| 40 | } |
||
| 41 | if( i == PD_LOOP ) break; |
||
| 42 | |||
| 43 | z02 = px*px+ py*py; |
||
| 44 | z0 = sqrt(px*px+ py*py); |
||
| 45 | } |
||
| 46 | |||
| 47 | *ix = px / dist_factor[3] + dist_factor[0]; |
||
| 48 | *iy = py / dist_factor[3] + dist_factor[1]; |
||
| 49 | |||
| 50 | return(0); |
||
| 51 | } |
||
| 52 | |||
| 53 | int arParamIdeal2Observ( const double dist_factor[4], const double ix, const double iy, |
||
| 54 | double *ox, double *oy ) |
||
| 55 | { |
||
| 56 | double x, y, d; |
||
| 57 | |||
| 58 | x = (ix - dist_factor[0]) * dist_factor[3]; |
||
| 59 | y = (iy - dist_factor[1]) * dist_factor[3]; |
||
| 60 | if( x == 0.0 && y == 0.0 ) { |
||
| 61 | *ox = dist_factor[0]; |
||
| 62 | *oy = dist_factor[1]; |
||
| 63 | } |
||
| 64 | else { |
||
| 65 | d = 1.0 - dist_factor[2]/100000000.0 * (x*x+y*y); |
||
| 66 | *ox = x * d + dist_factor[0]; |
||
| 67 | *oy = y * d + dist_factor[1]; |
||
| 68 | } |
||
| 69 | |||
| 70 | return(0); |
||
| 71 | } |