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

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

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