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

Linux Cross Reference
Tina6/tina-libs/tina/geometry/geomPlane_lsq.c

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

  1 /**********
  2  * 
  3  * This file is part of the TINA Open Source Image Analysis Environment
  4  * henceforth known as TINA
  5  *
  6  * TINA is free software; you can redistribute it and/or modify
  7  * it under the terms of the GNU General Public License as 
  8  * published by the Free Software Foundation.
  9  *
 10  * TINA is distributed in the hope that it will be useful,
 11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 13  * GNU General Public License for more details.
 14  *
 15  * You should have received a copy of the GNU General Public License
 16  * along with TINA; if not, write to the Free Software Foundation, Inc., 
 17  * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 18  *
 19  * ANY users of TINA who require exemption from the existing licence must
 20  * negotiate a new licence with Dr. Neil.A.Thacker, the sole agent for
 21  * the University of Manchester.
 22  *
 23  **********
 24  * 
 25  * Program :    TINA
 26  * File    :  $Source: /home/tina/cvs/tina-libs/tina/geometry/geomPlane_lsq.c,v $
 27  * Date    :  $Date: 2002/12/09 11:51:23 $
 28  * Version :  $Revision: 1.1.1.1 $
 29  * CVS Id  :  $Id: geomPlane_lsq.c,v 1.1.1.1 2002/12/09 11:51:23 cvstina Exp $
 30  *
 31  * Notes : Find least square fit of plane through list of assorted 3D geometry 
 32  *
 33  *********
 34 */
 35 
 36 
 37 #include "geomPlane_lsq.h"
 38 
 39 #if HAVE_CONFIG_H
 40   #include <config.h>
 41 #endif
 42 
 43 #include <stdio.h>
 44 #include <math.h>
 45 #include <tina/sys/sysDef.h>
 46 #include <tina/sys/sysPro.h>
 47 #include <tina/math/mathDef.h>
 48 #include <tina/math/mathPro.h>
 49 #include <tina/geometry/geomDef.h>
 50 #include <tina/geometry/geom_PlaneDef.h>
 51 #include <tina/geometry/geomPlane_gen.h>
 52 
 53 
 54 
 55 static void plane_fit_add_point(Matrix * A, Vector * b, int i, Vec3 p, double w)
 56 /* Ax = b */
 57 /* row of matrix/vector */
 58 /* data */
 59 /* weight */
 60 {
 61     A->el.double_v[i][0] = vec3_x(p) * w;
 62     A->el.double_v[i][1] = vec3_y(p) * w;
 63     A->el.double_v[i][2] = w;
 64     VECTOR_DOUBLE(b, i) = vec3_z(p) * w;
 65 }
 66 
 67 Plane  *plane_lsq(List * geom3, float *resid)
 68 /* assorted 3D data (Vec3, Point3, Line3) */
 69 
 70 {
 71     int     n, i;
 72     Matrix *A;
 73     Vector *x;
 74     Vector *b;
 75     Plane  *plane = NULL;
 76     List   *ptr;
 77 
 78     if (geom3 == NULL)
 79         return (NULL);
 80 
 81     for (n = 0, ptr = geom3; ptr != NULL; ptr = ptr->next)
 82     {
 83         switch (ptr->type)
 84         {
 85         case VEC3:
 86         case POINT3:
 87             n++;
 88             break;
 89         case LINE3:
 90             n += 2;
 91             break;
 92         default:
 93             break;
 94         }
 95     }
 96 
 97     A = matrix_alloc(n, 3, matrix_full, double_v);
 98     b = vector_alloc(n, double_v);
 99 
100     for (i = 0, ptr = geom3; ptr != NULL; ptr = ptr->next)
101     {
102         switch (ptr->type)
103         {
104         case VEC3:
105             plane_fit_add_point(A, b, i++, *(Vec3 *) ptr->to, 1.0);
106             break;
107         case POINT3:
108             plane_fit_add_point(A, b, i++, ((Point3 *) ptr->to)->p, 1.0);
109             break;
110         case LINE3:
111             plane_fit_add_point(A, b, i++, ((Line3 *) ptr->to)->p1, 1.0);
112             plane_fit_add_point(A, b, i++, ((Line3 *) ptr->to)->p2, 1.0);
113             break;
114         default:
115             break;
116         }
117     }
118 
119     x = matrix_cholesky_least_square(A, b);
120     if (x != NULL)
121     {
122         double  a = VECTOR_DOUBLE(x, 0);
123         double  b = VECTOR_DOUBLE(x, 1);
124         double  c = VECTOR_DOUBLE(x, 2);
125         Vec3    n = {Vec3_id};
126         Vec3    p = {Vec3_id};
127 
128         n = vec3_unit(vec3(a, b, -1.0));
129         p = vec3(0.0, 0.0, c);
130         plane = plane_make(p, n, THREEDIM);
131     }
132     matrix_free(A);
133     vector_free(b);
134     vector_free(x);
135 
136     *resid = (float)0.0;
137 
138     return (plane);
139 }
140 
141 Vec3    geom3_centroid(List * geom3)
142 /* assorted 3D data (Vec3, Point3, Line3) */
143 {
144     int     i;
145     List   *ptr;
146     Vec3    c = {Vec3_id};
147 
148     c = vec3(0.0, 0.0, 0.0);
149     for (i = 0, ptr = geom3; ptr != NULL; ptr = ptr->next)
150     {
151         switch (ptr->type)
152         {
153         case VEC3:
154             c = vec3_sum(c, *(Vec3 *) ptr->to);
155             ++i;
156             break;
157         case POINT3:
158             c = vec3_sum(c, ((Point3 *) ptr->to)->p);
159             ++i;
160             break;
161         case LINE3:
162             c = vec3_sum(c, ((Line3 *) ptr->to)->p1);
163             c = vec3_sum(c, ((Line3 *) ptr->to)->p2);
164             i += 2;
165             break;
166         default:
167             break;
168         }
169     }
170     return ((i == 0) ? c : vec3_times(1.0 / i, c));
171 }
172 

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