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

Diff markup

Differences between /src/vision/calib/ecal_smplx.c (Architecture ppc) and /src/vision/calib/ecal_smplx.c (Architecture sparc)


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