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

Linux Cross Reference
Tina5/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 Lesser 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 Lesser General Public License for more details.
 14  *
 15  * You should have received a copy of the GNU Lesser 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  **********
 20  * 
 21  * Program :    TINA
 22  * File    :  $Source: /home/tina/cvs/tina-libs/tina/geometry/geomPlane_lsq.c,v $
 23  * Date    :  $Date: 2002/12/09 11:51:23 $
 24  * Version :  $Revision: 1.1.1.1 $
 25  * CVS Id  :  $Id: geomPlane_lsq.c,v 1.1.1.1 2002/12/09 11:51:23 cvstina Exp $
 26  *
 27  * Notes : Find least square fit of plane through list of assorted 3D geometry 
 28  *
 29  *********
 30 */
 31 
 32 
 33 #include "geomPlane_lsq.h"
 34 
 35 #if HAVE_CONFIG_H
 36   #include <config.h>
 37 #endif
 38 
 39 #include <stdio.h>
 40 #include <math.h>
 41 #include <tina/sys/sysDef.h>
 42 #include <tina/sys/sysPro.h>
 43 #include <tina/math/mathDef.h>
 44 #include <tina/math/mathPro.h>
 45 #include <tina/geometry/geomDef.h>
 46 #include <tina/geometry/geom_PlaneDef.h>
 47 #include <tina/geometry/geomPlane_gen.h>
 48 
 49 
 50 
 51 static void plane_fit_add_point(Matrix * A, Vector * b, int i, Vec3 p, double w)
 52 /* Ax = b */
 53 /* row of matrix/vector */
 54 /* data */
 55 /* weight */
 56 {
 57     A->el.double_v[i][0] = vec3_x(p) * w;
 58     A->el.double_v[i][1] = vec3_y(p) * w;
 59     A->el.double_v[i][2] = w;
 60     VECTOR_DOUBLE(b, i) = vec3_z(p) * w;
 61 }
 62 
 63 Plane  *plane_lsq(List * geom3, float *resid)
 64 /* assorted 3D data (Vec3, Point3, Line3) */
 65 
 66 {
 67     int     n, i;
 68     Matrix *A;
 69     Vector *x;
 70     Vector *b;
 71     Plane  *plane = NULL;
 72     List   *ptr;
 73 
 74     if (geom3 == NULL)
 75         return (NULL);
 76 
 77     for (n = 0, ptr = geom3; ptr != NULL; ptr = ptr->next)
 78     {
 79         switch (ptr->type)
 80         {
 81         case VEC3:
 82         case POINT3:
 83             n++;
 84             break;
 85         case LINE3:
 86             n += 2;
 87             break;
 88         default:
 89             break;
 90         }
 91     }
 92 
 93     A = matrix_alloc(n, 3, matrix_full, double_v);
 94     b = vector_alloc(n, double_v);
 95 
 96     for (i = 0, ptr = geom3; ptr != NULL; ptr = ptr->next)
 97     {
 98         switch (ptr->type)
 99         {
100         case VEC3:
101             plane_fit_add_point(A, b, i++, *(Vec3 *) ptr->to, 1.0);
102             break;
103         case POINT3:
104             plane_fit_add_point(A, b, i++, ((Point3 *) ptr->to)->p, 1.0);
105             break;
106         case LINE3:
107             plane_fit_add_point(A, b, i++, ((Line3 *) ptr->to)->p1, 1.0);
108             plane_fit_add_point(A, b, i++, ((Line3 *) ptr->to)->p2, 1.0);
109             break;
110         default:
111             break;
112         }
113     }
114 
115     x = matrix_cholesky_least_square(A, b);
116     if (x != NULL)
117     {
118         double  a = VECTOR_DOUBLE(x, 0);
119         double  b = VECTOR_DOUBLE(x, 1);
120         double  c = VECTOR_DOUBLE(x, 2);
121         Vec3    n = {Vec3_id};
122         Vec3    p = {Vec3_id};
123 
124         n = vec3_unit(vec3(a, b, -1.0));
125         p = vec3(0.0, 0.0, c);
126         plane = plane_make(p, n, THREEDIM);
127     }
128     matrix_free(A);
129     vector_free(b);
130     vector_free(x);
131 
132     *resid = (float)0.0;
133 
134     return (plane);
135 }
136 
137 Vec3    geom3_centroid(List * geom3)
138 /* assorted 3D data (Vec3, Point3, Line3) */
139 {
140     int     i;
141     List   *ptr;
142     Vec3    c = {Vec3_id};
143 
144     c = vec3(0.0, 0.0, 0.0);
145     for (i = 0, ptr = geom3; ptr != NULL; ptr = ptr->next)
146     {
147         switch (ptr->type)
148         {
149         case VEC3:
150             c = vec3_sum(c, *(Vec3 *) ptr->to);
151             ++i;
152             break;
153         case POINT3:
154             c = vec3_sum(c, ((Point3 *) ptr->to)->p);
155             ++i;
156             break;
157         case LINE3:
158             c = vec3_sum(c, ((Line3 *) ptr->to)->p1);
159             c = vec3_sum(c, ((Line3 *) ptr->to)->p2);
160             i += 2;
161             break;
162         default:
163             break;
164         }
165     }
166     return ((i == 0) ? c : vec3_times(1.0 / i, c));
167 }
168 

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