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

Linux Cross Reference
Tina5/tina-libs/tina/vision/visCalib_error.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/vision/visCalib_error.c,v $
 23  * Date    :  $Date: 2008/10/02 11:53:05 $
 24  * Version :  $Revision: 1.6 $
 25  * CVS Id  :  $Id: visCalib_error.c,v 1.6 2008/10/02 11:53:05 neil Exp $
 26  *
 27  * Author  : Legacy TINA
 28  *
 29  * Notes :
 30  *
 31  *********
 32 */
 33 
 34 
 35 
 36 #if HAVE_CONFIG_H
 37   #include <config.h>
 38 #endif
 39 
 40 #include <math.h>
 41 #include <float.h>
 42 #include <tina/sys/sysDef.h>
 43 #include <tina/sys/sysPro.h>
 44 #include <tina/math/mathDef.h>
 45 #include <tina/math/mathPro.h>
 46 #include <tina/image/imgDef.h>
 47 #include <tina/image/imgPro.h>
 48 #include <tina/geometry/geomDef.h>
 49 #include <tina/geometry/geomPro.h>
 50 #include <tina/vision/vis_CalibPro.h>
 51 
 52 #include "visCalib_error.h"
 53 
 54 
 55 /* routine for calculating summed squared error in the image plane for
 56  * the first n_data points stored in the world3d matched structure NAT
 57  * 6/2/91     */
 58 double  camerror(int *n_data, double *f, Camera * cam, List * world3d, Vec2 * (*pix_get) ( /* ??? */ ), Vec3 * (*world_get) ( /* ??? */ ),
 59 double accuracy)
 60 {
 61     List   *l;
 62     Vec2    e = {Vec2_id};
 63     Vec2    cam_proj();
 64     Vec2   *c;
 65     Vec3   *v;
 66     double  diffx, diffy;
 67     double  errorsq = 0.0;
 68     int     i = 0;
 69 
 70     for (l = world3d; l != NULL; l = l->next)
 71     {
 72         if ((c = pix_get(l)) && (v = world_get(l)))
 73         {
 74             e = cam_proj(cam, *v);
 75             diffx = e.el[0] - c->el[0];
 76             diffy = e.el[1] - c->el[1];
 77             if (diffx > 3.0 * accuracy)
 78                 diffx = 3.0 * accuracy;
 79             if (diffx < -3.0 * accuracy)
 80                 diffx = -3.0 * accuracy;
 81             if (diffy > 3.0 * accuracy)
 82                 diffy = 3.0 * accuracy;
 83             if (diffy < -3.0 * accuracy)
 84                 diffy = -3.0 * accuracy;
 85             errorsq += diffx * diffx + diffy * diffy;
 86             if (f != NULL)
 87             {
 88                 f[i++] = diffx;
 89                 f[i++] = diffy;
 90             } else
 91                 i += 2;
 92             if (i == *n_data)
 93                 break;
 94         }
 95     }
 96     *n_data = i;
 97     if (errorsq > 0.0 || errorsq >= 0.0)
 98         return (errorsq);
 99     else
100         return (DBL_MAX);
101 }
102 
103 /*
104 Function calculates relative lat_shift for a line as projected onto the image plane.
105 Calculated for point calculated by get_semi_memo().
106 Set maximum search to twice the expected maximum shift to aid localisation.
107 */
108 float proj_shift_line( float lat_shift,  Camera * cam, Vec3 point  )
109 {
110     Vec3  cam_pos, cam_vec;
111     float  length;
112 
113     cam_pos = cam->transf->t;
114     cam_vec = vec3_diff( point, cam_pos );
115     length = 2.0*lat_shift*vec3_mod( cam_vec )*cam->pixel/cam->f;
116     if (length > 15.0)
117        length = 15.0;
118     if (length < -15.0)
119        length = -15.0;
120 
121     return (length);
122 }
123 
124 List * occluding_line( int choice, Cliche_id * cl_id, double *fOrient, Vec2 *perpVec, double pix_sep )
125 {
126         Vec2 p1,  p2;
127         double  t, diff, diff1;
128         Vec2   *pos, dv;
129         List   *line=NULL;
130         int n;
131 
132         if (choice ==0)
133         {
134            p1 =  cl_id->p1;
135            p2 =  cl_id->p2;
136            dv = vec2_diff(p2, p1);
137            if (cl_id->n1 == 0)
138            {
139                diff = vec2_dist(p1, p2);
140                n = (int) diff/pix_sep;
141                cl_id->n1  = n;
142            }
143            else n = cl_id->n1;
144         }
145         else
146         {
147            p1 =  cl_id->p3;
148            p2 =  cl_id->p4;
149            dv = vec2_diff(p2, p1);
150            if (cl_id->n2 == 0)
151            {
152                diff = vec2_dist(p1, p2);
153                n = (int) diff/pix_sep;
154                cl_id->n2  = n;
155            }
156            else n = cl_id->n2;
157         }
158         *perpVec = vec2_perp( dv );
159         *fOrient = get_line_orient( p1, p2 );
160 
161         diff1 = 1.0 / n;
162 
163         for (t = 0.0 + 0.5*diff1; t < 1.0; t += diff1)
164         {
165                 pos = (Vec2 *)ralloc( sizeof(Vec2) );
166                *pos = vec2_sum( p1,  vec2_times( t, dv ) );
167                 line = list_append( (List *) line, link_alloc((void *) pos, 0));
168         }
169         return( line );
170 }
171 
172 void split_ellipse( List * memo, List * part_list, List ** elist1, List ** elist2,
173                     List ** orients1, List ** orients2, Camera * cam,
174                     double * alpha, double * theta, Vec2 * centre, Vec2 * perp_vec )
175 {
176     List   *l,  *elistMem;
177     Vec3    *v, *v3;
178     Vec2    e, p1, p2, p3, p4, p5, /*centre,*/ prev, next, current, rPt, *e_ptr;
179     int     i, count, length;
180     double  beta, ref_pt_x, ref_pt_y, dot_prod, dot_prod2, x1, x2, y1, y2, fdx, fdy;
181     double  f_orient, *o_ptr;
182     Conic  *ellipse_p, ellipse;
183 
184     length = list_length( memo );
185     i = 0;
186     for (l = memo; l != NULL; l = l->next)
187     {
188         if( (v = l->to) )
189         {
190             if( i == 0 ) p1 = cam_proj( cam, *v );
191             else if( i == length / 5 ) p2 = cam_proj( cam, *v );
192             else if( i == 2 * length / 5 ) p3 = cam_proj( cam, *v );
193             else if( i == 3 * length / 5 ) p4 = cam_proj( cam, *v );
194             else if( i == 4 * length / 5 ) p5 = cam_proj( cam, *v );
195             i++;
196         }
197     }
198     if (i < 5 )
199     {
200         format("ellipse too small \n");
201         return;
202     }
203 
204     ellipse_p = conic_5pt( p1, p2, p3, p4, p5 );
205     ellipse = *ellipse_p;
206     *centre = ellipse.center;
207     *theta  = ellipse.theta;
208     *alpha  = ellipse.alpha;
209     beta   = ellipse.beta;
210     ref_pt_x = -sin(*theta);
211     ref_pt_y = cos(*theta);
212     *perp_vec  = vec2( ref_pt_x, ref_pt_y );
213 
214     count = 0;
215 
216     prev = cam_proj( cam, *((Vec3 *)list_get_end(memo)->to) );
217     current = cam_proj( cam, *( (Vec3 *)memo->to) );
218 
219     for ( l = memo; l != NULL; l = l->next)
220     {
221         count ++;
222         if (l->next != NULL) next = cam_proj( cam, *( (Vec3 *)(l->next)->to ) );
223         else                 next = cam_proj( cam, *( (Vec3 *)memo->to) );
224 
225         y1 = vec2_get_y( &next ); y2 = vec2_get_y( &prev );
226         x1 = vec2_get_x( &next ); x2 = vec2_get_x( &prev );
227 
228         fdy =  y2 - y1;
229         fdx =  x2 - x1;
230         f_orient = atan( -fdx/fdy );
231 
232         e_ptr = (Vec2 *) ralloc( sizeof( Vec2 ) );
233         o_ptr = (double *) ralloc( sizeof( double ));
234         *e_ptr = current;
235         *o_ptr = f_orient;
236 
237         e = vec2_sum( *e_ptr, vec2_minus( *centre ));
238         dot_prod = vec2_dot( *perp_vec, e );
239 
240         if( dot_prod >= 0.0 )
241         {
242             *elist1 = list_addtoend( *elist1, link_alloc((void *) e_ptr, 0 ) );
243             *orients1 = list_addtoend( *orients1, link_alloc((void *) o_ptr, 0 ) );
244         }
245         else
246         {
247             *elist2 = list_addtoend( *elist2, link_alloc((void *) e_ptr, 0 ) );
248             *orients2 = list_addtoend( *orients2, link_alloc((void *) o_ptr, 0 ) );
249         }
250         prev = current;
251         current = next;
252     }
253     v3 =   prop_get( ((Generic *) part_list->to)->props, ELLIPSE3D_OR );
254     rPt = cam_proj( cam, *v3 );
255     rPt = vec2_sum( rPt, vec2_minus( *centre ));
256     dot_prod2 = vec2_dot( *perp_vec, rPt );
257 /* swap lists depending upon orientation along line of sight */
258 /* 1 further from cam than 2 (ever tested?) NAT              */
259     if( dot_prod2 > 0.0 )
260     {
261         elistMem = *elist1; *elist1 = *elist2; *elist2 = elistMem;
262         elistMem = *orients1; *orients1 = *orients2; *orients2 = elistMem;
263         *perp_vec = vec2_minus(*perp_vec);
264     }
265 }
266 
267 List *proj_linepts(List * memo, Camera *cam, double * fOrient, Vec2 * perpVec)
268 {
269     List * l, *elist=NULL;
270     Vec2 *e_ptr;
271     Vec2 current;
272     Vec2 startPt, endPt;
273 
274     if (memo==NULL)
275     {
276          error("null data in proj_linepts\n",non_fatal);
277          return(NULL);
278     }
279 
280     l = memo;
281     startPt = cam_proj( cam, *( (Vec3 *)l->to) );
282     e_ptr = (Vec2 *) ralloc( sizeof( Vec2 ) );
283    *e_ptr = startPt;
284     elist = list_addtoend( elist, link_alloc((void *) e_ptr, 0 ) );
285 
286 
287     for (; l != NULL; l = l->next)
288     {
289           current = cam_proj( cam, *( (Vec3 *)l->to) );
290           e_ptr = (Vec2 *) ralloc( sizeof( Vec2 ) );
291           *e_ptr = current;
292           elist = list_addtoend( elist, link_alloc((void *) e_ptr, 0 ) );
293     }
294     endPt = current;
295     *perpVec = vec2_perp( vec2_diff( endPt, startPt ) );
296     *fOrient = get_line_orient( startPt, endPt );
297 
298     return(elist);
299 }
300 
301 Vec3  get_semi_memo( List * memo )
302 {
303     Vec3 loc;
304 /*
305     List *l, *semi = NULL;
306     int   length, i = 0;
307 
308     length = list_length( memo );
309     for ( l = memo; l != NULL; l = l->next )
310     {
311         loc = l->to;
312         if ( i < length/2 ) semi = ref_addtostart( semi, l->to, 0 );
313         else break;
314         i++;
315     }
316 */
317     loc = *(Vec3 *)( memo->to);
318     return ( loc );
319 }
320 
321 double get_line_orient( Vec2 sp, Vec2 ep )
322 {
323     double fdx, fdy;
324 
325     fdx =  vec2_get_x(&sp) - vec2_get_x(&ep) ;
326     fdy =  vec2_get_y(&sp) - vec2_get_y(&ep) ;
327 
328 
329     if      ( fdx == 0.0 && fdy != 0.0 ) return( PI / 2.0 );
330     else if ( fdy == 0.0 && fdx != 0.0 ) return( 0.0 );
331     else                                 return( atan( -fdx/fdy ) );
332 }
333 
334 double lat_shift( List * list, Vec2 perpVec, Camera * cam, Model_smplx *ms,
335                   double lshift1, double lshift2, double line_orient, List * el_orients, double *sh)
336 {
337     int     lSwitch;
338     int     i, llc, left, right;
339     double  idx, idy, logL2, llPeak, log_likelihood1, log_likelihood2;
340     double  iOrient, fOrient, angDiff, shift_pen=0.0, facx, facy;
341     double  shift=0.0;
342     Vec2    pt;
343     double  ptx, pty;
344     List    *l, *l2;
345     Imrect *gradx, *grady, *impot;
346     double a,b,c,x;
347     float *logLVec=NULL;
348     int    pix_div;
349     double scale_factor, scale_factor2, scale_factor3, or_err;
350     double olThresh;
351     shistogram **hists = hist_vec();
352 
353     gradx = ms->gradx;
354     grady = ms->grady;
355     impot = ms->potim;
356     lSwitch = ms->lSwitch;
357     pix_div = ms->pix_div;
358     scale_factor = ms->scale_factor;
359     scale_factor2 = ms->scale_factor2;
360     scale_factor3 = ms->scale_factor3;
361     or_err = ms->or_err;
362     olThresh = 2.0*ms->olThresh/scale_factor2;
363 
364     left  = (int)ceil( (double)pix_div * lshift1 );
365     right = (int)ceil( (double)pix_div * lshift2 );
366     if (left+right < 3)
367     {
368         if (left+right < 0) error("invalid search in lat_shift \n", non_fatal);
369         left = 0;
370         right = 0;
371     } 
372 
373     logLVec = fvector_alloc(0, left+right+3);
374 
375     for ( i = -left; i <= right; i++ )
376     {
377         l2 = el_orients;
378         log_likelihood1 = 0.0;
379         log_likelihood2 = 0.0;
380         facx =  (double) i*perpVec.el[0] / (double) pix_div;
381         facy =  (double) i*perpVec.el[1] / (double) pix_div;
382 
383         for ( l = list; l != NULL; l = l->next )
384         {
385             pt = *(Vec2 *)l->to;
386             ptx = pt.el[0] + facx;
387             pty = pt.el[1] + facy;
388 
389             if( lSwitch != 2  )
390             {
391                 log_likelihood1 += im_sub_pixf( impot, pty, ptx );
392             }
393             if( lSwitch == 2 || lSwitch == 3 )
394             {
395                 idx = im_sub_pixf( gradx , pty, ptx );
396                 idy = im_sub_pixf( grady , pty, ptx );
397 
398                 if      ( idx == 0.0 ) iOrient = PI / 2.0;
399                 else if ( idy == 0.0 ) iOrient = 0.0;
400                 else                   iOrient = atan( idy/idx );
401 
402                 if( el_orients==NULL ) fOrient = line_orient;
403                 else
404                 {   
405                    fOrient = *((double *)l2->to); 
406                    l2 = l2->next;
407                 }
408                 angDiff = fabs( fOrient - iOrient );
409                 if( angDiff > (PI/2.0) ) angDiff = PI - angDiff;
410 
411                 logL2 = angDiff/or_err;
412                 if( logL2 > olThresh ) logL2 =  olThresh;
413                 log_likelihood2 -= logL2;
414             }
415         }        
416         logLVec[i + left + 1] = scale_factor*log_likelihood1 + scale_factor2*log_likelihood2/2.0;
417     }
418  
419 /* pad array */
420     logLVec[0] = logLVec[1]; 
421     logLVec[left + right +2] = logLVec[left + right +1];
422 
423     llPeak = logLVec[1];
424     llc = 1;                                      
425     for( i = llc + 1; i <= ( left + right +1 ); i++ )
426     {
427         if( logLVec[i]  > llPeak )
428         {
429            llPeak = logLVec[i];
430            llc = i;
431         }
432     } 
433     x = 0.0;
434     if ((left+right) !=0)
435     {
436         c = logLVec[llc];
437         b = (logLVec[llc+1]-logLVec[llc-1])/2.0;
438         a = (logLVec[llc+1]+logLVec[llc-1] - 2.0*c)/2.0;
439         if (a!=0.0) x = -b/(2.0*a);
440         if (x >  0.5) x =  0.5;
441         if (x < -0.5) x = -0.5;
442         llPeak = a*x*x + b*x + c;
443     }
444     shift = llc + x - left -1.0; 
445 
446     if ( shift != 0.0 ) shift /= (double)pix_div;
447 
448 /* include prior knowledge on allowed shift AFTER finding the best NAT 5/8/08 */
449 /* assume allowed shift is 1/6 of lshift ie: 3 S.D. */
450     if ( lshift1 > 0.0 || lshift2 >0.0 )
451     {
452         if (shift <= 0.0)
453         {
454             if (lshift1 > 0.0) shift_pen = 18.0*scale_factor3*shift / lshift1;
455         }
456         else
457         {
458             if (lshift2 > 0.0) shift_pen = -18.0*scale_factor3*shift / lshift2;
459         }
460     }
461     else 
462         shift_pen = 0.0; 
463 
464     hfill1(hists[4], -shift_pen, 1.0);
465     *sh = shift; 
466     fvector_free(logLVec,0);
467 
468     if (lSwitch !=0) return(llPeak + shift_pen);
469     else return ( shift_pen );
470 }
471 
472 double pot_camerror(int *n_data, double *f, Model_smplx *ms, double accuracy,  List *cliche_spec)
473 {
474     Camera *cam;
475     List   *partList;
476     List   *l, *memo;
477     Vec2    cam_proj();
478     double  errorsq = 0.0;
479     int     geom_type;
480     Vec2    centre, perpVec;
481     double  theta, alpha, x, y, lat_shift1, lat_shift2;
482     double  shift, def_shift =5.0;
483     static List   *elist1 = NULL, *elist2 = NULL, *orients1 = NULL, *orients2 = NULL;
484     static List   *occ_line = NULL;
485     int     partNum;
486     List   *occ_bounds = NULL, *ptr;
487     Vec3    semi_memo;
488     Cliche_id   *cl_id, *o_s;
489     double  fOrient;
490     double shift_factor, pix_sep;
491 
492     cam = ms->cam;
493     partList = ms->parts;
494     shift_factor = ms->shift_factor;
495     pix_sep = ms->pix_sep;
496 
497     for ( ptr = cliche_spec; ptr != NULL; ptr = ptr->next )
498     {
499         cl_id = ((Cliche_id *)(ptr->to)); 
500 
501         if ( cl_id->type == 'O' )
502         {       
503             occ_bounds = ref_addtostart( occ_bounds, (void *)   cl_id, 0 );
504         }
505     }
506 
507     errorsq=0.0; 
508     l = partList;
509     for ( ; partList != NULL; partList = partList->next)
510     {
511         memo = ((Generic *) partList->to)->to;
512         if (memo ==NULL) continue;
513         geom_type = ((Generic *) partList->to)->type;
514         partNum = ((Generic *)(partList->to))->label;
515         cl_id = NULL;
516         
517         for ( ptr = cliche_spec; ptr != NULL; ptr = ptr->next )
518         {                       
519             cl_id = ((Cliche_id *)(ptr->to));
520             if ( cl_id->feat1 == partNum && cl_id->type != 'O' ) break;
521             else if ( ptr->next == NULL ) cl_id = NULL;
522         }                       
523         switch ( geom_type )
524         {
525             case LINE3:
526 
527             elist1 = proj_linepts(memo, cam, &fOrient, &perpVec);
528             if (cl_id !=NULL)
529             {
530                 semi_memo = get_semi_memo( memo );
531                 lat_shift1 = shift_factor*proj_shift_line( cl_id->latShift1, cam, semi_memo );
532                 lat_shift2 = shift_factor*proj_shift_line( cl_id->latShift2, cam, semi_memo );
533             }
534             else
535             {
536                 lat_shift1 = def_shift;
537                 lat_shift2 = def_shift;
538             }
539             errorsq += lat_shift( elist1, perpVec, cam, ms, lat_shift1, lat_shift2, fOrient, NULL, &shift);
540             list_rm(elist1, rfree);
541             elist1 = NULL;
542             break;
543 
544             case CONIC3:
545 
546             split_ellipse( memo, partList, &elist1, &elist2, &orients1, &orients2, cam, &alpha, &theta, &centre, &perpVec ); 
547             x = vec2_get_x( &centre ); 
548             y = vec2_get_y( &centre );
549             semi_memo = get_semi_memo( memo );
550             for ( ptr = occ_bounds; ptr != NULL; ptr = ptr->next ) 
551             {
552                 o_s = ptr->to;
553                 if ( o_s->feat1 == partNum )
554                 {
555                     o_s->p1 = vec2( alpha * cos(theta) + x,  alpha * sin(theta) + y );
556                     o_s->p3 = vec2( -(alpha * cos(theta)) + x, -(alpha * sin(theta)) +y );
557                     o_s->l1 = semi_memo;
558                 }
559                 if ( o_s->feat2 == partNum )
560                 {
561                     o_s->p2 = vec2( alpha * cos(theta) + x,  alpha * sin(theta) + y );
562                     o_s->p4 = vec2( -(alpha * cos(theta)) + x, -(alpha * sin(theta)) +y );
563                     o_s->l2 = semi_memo;
564                 }
565             }
566 
567             if ( (cl_id !=NULL) && ( cl_id->display1 == 1 )  ) 
568             {
569                 lat_shift1 = shift_factor*proj_shift_line( cl_id->latShift1, cam, semi_memo );
570                 lat_shift2 = shift_factor*proj_shift_line( cl_id->latShift2, cam, semi_memo );
571                 errorsq += lat_shift(elist1, perpVec, cam, ms, lat_shift1, lat_shift2, 0.0, orients1, &shift);
572             }
573             else if (cl_id ==NULL)
574             {
575                 lat_shift1 = def_shift;
576                 lat_shift2 = def_shift;
577                 errorsq += lat_shift(elist1, perpVec, cam, ms, lat_shift1, lat_shift2, 0.0, orients1, &shift);
578             }
579 
580             if ( (cl_id !=NULL) && ( cl_id->display2 == 1 )  )
581             {
582                 lat_shift1 = shift_factor*proj_shift_line( cl_id->latShift3, cam, semi_memo );
583                 lat_shift2 = shift_factor*proj_shift_line( cl_id->latShift4, cam, semi_memo );
584                 errorsq += lat_shift(elist2, perpVec, cam, ms, lat_shift1, lat_shift2, 0.0, orients2, &shift);
585             }
586             else if (cl_id ==NULL)
587             {
588                 lat_shift1 = def_shift;
589                 lat_shift2 = def_shift;
590                 errorsq += lat_shift(elist2, perpVec, cam, ms, lat_shift1, lat_shift2, 0.0, orients2, &shift);
591             }
592 
593             list_rm( elist1, rfree);
594             list_rm( elist2, rfree);
595             list_rm( orients1, rfree);
596             list_rm( orients2, rfree);
597             elist1 = NULL; elist2 = NULL; orients1 = NULL; orients2 = NULL;
598             break;
599         }
600     }
601     partList=l;
602 
603     if(ms->occflag==1)
604     {
605         for ( ptr = occ_bounds; ptr != NULL; ptr = ptr->next )
606         {
607             o_s = ptr->to;  
608             if ( o_s->display1 == 1 )
609             {
610                 occ_line = occluding_line(0, o_s, &fOrient, &perpVec, pix_sep );
611                 lat_shift1 = shift_factor*proj_shift_line( o_s->latShift1, cam, o_s->l1 );
612                 lat_shift2 = shift_factor*proj_shift_line( o_s->latShift2, cam, o_s->l2 );
613                 errorsq += lat_shift( occ_line, perpVec, cam, ms, lat_shift1, lat_shift2, fOrient, NULL, &shift );
614                 list_rm(occ_line, rfree);
615             }
616             if ( o_s->display2 == 1 )
617             {
618                 occ_line = occluding_line( 1, o_s, &fOrient, &perpVec, pix_sep );
619                 lat_shift1 = shift_factor*proj_shift_line( o_s->latShift3, cam, o_s->l1 );
620                 lat_shift2 = shift_factor*proj_shift_line( o_s->latShift4, cam, o_s->l2 );
621                 errorsq += lat_shift( occ_line, perpVec, cam, ms, lat_shift1, lat_shift2, fOrient, NULL, &shift );
622                 list_rm(occ_line, rfree);
623             }
624         }
625     }
626 /*
627     else 
628          format("oc boundaires not used \n");
629 */
630         list_rm_links(occ_bounds);
631 
632         return (errorsq);
633 }
634 
635 /* routine for calculating summed squared error in the image plane for
636  * the first n_data points stored in the world3d matched structure NAT
637  * 6/2/91 */
638 double  rad_camerror(int *n_data, double *rad, double *err, Camera * cam, List * world3d, Vec2 * (*pix_get) ( /* ??? */ ), Vec3 * (*world_get) (
639 /* ??? */ ))
640 {
641     List   *l;
642     Vec2    e = {Vec2_id};
643     Vec2    cam_proj();
644     Vec2   *c;
645     Vec3   *v;
646     double  diff;
647     double  errorsq = 0.0;
648     double  rad1, rad2;
649     int     i = 0;
650 
651     for (l = world3d; l != NULL; l = l->next)
652     {
653         if ((c = pix_get(l)) && (v = world_get(l)))
654         {
655             e = cam_proj(cam, *v);
656             e.el[0] -= cam->cx;
657             e.el[1] -= cam->cy;
658             rad1 = e.el[0] * e.el[0] / (cam->ax * cam->ax)
659                 + e.el[1] * e.el[1] / (cam->ay * cam->ay);
660             rad1 = sqrt(rad1);
661             rad2 = (c->el[0] - cam->cx) * (c->el[0] - cam->cx) / (cam->ax * cam->ax)
662                 + (c->el[1] - cam->cy) * (c->el[1] - cam->cy) / (cam->ay * cam->ay);
663             rad2 = sqrt(rad2);
664             diff = rad1 - rad2;
665             errorsq += diff * diff;
666             if (rad != NULL && err != NULL)
667             {
668                 rad[i] = rad1;
669                 err[i++] = diff;
670             } else
671                 i++;
672             if (i == *n_data)
673                 break;
674         }
675     }
676     *n_data = i;
677     if (errorsq > 0.0 || errorsq >= 0.0)
678         return (errorsq);
679     else
680         return (DBL_MAX);
681 }
682 
683 
684 /* calculates the total off-epipolar error for a set of matched stereo
685  * data points using the Trivedi formulation for the between camera
686  * transformation NAT 6/6/91 */
687 double  triv_camerror(int *n_data, double *x, Camera * cam1, Camera * cam2, List * world3d, Vec2 * (*pix_get1) ( /* ??? */ ), Vec2 * (*pix_get2)
688 ( /* ??? */ ), double accuracy)
689 {
690     Transform3 rel2to1 = {Transform3_id};
691     Transform3 inv2 = {Transform3_id};
692     List   *plist;
693     Vec2    tl = {Vec2_id};
694     Vec2    tr = {Vec2_id};
695     Vec2   *l;
696     Vec2   *r;
697     Vec3    lp = {Vec3_id};
698     Vec3    rp = {Vec3_id};
699     Vec3    gradfl = {Vec3_id};
700     Vec3    gradfr = {Vec3_id};
701     Vec3    mat3_vprod();
702     Vec3    mat3_transpose_vprod();
703     int     i = 0;
704     Mat3    R = {Mat3_id};
705     Mat3    S = {Mat3_id};
706     Mat3    T = {Mat3_id};
707     Mat3    S_array(double T1, double T2, double T3);
708     double  f, gradsq, lam;
709     double  m, toterr = 0.0, errorsq;
710     double  elx, ely, erx, ery;
711 
712     if (cam1 == NULL || cam2 == NULL || world3d == NULL)
713     {
714         *n_data = 0;
715         return (0.0);
716     }
717 
718     inv2 = trans3_inverse(*(cam2->transf));
719     rel2to1 = trans3_prod(*(cam1->transf), inv2);
720     m = sqrt(rel2to1.t.el[0] * rel2to1.t.el[0]
721              + rel2to1.t.el[1] * rel2to1.t.el[1]
722              + rel2to1.t.el[2] * rel2to1.t.el[2]);
723     R = rel2to1.R;
724     S = S_array(rel2to1.t.el[0] / m, rel2to1.t.el[1] / m, rel2to1.t.el[2] / m);
725     T = mat3_prod(S, R);
726 
727     for (plist = world3d; plist != NULL; plist = plist->next)
728     {
729         if ((l = pix_get1(plist)) && (r = pix_get2(plist)))
730         {
731             tl = cam_correct(*l, cam1);
732             tr = cam_correct(*r, cam2);
733             lp.el[0] = (tl.el[0] - cam1->cx) / cam1->ax;
734             lp.el[1] = (tl.el[1] - cam1->cy) / cam1->ay;
735             lp.el[2] = cam1->f / cam1->pixel;
736             rp.el[0] = (tr.el[0] - cam2->cx) / cam2->ax;
737             rp.el[1] = (tr.el[1] - cam2->cy) / cam2->ay;
738             rp.el[2] = cam2->f / cam2->pixel;
739             gradfl = mat3_vprod(T, rp);
740             gradfr = mat3_transpose_vprod(T, lp);
741 
742             f = gradfl.el[0] * lp.el[0]
743                 + gradfl.el[1] * lp.el[1]
744                 + gradfl.el[2] * lp.el[2];
745 
746             gradsq = gradfl.el[0] * gradfl.el[0] / (cam1->ax * cam1->ax)
747                 + gradfl.el[1] * gradfl.el[1] / (cam1->ay * cam1->ay)
748                 + gradfr.el[0] * gradfr.el[0] / (cam2->ax * cam2->ax)
749                 + gradfr.el[1] * gradfr.el[1] / (cam2->ay * cam2->ay);
750 
751             lam = f / gradsq;
752 
753             elx = -gradfl.el[0] * lam / (cam1->ax * cam1->ax);
754             ely = -gradfl.el[1] * lam / (cam1->ay * cam1->ay);
755             erx = -gradfr.el[0] * lam / (cam2->ax * cam2->ax);
756             ery = -gradfr.el[1] * lam / (cam2->ay * cam2->ay);
757 
758             if (elx > 3.0 * accuracy)
759                 elx = 3.0 * accuracy;
760             if (ely > 3.0 * accuracy)
761                 ely = 3.0 * accuracy;
762             if (erx > 3.0 * accuracy)
763                 erx = 3.0 * accuracy;
764             if (ery > 3.0 * accuracy)
765                 ery = 3.0 * accuracy;
766             if (elx < -3.0 * accuracy)
767                 elx = -3.0 * accuracy;
768             if (ely < -3.0 * accuracy)
769                 ely = -3.0 * accuracy;
770             if (erx < -3.0 * accuracy)
771                 erx = -3.0 * accuracy;
772             if (ery < -3.0 * accuracy)
773                 ery = -3.0 * accuracy;
774 
775             /* errorsq = lam*f; */
776             errorsq = elx * elx + ely * ely + erx * erx + ery * ery;
777             if (x != NULL)
778             {
779                 x[i++] = elx;
780                 x[i++] = ely;
781                 x[i++] = erx;
782                 x[i++] = ery;
783             } else
784                 i += 4;
785             toterr += errorsq;
786             if (i == *n_data)
787                 break;
788         }
789     }
790     *n_data = i;
791     if (toterr >= 0.0)
792         return (toterr);
793     else
794         return (DBL_MAX);
795 }
796 
797 double  cam_reg(Covar * incov, int mask, double *a)
798 {
799     Matrix *matrix_alloc();
800     Matrix *matrix_prod();
801     Matrix *delta;
802     Matrix *dprod;
803     double  chisq = 0.0;
804     int     i, n_par;
805 
806     if (incov == NULL)
807         return (0.0);
808 
809     for (i = 0, n_par = 6; i < 16; i++)
810         if (mask & (1 << i))
811             n_par++;
812     delta = matrix_alloc(1, n_par, matrix_full, double_v);
813     for (i = 0; i < n_par; i++)
814     {
815         delta->el.double_v[0][i] = a[i + 6] - VECTOR_DOUBLE(incov->vec, i);
816     }
817     dprod = matrix_prod(delta, incov->mat);
818     for (i = 0; i < n_par; i++)
819     {
820         chisq += dprod->el.double_v[0][i] * delta->el.double_v[0][i];
821     }
822     matrix_free(delta);
823     matrix_free(dprod);
824     return (chisq);
825 }
826 
827 double  stereo_reg(Covar * incov, int mask, double *a)
828 {
829     Matrix *matrix_alloc();
830     Matrix *matrix_prod();
831     Matrix *delta;
832     Matrix *dprod;
833     double  chisq = 0.0;
834     int     i, n_par = 0;
835 
836     if (incov == NULL)
837         return (0.0);
838 
839     for (i = 0; i < 16; i++)
840         if (mask & (1 << i))
841             n_par++;
842     delta = matrix_alloc(1, 2 * n_par + 6, matrix_full, double_v);
843     for (i = 0; i < 2 * n_par + 6; i++)
844     {
845         delta->el.double_v[0][i] = a[i] - VECTOR_DOUBLE(incov->vec, i);
846     }
847     dprod = matrix_prod(delta, incov->mat);
848     for (i = 0; i < 2 * n_par + 6; i++)
849     {
850         chisq += dprod->el.double_v[0][i] * delta->el.double_v[0][i];
851     }
852     matrix_free(delta);
853     matrix_free(dprod);
854 
855     return (chisq);
856 }
857 
858 Mat3    S_array(double T1, double T2, double T3)
859 {
860     Mat3    S = {Mat3_id};
861 
862     S.el[0][0] = (float)0.0;
863     S.el[1][0] = (float)T3;
864     S.el[2][0] = (float)-T2;
865     S.el[0][1] = (float)-T3;
866     S.el[1][1] = (float)0.0;
867     S.el[2][1] = (float)T1;
868     S.el[0][2] = (float)T2;
869     S.el[1][2] = (float)-T1;
870     S.el[2][2] = (float)0.0;
871     return (S);
872 }
873 
874 
875 

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