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

Linux Cross Reference
Tina4/src/vision/plane/plane_lsq.c

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

  1 /**@(#)
  2 **/
  3 #include <tina/sys.h>
  4 #include <tina/math.h>
  5 #include <tina/mathfuncs.h>
  6 #include <tina/vision.h>
  7 #include <tina/visionfuncs.h>
  8 
  9 /* find least square fit of plane through list of assorted 3D geometry */
 10 
 11 static void plane_fit_add_point(Matrix * A, Vector * b, int i, Vec3 p, double w)
 12 /* Ax = b */
 13 /* row of matrix/vector */
 14 /* data */
 15 /* weight */
 16 {
 17     A->el.double_v[i][0] = vec3_x(p) * w;
 18     A->el.double_v[i][1] = vec3_y(p) * w;
 19     A->el.double_v[i][2] = w;
 20     VECTOR_DOUBLE(b, i) = vec3_z(p) * w;
 21 }
 22 
 23 Plane  *plane_lsq(List * geom3, float *resid)
 24 /* assorted 3D data (Vec3, Point3, Line3) */
 25 
 26 {
 27     int     n, i;
 28     Matrix *A;
 29     Vector *x;
 30     Vector *b;
 31     Plane  *plane = NULL;
 32     List   *ptr;
 33 
 34     if (geom3 == NULL)
 35         return (NULL);
 36 
 37     for (n = 0, ptr = geom3; ptr != NULL; ptr = ptr->next)
 38     {
 39         switch (ptr->type)
 40         {
 41         case VEC3:
 42         case POINT3:
 43             n++;
 44             break;
 45         case LINE3:
 46             n += 2;
 47             break;
 48         default:
 49             break;
 50         }
 51     }
 52 
 53     A = matrix_alloc(n, 3, matrix_full, double_v);
 54     b = vector_alloc(n, double_v);
 55 
 56     for (i = 0, ptr = geom3; ptr != NULL; ptr = ptr->next)
 57     {
 58         switch (ptr->type)
 59         {
 60         case VEC3:
 61             plane_fit_add_point(A, b, i++, *(Vec3 *) ptr->to, 1.0);
 62             break;
 63         case POINT3:
 64             plane_fit_add_point(A, b, i++, ((Point3 *) ptr->to)->p, 1.0);
 65             break;
 66         case LINE3:
 67             plane_fit_add_point(A, b, i++, ((Line3 *) ptr->to)->p1, 1.0);
 68             plane_fit_add_point(A, b, i++, ((Line3 *) ptr->to)->p2, 1.0);
 69             break;
 70         default:
 71             break;
 72         }
 73     }
 74 
 75     x = matrix_cholesky_least_square(A, b);
 76     if (x != NULL)
 77     {
 78         double  a = VECTOR_DOUBLE(x, 0);
 79         double  b = VECTOR_DOUBLE(x, 1);
 80         double  c = VECTOR_DOUBLE(x, 2);
 81         Vec3    n = {Vec3_id};
 82         Vec3    p = {Vec3_id};
 83 
 84         n = vec3_unit(vec3(a, b, -1.0));
 85         p = vec3(0.0, 0.0, c);
 86         plane = plane_make(p, n, THREEDIM);
 87     }
 88     matrix_free(A);
 89     vector_free(b);
 90     vector_free(x);
 91 
 92     *resid = (float)0.0;
 93 
 94     return (plane);
 95 }
 96 
 97 Vec3    geom3_centroid(List * geom3)
 98 /* assorted 3D data (Vec3, Point3, Line3) */
 99 {
100     int     i;
101     List   *ptr;
102     Vec3    c = {Vec3_id};
103 
104     c = vec3(0.0, 0.0, 0.0);
105     for (i = 0, ptr = geom3; ptr != NULL; ptr = ptr->next)
106     {
107         switch (ptr->type)
108         {
109         case VEC3:
110             c = vec3_sum(c, *(Vec3 *) ptr->to);
111             ++i;
112             break;
113         case POINT3:
114             c = vec3_sum(c, ((Point3 *) ptr->to)->p);
115             ++i;
116             break;
117         case LINE3:
118             c = vec3_sum(c, ((Line3 *) ptr->to)->p1);
119             c = vec3_sum(c, ((Line3 *) ptr->to)->p2);
120             i += 2;
121             break;
122         default:
123             break;
124         }
125     }
126     return ((i == 0) ? c : vec3_times(1.0 / i, c));
127 }
128 

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