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

Linux Cross Reference
Tina4/src/vision/calib/ecal_smplx.c

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

  1 /**@(#)
  2 **/
  3 #ifdef TRANSPUTER
  4 #include <valuesdual.h>
  5 #else
  6 #include <values.h>
  7 #endif
  8 #include <math.h>
  9 #include <tina/sys.h>
 10 #include <tina/sysfuncs.h>
 11 #include <tina/math.h>
 12 #include <tina/mathfuncs.h>
 13 #include <tina/vision.h>
 14 #include <tina/visionfuncs.h>
 15 
 16 static double scale_init = 0.04, c_test1 = 0.00001, c_test2 = 0.1;
 17 static double accuracy = 1.0;
 18 
 19 void    cam_cal_triv_smplx_params_set(double s_init, double c1, double c2, double a)
 20 {
 21     scale_init = s_init;
 22     c_test1 = c1;
 23     c_test2 = c2;
 24     accuracy = a;
 25 }
 26 
 27 static void format_params(char *string, double *a, int n)
 28 {
 29     int     i;
 30 
 31     format(string);
 32     for (i = 0; i < n; i++)
 33         format("%f ", a[i]);
 34     format("\n");
 35 }
 36 
 37 static Camera *cal_caml;
 38 static Camera *cal_camr;
 39 static int cal_mask;
 40 static List *cal_data;
 41 static Vec2 *(*cal_get_pixl) ();
 42 static Vec2 *(*cal_get_pixr) ();
 43 static Covar *cal_in_cov;
 44 
 45 static double pixchisq(int n_par, double *a)
 46 {
 47     double  chisq = MAXDOUBLE;
 48     double *f = NULL;
 49 
 50     (void) store_camera_int(cal_mask, a, cal_caml);
 51     (void) store_camera_int(cal_mask, a + n_par / 2 + 3, cal_camr);
 52     if (store_camera_rel(a + n_par / 2 - 3, cal_caml, cal_camr))
 53     {
 54         int     n = MAXINT;
 55 
 56         chisq = triv_camerror(&n, f, cal_caml, cal_camr, cal_data, cal_get_pixl, cal_get_pixr, accuracy);
 57         chisq += stereo_reg(cal_in_cov, cal_mask, a);
 58     }
 59     return (chisq);
 60 }
 61 
 62 double  cam_cal_triv_simplex(Camera * caml, Camera * camr, int mask, List * data, Vec2 * (*getpixl) ( /* ??? */ ), Vec2 * (*getpixr) ( /* ??? */ ), Covar * in_cov)
 63 
 64 
 65 
 66 
 67 
 68 /* inverse covarience */
 69 {
 70     double  chisq, chisq_old;
 71     double *a;
 72     int     n_par, n_parm2p6, i;
 73 
 74     if (data == NULL || caml == NULL || camr == NULL)
 75         return (0.0);
 76 
 77     cal_mask = mask;
 78     cal_data = data;
 79     cal_caml = caml;
 80     cal_camr = camr;
 81     cal_get_pixl = getpixl;
 82     cal_get_pixr = getpixr;
 83     cal_in_cov = in_cov;
 84 
 85     for (i = 0, n_par = 0; i < 16; i++)
 86         if (mask & (1 << i))
 87             n_par++;
 88     n_parm2p6 = 2 * n_par + 6;
 89     a = (double *) ralloc((unsigned) n_parm2p6 * sizeof(double));
 90 
 91     (void) conv_camera_int(mask, caml, a);
 92     (void) conv_camera_int(mask, camr, a + n_par + 6);
 93     (void) conv_camera_rel(caml, camr, a + n_par);
 94 
 95     format_params("Initial camera parameters = ", a, n_parm2p6);
 96 
 97     chisq_old = pixchisq(n_parm2p6, a);
 98     for (i = 0; i < 5; ++i)
 99     {
100         chisq = simplexmin(n_parm2p6, a, scale_init, pixchisq, c_test1,
101                            (void (*) ()) format);
102         if (chisq_old - chisq < c_test2)
103             break;
104         chisq_old = chisq;
105     }
106 
107     format_params("Final camera parameters = ", a, n_parm2p6);
108 
109     (void) store_camera_int(mask, a, caml);
110     (void) store_camera_int(mask, a + n_par + 6, camr);
111     (void) store_camera_rel(a + n_par, caml, camr);
112     rfree((void *) a);
113     return (chisq);
114 }
115 
116 static double trivedi_chisq(double *f, int n, double *a, int n_par)
117 {
118     double  chisq = MAXDOUBLE;
119 
120     (void) store_camera_int(cal_mask, a, cal_caml);
121     (void) store_camera_int(cal_mask, a + n_par / 2 + 3, cal_camr);
122     if (store_camera_rel(a + n_par / 2 - 3, cal_caml, cal_camr))
123         chisq = triv_camerror(&n, f, cal_caml, cal_camr, cal_data, cal_get_pixl,
124                               cal_get_pixr, accuracy);
125     return (chisq);
126 }
127 
128 Covar  *cal_trivedi_invcovar(Camera * caml, Camera * camr, int mask, List * data, Vec2 * (*getpixl) ( /* ??? */ ), Vec2 * (*getpixr) ( /* ??? */ ))
129 {
130     Covar  *incov;
131     double *a;
132     int     n_data = MAXINT;
133     int     n_par, i;
134     double  *f = NULL;
135     double  chisq;
136 
137     if (data == NULL || caml == NULL || camr == NULL)
138         return (NULL);
139 
140     cal_mask = mask;
141     cal_data = data;
142     cal_caml = caml;
143     cal_camr = camr;
144     cal_get_pixl = getpixl;
145     cal_get_pixr = getpixr;
146 
147     for (i = 0, n_par = 0; i < 16; i++)
148         if (mask & (1 << i))
149             n_par++;
150     a = (double *) ralloc((unsigned) ((2 * n_par + 6) * sizeof(double)));
151 
152     (void) conv_camera_int(mask, caml, a);
153     (void) conv_camera_int(mask, camr, a + n_par + 6);
154     (void) conv_camera_rel(caml, camr, a + n_par);
155 
156     chisq = triv_camerror(&n_data, f, caml, camr, data, getpixl,
157                           getpixr, accuracy);
158     incov = invcov(2 * n_par + 6, a, trivedi_chisq, n_data);
159     incov->label = mask;
160 
161     rfree((void *) a);
162     return (incov);
163 }
164 
165 Covar *init_cal_invcovar(Camera *caml,Camera *camr,int mask)
166 {
167     Covar *incov;
168     double *a, *f=NULL;
169     int n_par,m_par,i,j,k;
170 
171     if (caml==NULL || camr==NULL)
172         return(NULL);
173 
174     cal_mask     = mask;
175     cal_caml     = caml;
176     cal_camr     = camr;
177 
178     for (i=0,n_par=0;i<16;i++) if (mask&(1<<i)) n_par++;
179     m_par = 2*n_par+6;
180     a = (double *)ralloc((m_par)*sizeof(double));
181 
182     conv_camera_int(mask, caml, a);
183     conv_camera_int(mask, camr, a+n_par+6);
184     conv_camera_rel(caml, camr, a+n_par);
185 
186     incov = covar_alloc(m_par);
187     incov->mat = matrix_alloc(m_par, m_par, matrix_full, double_v);
188     incov->vec = vector_alloc(m_par, double_v);
189     incov->label = mask;
190     for (k = 0; k < m_par; k++)
191     {
192         Vector *shitv;
193         shitv = incov->vec;
194         VECTOR_DOUBLE(shitv,k) = a[k];
195 
196         for (j = 0; j < m_par; j++)
197         {
198             Matrix *shitm;
199             shitm = incov->mat;
200             if (j==k) shitm->el.double_v[j][k] = 1.0;
201             else shitm->el.double_v[j][k]=0.0;
202         }
203     }
204     rfree(a);
205     return(incov);
206 }
207 
208 

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