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

Linux Cross Reference
Tina6/tina-libs/tina/vision/visCalib_stereo.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_stereo.c,v $
 27  * Date    :  $Date: 2004/02/13 12:30:33 $
 28  * Version :  $Revision: 1.4 $
 29  * CVS Id  :  $Id: visCalib_stereo.c,v 1.4 2004/02/13 12:30:33 neil Exp $
 30  *
 31  * Author  : Legacy TINA
 32  *
 33  * Notes :
 34  *
 35  *********
 36 */
 37 
 38 #include "visCalib_stereo.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/geometry/geomPro.h>
 53 #include <tina/vision/vis_CalibPro.h>
 54 
 55 static double scale_init = 0.04, c_test1 = 0.00001, c_test2 = 0.1;
 56 static double accuracy = 1.0;
 57 
 58 void    cam_cal_stereo_smplx_params_set(double s_init, double c1, double c2, double a)
 59 {
 60     scale_init = s_init;
 61     c_test1 = c1;
 62     c_test2 = c2;
 63     accuracy = a;
 64 }
 65 
 66 static void format_params(char *string, double *a, int n)
 67 {
 68     int     i;
 69 
 70     format(string);
 71     for (i = 0; i < n; i++)
 72         format("%f ", a[i]);
 73     format("\n");
 74 }
 75 
 76 static Camera *cal_caml;
 77 static Camera *cal_camr;
 78 static int cal_mask;
 79 static List *cal_data;
 80 static Vec3 *(*cal_get_3d) ();
 81 static Vec2 *(*cal_get_pixl) ();
 82 static Vec2 *(*cal_get_pixr) ();
 83 static Covar *cal_in_cov;
 84 
 85 static double pixchisq(int n_par, double *a)
 86 {
 87     double  chisq = DBL_MAX;
 88     double *f = NULL;
 89     int     n;
 90 
 91     (void) store_camera_int(cal_mask, a + 6, cal_caml);
 92     (void) store_camera_int(cal_mask, a + n_par / 2 + 6, cal_camr);
 93     if (store_camera_ext(a, cal_caml) &&
 94         store_camera_rel(a + n_par / 2, cal_caml, cal_camr))
 95     {
 96         n = INT_MAX;
 97         chisq = camerror(&n, f, cal_caml, cal_data, cal_get_pixl, cal_get_3d, accuracy);
 98         n = INT_MAX;
 99         chisq += camerror(&n, f, cal_camr, cal_data, cal_get_pixr, cal_get_3d, accuracy);
100         chisq += stereo_reg(cal_in_cov, cal_mask, a + 6);
101     }
102     return (chisq);
103 }
104 
105 /* inverse covarience */
106 double  cam_cal_stereo_simplex(Camera * caml, Camera * camr, int mask, List * data, Vec3 * (*get3d) ( /* ??? */ ), Vec2 * (*getpixl) ( /* ??? */
107 ), Vec2 * (*getpixr) ( /* ??? */ ), Covar * in_cov)
108 {
109     double  chisq, chisq_old;
110     double *a;
111     int     n_par, n_parm2p12, 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_3d = get3d;
121     cal_get_pixl = getpixl;
122     cal_get_pixr = getpixr;
123     cal_in_cov = in_cov;
124 
125     for (i = 0, n_par = 0; i < 16; i++)
126         if (mask & (1 << i))
127             n_par++;
128     n_parm2p12 = 2 * n_par + 12;
129     a = (double *) ralloc((unsigned) n_parm2p12 * sizeof(double));
130 
131     (void) conv_camera_int(mask, caml, a + 6);
132     (void) conv_camera_ext(caml, a);
133     (void) conv_camera_int(mask, camr, a + n_par + 12);
134     (void) conv_camera_rel(caml, camr, a + n_par + 6);
135 
136     format_params("Initial camera parameters = ", a, n_parm2p12);
137 
138     chisq_old = pixchisq(n_parm2p12, a);
139     for (i = 0; i < 5; ++i)
140     {
141         chisq = simplexmin(n_parm2p12, a, scale_init, pixchisq, NULL, c_test1,
142                            (void (*) ()) format);
143         if (chisq_old - chisq < c_test2)
144             break;
145         chisq_old = chisq;
146     }
147 
148     format_params("Final camera parameters = ", a, n_parm2p12);
149 
150     (void) store_camera_int(mask, a + 6, caml);
151     (void) store_camera_ext(a, caml);
152     (void) store_camera_int(mask, a + n_par + 12, camr);
153     (void) store_camera_rel(a + n_par + 6, caml, camr);
154     rfree((void *) a);
155     return (chisq);
156 }
157 
158 static double stereo_chisq(double *f, int n_data, double *a, int n_par)
159 {
160     double  chisq = DBL_MAX;
161     double *g = NULL;
162     int     n = n_data;
163 
164     (void) store_camera_int(cal_mask, a + 6, cal_caml);
165     (void) store_camera_int(cal_mask, a + n_par / 2 + 6, cal_camr);
166     if (store_camera_ext(a, cal_caml) &&
167         store_camera_rel(a + n_par / 2, cal_caml, cal_camr))
168     {
169         chisq = camerror(&n, f, cal_caml, cal_data, cal_get_pixl, cal_get_3d, accuracy);
170         if (f)
171             g = &f[n];
172         chisq += camerror(&n_data, g, cal_camr, cal_data, cal_get_pixr, cal_get_3d, accuracy);
173     }
174     return (chisq);
175 }
176 
177 Covar  *cal_full_stereo_covar(Camera * caml, Camera * camr, int mask, List * data, Vec3 * (*get3d) ( /* ??? */ ), Vec2 * (*getpixl) ( /* ??? */
178 ), Vec2 * (*getpixr) ( /* ??? */ ))
179 {
180     Covar  *cov;
181     Covar  *incov;
182     double *a;
183     int     n_data1 = INT_MAX, n_data2 = INT_MAX, n_data;
184     int     n_par, i;
185     double  condition = 1.0e-9;
186 
187     if (data == NULL || caml == NULL || camr == NULL)
188         return (NULL);
189 
190     cal_mask = mask;
191     cal_data = data;
192     cal_caml = caml;
193     cal_camr = camr;
194     cal_get_3d = get3d;
195     cal_get_pixl = getpixl;
196     cal_get_pixr = getpixr;
197 
198     for (i = 0, n_par = 6; i < 16; i++)
199         if (mask & (1 << i))
200             n_par++;
201     a = (double *) ralloc((unsigned) 2 * n_par * sizeof(double));
202 
203     (void) conv_camera_int(mask, caml, a + 6);
204     (void) conv_camera_int(mask, camr, a + n_par + 6);
205     (void) conv_camera_ext(caml, a);
206     (void) conv_camera_rel(caml, camr, a + n_par);
207 
208     /* .  BUGFIX 1 camerror takes 7 args,  added 1.0 BUGFIX 2 chisq
209      * unused */
210     /* BUGFIX 3 --- BUGFIX 2 is incorrect. 
211      *   Yes, chisq is unused.
212      *   However, camerror updates n_data1 & n_data2
213      *   The code has been uncommented and enclosed in a local group
214      *   SDHC 08/12/93
215      */
216     {
217             double  *f = NULL; double  chisq; chisq = camerror(&n_data1, f,
218               caml, data, getpixl, get3d, 1.0);
219             chisq += camerror(&n_data2, f, camr, data, getpixr, get3d, 1.0);
220     }
221     n_data = n_data1 + n_data2;
222 
223     cov = covar(2 * n_par, a, stereo_chisq, n_data, condition);
224     cov->label = mask;
225     incov = invcov(2 * n_par, a, stereo_chisq, n_data);
226     dmatrix_checkinv((void (*) ()) format, cov->mat, incov->mat, "covariance matrix");
227 
228     rfree((void *) a);
229     return (cov);
230 }
231 
232 Covar  *cal_full_stereo_invcovar(int mask, Covar * cov, double condition)
233 {
234     int     n_par, i, j, n;
235     Covar  *stereo_incov;
236     Matrix *subcov;
237 
238     if (cov == NULL)
239         return (NULL);
240 
241     for (i = 0, n_par = 6; i < 16; i++)
242         if (mask & (1 << i))
243             n_par++;
244     n = n_par * 2 - 6;
245 
246     stereo_incov = covar_alloc(n);
247     subcov = matrix_alloc(n, n, matrix_full, double_v);
248     stereo_incov->n = n;
249     stereo_incov->label = mask;
250     stereo_incov->vec = vector_alloc(n, double_v);
251     for (i = 0; i < n; i++)
252     {
253         VECTOR_SET(stereo_incov->vec, i, VECTOR_DOUBLE(cov->vec, i + 6));
254         for (j = 0; j < n; j++)
255             subcov->el.double_v[i][j] = cov->mat->el.double_v[i + 6][j + 6];
256     }
257 
258     stereo_incov->mat = matrix_invsvd(subcov, condition);
259     dmatrix_checkinv((void (*) ()) format, stereo_incov->mat, subcov, "sub covariance inverse");
260     matrix_free(subcov);
261 
262     return (stereo_incov);
263 }
264 

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