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

Linux Cross Reference
Tina6/tina-libs/tina/vision/visCalib_epi.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/vision/visCalib_epi.c,v $
 27  * Date    :  $Date: 2005/01/09 17:49:25 $
 28  * Version :  $Revision: 1.5 $
 29  * CVS Id  :  $Id: visCalib_epi.c,v 1.5 2005/01/09 17:49:25 paul Exp $
 30  *
 31  * Author  : Legacy TINA
 32  *
 33  * Notes :
 34  *
 35  *********
 36 */
 37 
 38 #include "visCalib_epi.h"
 39 
 40 #if HAVE_CONFIG_H
 41   #include <config.h>
 42 #endif
 43 
 44 #include <math.h>
 45 #include <float.h>
 46 #include <limits.h>
 47 #include <tina/sys/sysDef.h>
 48 #include <tina/sys/sysPro.h>
 49 #include <tina/math/mathDef.h>
 50 #include <tina/math/mathPro.h>
 51 #include <tina/geometry/geomDef.h>
 52 #include <tina/vision/vis_CalibPro.h>
 53 
 54 static double scale_init = 0.04, c_test1 = 0.00001, c_test2 = 0.1;
 55 static double accuracy = 1.0;
 56 
 57 void    cam_cal_triv_smplx_params_set(double s_init, double c1, double c2, double a)
 58 {
 59     scale_init = s_init;
 60     c_test1 = c1;
 61     c_test2 = c2;
 62     accuracy = a;
 63 }
 64 
 65 static void format_params(char *string, double *a, int n)
 66 {
 67     int     i;
 68 
 69     format(string);
 70     for (i = 0; i < n; i++)
 71         format("%f ", a[i]);
 72     format("\n");
 73 }
 74 
 75 static Camera *cal_caml;
 76 static Camera *cal_camr;
 77 static int cal_mask;
 78 static List *cal_data;
 79 static Vec2 *(*cal_get_pixl) ();
 80 static Vec2 *(*cal_get_pixr) ();
 81 static Covar *cal_in_cov;
 82 
 83 static double pixchisq(int n_par, double *a)
 84 {
 85     double  chisq = DBL_MAX;
 86     double *f = NULL;
 87 
 88     (void) store_camera_int(cal_mask, a, cal_caml);
 89     (void) store_camera_int(cal_mask, a + n_par / 2 + 3, cal_camr);
 90     if (store_camera_rel(a + n_par / 2 - 3, cal_caml, cal_camr))
 91     {
 92         int     n = INT_MAX;
 93 
 94         chisq = triv_camerror(&n, f, cal_caml, cal_camr, cal_data, cal_get_pixl, cal_get_pixr, accuracy);
 95         chisq += stereo_reg(cal_in_cov, cal_mask, a);
 96     }
 97     return (chisq);
 98 }
 99 
100 double  cam_cal_triv_simplex(Camera * caml, Camera * camr, int mask, List * data, Vec2 * (*getpixl) ( /* ??? */ ), Vec2 * (*getpixr) ( /* ??? */
101 ), Covar * in_cov)
102 
103 
104 
105 
106 
107 /* inverse covarience */
108 {
109     double  chisq, chisq_old;
110     double *a;
111     int     n_par, n_parm2p6, i;
112 
113     if (data == NULL || caml == NULL || camr == NULL)
114         return (0.0);
115 
116     cal_mask = mask;
117     cal_data = data;
118     cal_caml = caml;
119     cal_camr = camr;
120     cal_get_pixl = getpixl;
121     cal_get_pixr = getpixr;
122     cal_in_cov = in_cov;
123 
124     for (i = 0, n_par = 0; i < 16; i++)
125         if (mask & (1 << i))
126             n_par++;
127     n_parm2p6 = 2 * n_par + 6;
128     a = (double *) ralloc((unsigned) n_parm2p6 * sizeof(double));
129 
130     (void) conv_camera_int(mask, caml, a);
131     (void) conv_camera_int(mask, camr, a + n_par + 6);
132     (void) conv_camera_rel(caml, camr, a + n_par);
133 
134     format_params("Initial camera parameters = ", a, n_parm2p6);
135 
136     chisq_old = pixchisq(n_parm2p6, a);
137     for (i = 0; i < 5; ++i)
138     {
139         chisq = simplexmin(n_parm2p6, a, scale_init, pixchisq, NULL, c_test1,
140                            (void (*) ()) format);
141         if (chisq_old - chisq < c_test2)
142             break;
143         chisq_old = chisq;
144     }
145 
146     format_params("Final camera parameters = ", a, n_parm2p6);
147 
148     (void) store_camera_int(mask, a, caml);
149     (void) store_camera_int(mask, a + n_par + 6, camr);
150     (void) store_camera_rel(a + n_par, caml, camr);
151     rfree((void *) a);
152     return (chisq);
153 }
154 
155 static double trivedi_chisq(double *f, int n, double *a, int n_par)
156 {
157     double  chisq = DBL_MAX;
158 
159     (void) store_camera_int(cal_mask, a, cal_caml);
160     (void) store_camera_int(cal_mask, a + n_par / 2 + 3, cal_camr);
161     if (store_camera_rel(a + n_par / 2 - 3, cal_caml, cal_camr))
162         chisq = triv_camerror(&n, f, cal_caml, cal_camr, cal_data, cal_get_pixl,
163                               cal_get_pixr, accuracy);
164     return (chisq);
165 }
166 
167 Covar  *cal_trivedi_invcovar(Camera * caml, Camera * camr, int mask, List * data, Vec2 * (*getpixl) ( /* ??? */ ), Vec2 * (*getpixr) ( /* ??? */
168 ))
169 {
170     Covar  *incov;
171     double *a;
172     int     n_data = INT_MAX;
173     int     n_par, i;
174     double  *f = NULL;
175     double  chisq;
176 
177     if (data == NULL || caml == NULL || camr == NULL)
178         return (NULL);
179 
180     cal_mask = mask;
181     cal_data = data;
182     cal_caml = caml;
183     cal_camr = camr;
184     cal_get_pixl = getpixl;
185     cal_get_pixr = getpixr;
186 
187     for (i = 0, n_par = 0; i < 16; i++)
188         if (mask & (1 << i))
189             n_par++;
190     a = (double *) ralloc((unsigned) ((2 * n_par + 6) * sizeof(double)));
191 
192     (void) conv_camera_int(mask, caml, a);
193     (void) conv_camera_int(mask, camr, a + n_par + 6);
194     (void) conv_camera_rel(caml, camr, a + n_par);
195 
196     chisq = triv_camerror(&n_data, f, caml, camr, data, getpixl,
197                           getpixr, accuracy);
198     incov = invcov(2 * n_par + 6, a, trivedi_chisq, n_data);
199     incov->label = mask;
200 
201     rfree((void *) a);
202     return (incov);
203 }
204 
205 Covar *init_cal_invcovar(Camera *caml,Camera *camr,int mask)
206 {
207     Covar *incov;
208     double *a;
209     int n_par,m_par,i,j,k;
210 
211     if (caml==NULL || camr==NULL)
212         return(NULL);
213 
214     cal_mask     = mask;
215     cal_caml     = caml;
216     cal_camr     = camr;
217 
218     for (i=0,n_par=0;i<16;i++) if (mask&(1<<i)) n_par++;
219     m_par = 2*n_par+6;
220     a = (double *)ralloc((m_par)*sizeof(double));
221 
222     conv_camera_int(mask, caml, a);
223     conv_camera_int(mask, camr, a+n_par+6);
224     conv_camera_rel(caml, camr, a+n_par);
225 
226     incov = covar_alloc(m_par);
227     incov->mat = matrix_alloc(m_par, m_par, matrix_full, double_v);
228     incov->vec = vector_alloc(m_par, double_v);
229     incov->label = mask;
230     for (k = 0; k < m_par; k++)
231     {
232         Vector *shitv;
233         shitv = incov->vec;
234         VECTOR_DOUBLE(shitv,k) = a[k];
235 
236         for (j = 0; j < m_par; j++)
237         {
238             Matrix *shitm;
239             shitm = incov->mat;
240             if (j==k) shitm->el.double_v[j][k] = 1.0;
241             else shitm->el.double_v[j][k]=0.0;
242         }
243     }
244     rfree(a);
245     return(incov);
246 }
247 
248 

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