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

Linux Cross Reference
Tina4/src/vision/transform/rotation.c

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

  1 /**@(#)
  2 **/
  3 #include <math.h>
  4 #include <tina/sys.h>
  5 #include <tina/sysfuncs.h>
  6 #include <tina/math.h>
  7 #include <tina/mathfuncs.h>
  8 #include <tina/vision.h>
  9 
 10 static int rot_matrix_get(Matrix * M, Vec3 v1, Vec3 v2, double w);
 11 static Mat3 rot_compute(Matrix * M, int *rot_error);
 12 
 13 /* compute rotation from pure vector information */
 14 Mat3    mlist_comp_rot_vec3(List * match_list, int *rot_error)
 15 {
 16     Matrix *A = matrix_alloc(4, 4, matrix_full, float_v);
 17     Matrix *B = matrix_alloc(4, 4, matrix_full, float_v);
 18     Mat3    rot = {Mat3_id};
 19     List   *lptr;
 20 
 21     for (lptr = match_list; lptr != NULL; lptr = lptr->next)
 22     {
 23         Match  *mptr = (Match *) lptr->to;
 24         Vec3    v1 = {Vec3_id};
 25         Vec3    v2 = {Vec3_id};
 26 
 27         switch (mptr->type)
 28         {
 29         default:
 30             continue;           /* to next match */
 31         case LINE3:
 32             v1 = ((Line3 *) mptr->to1)->v;
 33             v2 = ((Line3 *) mptr->to2)->v;
 34             break;
 35         case CONIC3:
 36             v1 = ((Conic3 *) mptr->to1)->ez;
 37             v2 = ((Conic3 *) mptr->to2)->ez;
 38             break;
 39         case PLANE:
 40             v1 = ((Plane *) mptr->to1)->n;
 41             v2 = ((Plane *) mptr->to2)->n;
 42             break;
 43         }
 44 
 45         if (rot_matrix_get(A, v1, v2, mptr->weight))
 46             (void) fmatrix_add_inplace(B, A);
 47     }
 48 
 49     rot = rot_compute(B, rot_error);
 50     matrix_free(A);
 51     matrix_free(B);
 52 
 53     return (rot);
 54 }
 55 
 56 /* compute rotation from 3D point position information */
 57 Mat3    mlist_comp_rot_pos3(List * match_list, Vec3 c1, Vec3 c2, int *rot_error)
 58 
 59 /* centroids */
 60 
 61 {
 62     Matrix *A = matrix_alloc(4, 4, matrix_full, float_v);
 63     Matrix *B = matrix_alloc(4, 4, matrix_full, float_v);
 64     Mat3    rot = {Mat3_id};
 65     List   *lptr;
 66 
 67     for (lptr = match_list; lptr != NULL; lptr = lptr->next)
 68     {
 69         Match  *mptr = (Match *) lptr->to;
 70         Vec3    v1 = {Vec3_id};
 71         Vec3    v2 = {Vec3_id};
 72         Point3 *p1;
 73         Point3 *p2;
 74 
 75         if (mptr->type != POINT3)
 76             continue;
 77 
 78         p1 = (Point3 *) mptr->to1;
 79         p2 = (Point3 *) mptr->to2;
 80 
 81         v1 = vec3_unit(vec3_diff(p1->p, c1));
 82         v2 = vec3_unit(vec3_diff(p2->p, c2));
 83 
 84         if (rot_matrix_get(A, v1, v2, mptr->weight))
 85             (void) fmatrix_add_inplace(B, A);
 86     }
 87 
 88     rot = rot_compute(B, rot_error);
 89     matrix_free(A);
 90     matrix_free(B);
 91 
 92     return (rot);
 93 }
 94 
 95 /* compute rotation from all information */
 96 Mat3    mlist_comp_rot_all(List * match_list, int *rot_error)
 97 {
 98     Matrix *A = matrix_alloc(4, 4, matrix_full, float_v);
 99     Matrix *B = matrix_alloc(4, 4, matrix_full, float_v);
100     Mat3    rot = {Mat3_id};
101     List   *lptr1;
102     List   *lptr2;
103 
104     for (lptr1 = match_list; lptr1 != NULL; lptr1 = lptr1->next)
105     {
106         Match  *mptr1 = (Match *) lptr1->to;
107         Vec3    v1 = {Vec3_id};
108         Vec3    v2 = {Vec3_id};
109         Vec3    p1 = {Vec3_id};
110         Vec3    p2 = {Vec3_id};
111 
112         switch (mptr1->type)
113         {
114         default:
115             continue;
116         case POINT3:
117             break;
118         case LINE3:
119             v1 = ((Line3 *) mptr1->to1)->v;
120             v2 = ((Line3 *) mptr1->to2)->v;
121             break;
122         case CONIC3:
123             v1 = ((Conic3 *) mptr1->to1)->ez;
124             v2 = ((Conic3 *) mptr1->to2)->ez;
125             break;
126         case PLANE:
127             v1 = ((Plane *) mptr1->to1)->n;
128             v2 = ((Plane *) mptr1->to2)->n;
129             break;
130         }
131 
132         if (mptr1->type != POINT3 && rot_matrix_get(A, v1, v2, mptr1->weight))
133             (void) fmatrix_add_inplace(B, A);
134 
135         /* consider all pair-wise vectors */
136 
137         switch (mptr1->type)
138         {
139         default:
140             continue;
141         case POINT3:
142             p1 = ((Point3 *) mptr1->to1)->p;
143             p2 = ((Point3 *) mptr1->to2)->p;
144             break;
145         case CONIC3:
146             p1 = ((Conic3 *) mptr1->to1)->origin;
147             p2 = ((Conic3 *) mptr1->to2)->origin;
148             break;
149         }
150 
151         for (lptr2 = lptr1->next; lptr2 != NULL; lptr2 = lptr2->next)
152         {
153             Match  *mptr2 = (Match *) lptr2->to;
154 
155             switch (mptr2->type)
156             {
157             default:
158                 continue;
159             case POINT3:
160                 v1 = vec3_diff(p1, ((Point3 *) mptr2->to1)->p);
161                 v2 = vec3_diff(p2, ((Point3 *) mptr2->to2)->p);
162                 break;
163             case CONIC3:
164                 v1 = vec3_diff(p1, ((Conic3 *) mptr2->to1)->origin);
165                 v2 = vec3_diff(p2, ((Conic3 *) mptr2->to2)->origin);
166                 break;
167             }
168             v1 = vec3_unit(v1);
169             v2 = vec3_unit(v2);
170             if (rot_matrix_get(A, v1, v2, (mptr1->weight + mptr2->weight) * 0.5))
171                 (void) fmatrix_add_inplace(B, A);
172         }
173     }
174 
175     rot = rot_compute(B, rot_error);
176     matrix_free(A);
177     matrix_free(B);
178 
179     return (rot);
180 }
181 
182 static Mat3 rot_compute(Matrix * M, int *rot_error)
183 {
184     Matrix *evec;
185     Vector *eval;
186     Vector *quat;
187     double  theta, sin_theta;
188     float  *el;
189     Vec3    axis = {Vec3_id};
190 
191     if (M == NULL || M->m != 4 || M->n != 4)
192     {
193         *rot_error = 1;
194         return (mat3_unit());
195     }
196     evec = matrix_alloc(4, 4, matrix_full, float_v);
197     eval = vector_alloc(4, float_v);
198 
199     if (!matrix_eigen_sym(M, eval, evec))
200     {
201         matrix_free(evec);
202         vector_free(eval);
203         *rot_error = 1;
204         return (mat3_unit());
205     }
206     *rot_error = 0;
207     quat = matrix_col_vector(evec, 0);
208     el = quat->data;
209     if (el[0] < 0)
210         vector_minus_inplace(quat);
211 
212     theta = tina_acos(el[0]);
213     sin_theta = sin(theta);
214     if (fabs(sin_theta) < 0.000000001)
215         axis = vec3_ex();
216     else
217         axis = vec3_unit(vec3(el[1] / sin_theta, el[2] / sin_theta, el[3] / sin_theta));
218     matrix_free(evec);
219     vector_free(eval);
220     vector_free(quat);
221     return (rot3(theta * 2, axis));
222 }
223 
224 static int rot_matrix_get(Matrix * M, Vec3 v1, Vec3 v2, double w)
225 /* of type float */
226 
227 /* weight factor */
228 {
229     double  xY, yX, xZ, zX, yZ, zY;
230     double  xpXs, ypYs, zpZs;
231     double  xmXs, ymYs, zmZs;
232     double  w2 = 2 * w;
233     float **el;
234 
235     if (M->vtype != float_v || M->shape != matrix_full)
236         return (0);
237 
238     if (M->m != 4 || M->n != 4)
239         return (0);
240 
241     el = M->el.float_v;
242 
243     xpXs = sqr(vec3_x(v1) + vec3_x(v2));
244     ypYs = sqr(vec3_y(v1) + vec3_y(v2));
245     zpZs = sqr(vec3_z(v1) + vec3_z(v2));
246     xmXs = sqr(vec3_x(v1) - vec3_x(v2));
247     ymYs = sqr(vec3_y(v1) - vec3_y(v2));
248     zmZs = sqr(vec3_z(v1) - vec3_z(v2));
249 
250     xY = vec3_x(v1) * vec3_y(v2);
251     yX = vec3_y(v1) * vec3_x(v2);
252     xZ = vec3_x(v1) * vec3_z(v2);
253     zX = vec3_z(v1) * vec3_x(v2);
254     yZ = vec3_y(v1) * vec3_z(v2);
255     zY = vec3_z(v1) * vec3_y(v2);
256 
257     el[0][0] = (float)((xmXs + ymYs + zmZs) * w);
258     el[1][1] = (float)((xmXs + ypYs + zpZs) * w);
259     el[2][2] = (float)((xpXs + ymYs + zpZs) * w);
260     el[3][3] = (float)((xpXs + ypYs + zmZs) * w);
261     el[0][1] = el[1][0] = (float)(w2 * (zY - yZ));
262     el[0][2] = el[2][0] = (float)(w2 * (xZ - zX));
263     el[0][3] = el[3][0] = (float)(w2 * (yX - xY));
264     el[1][2] = el[2][1] = (float)(-w2 * (xY + yX));
265     el[1][3] = el[3][1] = (float)(-w2 * (xZ + zX));
266     el[2][3] = el[3][2] = (float)(-w2 * (yZ + zY));
267     return (1);
268 }
269 

~ [ 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.