~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

Linux Cross Reference
Tina4/src/pgh/im_pairs_points.c

Version: ~
Architecture: ~ [ i386 ] ~ [ alpha ] ~ [ m68k ] ~ [ mips ] ~ [ ppc ] ~ [ sparc ] ~ [ sparc64 ] ~

  1 #include <values.h>
  2 #include <tina/sys.h>
  3 #include <tina/sysfuncs.h>
  4 #include <tina/math.h>
  5 #include <tina/mathfuncs.h>
  6 #include <tina/vision.h>
  7 #include <tina/visionfuncs.h>
  8 #include <tina/tv.h>
  9 #include <tina/draw.h>
 10 #include <tina/drawfuncs.h>
 11 
 12 #include <pgh/pgh_defs.h>
 13 #include <pgh/pgh_types.h>
 14 #include <pgh/pgh_funcs.h>
 15 
 16 #include <math.h>
 17 
 18 
 19 
 20 /* ---------- Code from hough1_float.c APA 4/2/94 ---------- */
 21 
 22 /* ---------- Code from covariance.c APA 4/2/94 ---------- */
 23 
 24 void  get_covariance(Line2 *ue_lin1_s, Line2 *ue_lin2_s,
 25                      double *a, double *b, double *c, double *d)
 26 {
 27      double  matrix[8][2], matrix_T[2][8], inter_covar_matrix[2][2];
 28      double  det;
 29      float   ax1, ay1, ax2, ay2, bx1, by1, bx2, by2;
 30 
 31 /* Does a chi squared test by propagating the measurement errors through 
 32    if the chi squared value is high then the centroid is probably near the
 33    previously calculated centroid */
 34 
 35 /* Get data about the position of the two lines */
 36 
 37      ax1 = vec2_x(ue_lin1_s->p1);
 38      ay1 = vec2_y(ue_lin1_s->p1);
 39      ax2 = vec2_x(ue_lin1_s->p2);
 40      ay2 = vec2_y(ue_lin1_s->p2);
 41      bx1 = vec2_x(ue_lin2_s->p1);
 42      by1 = vec2_y(ue_lin2_s->p1);
 43      bx2 = vec2_x(ue_lin2_s->p2);
 44      by2 = vec2_y(ue_lin2_s->p2);
 45 
 46 /* First find the covariance matrix for the position of the centroid, then
 47    find the transform */
 48 
 49    partial_derv_covariance(ax1, ay1, ax2, ay2, bx1, by1, bx2, by2, &matrix[0][0], &matrix[0][1]);
 50    partial_derv_covariance(ax2, ay2, ax1, ay1, bx1, by1, bx2, by2, &matrix[2][0], &matrix[2][1]);
 51    partial_derv_covariance(ay1, ax1, ay2, ax2, by1, bx1, by2, bx2, &matrix[1][1], &matrix[1][0]);
 52    partial_derv_covariance(ay2, ax2, ay1, ax1, by1, bx1, by2, bx2, &matrix[3][1], &matrix[3][0]);
 53    partial_derv_covariance(bx1, by1, bx2, by2, ax1, ay1, ax2, ay2, &matrix[4][0], &matrix[4][1]);
 54    partial_derv_covariance(bx2, by2, bx1, by1, ax1, ay1, ax2, ay2, &matrix[6][0], &matrix[6][1]);
 55    partial_derv_covariance(by1, bx1, by2, bx2, ay1, ax1, ay2, ax2, &matrix[5][1], &matrix[5][0]);
 56    partial_derv_covariance(by2, bx2, by1, bx1, ay1, ax1, ay2, ax2, &matrix[7][1], &matrix[7][0]);
 57 
 58 /* Transpose the matrix */
 59 
 60      matrix_T[0][0] = matrix[0][0];
 61      matrix_T[0][1] = matrix[1][0];
 62      matrix_T[0][1] = matrix[1][0];
 63      matrix_T[0][2] = matrix[2][0];
 64      matrix_T[0][3] = matrix[3][0];
 65      matrix_T[0][4] = matrix[4][0];
 66      matrix_T[0][5] = matrix[5][0];
 67      matrix_T[0][6] = matrix[6][0];
 68      matrix_T[0][7] = matrix[7][0];
 69    
 70      matrix_T[1][0] = matrix[0][1];
 71      matrix_T[1][1] = matrix[1][1];
 72      matrix_T[1][2] = matrix[2][1];
 73      matrix_T[1][3] = matrix[3][1];
 74      matrix_T[1][4] = matrix[4][1];
 75      matrix_T[1][5] = matrix[5][1];
 76      matrix_T[1][6] = matrix[6][1];
 77      matrix_T[1][7] = matrix[7][1];
 78    
 79 /* Find the propagated covariance matrix for the intersection point */
 80 
 81      inter_covar_matrix[0][0] = (matrix_T[0][0]*matrix[0][0]) + (matrix_T[0][1]*matrix[1][0]);
 82      inter_covar_matrix[0][0] = inter_covar_matrix[0][0] + (matrix_T[0][2]*matrix[2][0]) 
 83                                 + (matrix_T[0][3]*matrix[3][0]);
 84      inter_covar_matrix[0][0] = inter_covar_matrix[0][0] + (matrix_T[0][4]*matrix[4][0]) 
 85                                 + (matrix_T[0][5]*matrix[5][0]);
 86      inter_covar_matrix[0][0] = inter_covar_matrix[0][0] + (matrix_T[0][6]*matrix[6][0]) 
 87                                 + (matrix_T[0][7]*matrix[7][0]);
 88 
 89      inter_covar_matrix[0][1] = (matrix_T[0][0]*matrix[0][1]) + (matrix_T[0][1]*matrix[1][1]);
 90      inter_covar_matrix[0][1] = inter_covar_matrix[0][1] + (matrix_T[0][2]*matrix[2][1]) 
 91                                 + (matrix_T[0][3]*matrix[3][1]);
 92      inter_covar_matrix[0][1] = inter_covar_matrix[0][1] + (matrix_T[0][4]*matrix[4][1]) 
 93                                 + (matrix_T[0][5]*matrix[5][1]);
 94      inter_covar_matrix[0][1] = inter_covar_matrix[0][1] + (matrix_T[0][6]*matrix[6][1]) 
 95                                 + (matrix_T[0][7]*matrix[7][1]);
 96 
 97      inter_covar_matrix[1][0] = (matrix_T[1][0]*matrix[0][0]) + (matrix_T[1][1]*matrix[1][0]);
 98      inter_covar_matrix[1][0] = inter_covar_matrix[1][0] + (matrix_T[1][2]*matrix[2][0]) 
 99                                 + (matrix_T[1][3]*matrix[3][0]);
100      inter_covar_matrix[1][0] = inter_covar_matrix[1][0] + (matrix_T[1][4]*matrix[4][0])
101                                 + (matrix_T[1][5]*matrix[5][0]);
102      inter_covar_matrix[1][0] = inter_covar_matrix[1][0] + (matrix_T[1][6]*matrix[6][0]) 
103                                 + (matrix_T[1][7]*matrix[7][0]);
104 
105      inter_covar_matrix[1][1] = (matrix_T[1][0]*matrix[0][1]) + (matrix_T[1][1]*matrix[1][1]);
106      inter_covar_matrix[1][1] = inter_covar_matrix[1][1]+ (matrix_T[1][2]*matrix[2][1]) 
107                                 + (matrix_T[1][3]*matrix[3][1]);     inter_covar_matrix[1][1] = inter_covar_matrix[1][1] + (matrix_T[1][4]*matrix[4][1]) 
108                                 + (matrix_T[1][5]*matrix[5][1]);
109      inter_covar_matrix[1][1] = inter_covar_matrix[1][1] + (matrix_T[1][6]*matrix[6][1])
110                                 + (matrix_T[1][7]*matrix[7][1]);
111 
112     *a = inter_covar_matrix[0][0];
113     *b = inter_covar_matrix[0][1];
114     *c = inter_covar_matrix[1][0];
115     *d = inter_covar_matrix[1][1];
116 
117      det = ((*a) * (*d)) - ((*c) * (*b));
118 
119      inter_covar_matrix[0][0] =  (*d) / det;
120      inter_covar_matrix[0][1] = -((*b) / det);
121      inter_covar_matrix[1][0] = -((*c) / det);
122      inter_covar_matrix[1][1] =  (*a) / det;
123 
124     *a = inter_covar_matrix[0][0];
125     *b = inter_covar_matrix[0][1];
126     *c = inter_covar_matrix[1][0];
127     *d = inter_covar_matrix[1][1];
128 }
129 
130 /* ---------- */
131 
132 void partial_derv_covariance(float ax1, float ay1, float ax2, float ay2,
133                              float bx1, float by1, float bx2, float by2,
134                              double *matrix_1, double *matrix_2)
135 {
136      double v, u, derv;
137 
138 /* Compute the values of u and v for differentiation of a quotient */
139 
140      v = (((ay2 - ay1)*(bx2 - bx1)) - ((by2 - by1)*(ax2 - ax1)));
141      u = (((by1*bx2) - (bx1 * by2))*(ax2 - ax1)) - (((ay1*ax2)-(ax1*ay2))*(bx2-bx1));
142 
143 /* Computes the parital derviative with respect to ax1 */
144 
145      derv = (((-(by1 * bx2) + (bx1*by2) + (ay2*bx2) - (ay2*bx1))*v)-(u*(by2-by1)))/(v*v);
146     
147     *matrix_1 = derv;
148      if (fabs(bx2 - bx1)<MINFLOAT ) *matrix_2 = MAXFLOAT;
149      else
150          *matrix_2 = ((by2 - by1) / (bx2 - bx1)) * derv; 
151 }
152 
153 /* ---------- */
154 
155 double  chi_squared_test(Vec2 *int_pt, Point2 *loc, double *a, double *b,
156                          double *c, double *d)
157 {
158      double   delta_x, delta_y, chi_sq;
159 
160      delta_x = vec2_x(loc->p) - vec2_x(*int_pt);
161      delta_y = vec2_y(loc->p) - vec2_y(*int_pt);
162      chi_sq = ((*a) * delta_x * delta_x) + ((*c) * delta_x * delta_y) + 
163               ((*b) * delta_x * delta_y) + ((*d) * delta_y * delta_y);
164 
165 return(chi_sq);
166 }
167 
168 /* ---------- */
169 
170 
171 

~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

This page was automatically generated by the LXR engine.
Visit the LXR main site for more information.