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

Linux Cross Reference
Tina6/tina-libs/tina/geometry/geomCurve_es_string3.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/geometry/geomCurve_es_string3.c,v $
 27  * Date    :  $Date: 2008/12/04 15:28:02 $
 28  * Version :  $Revision: 1.4 $
 29  * CVS Id  :  $Id: geomCurve_es_string3.c,v 1.4 2008/12/04 15:28:02 paul Exp $
 30  *
 31  * Author  : Legacy TINA
 32  *
 33  * Notes :
 34  *
 35  * build a string of 3d disparity points from an approximation to a curve
 36  * and a string of edges
 37  * 
 38  * the curve is a suitable approx to the edge string found by a spline or
 39  * conic fitter say
 40  * 
 41  *********
 42 */
 43 
 44 #include "geomCurve_es_string3.h"
 45 
 46 #if HAVE_CONFIG_H
 47   #include <config.h>
 48 #endif
 49 
 50 #include <math.h>
 51 #include <tina/sys/sysDef.h>
 52 #include <tina/sys/sysPro.h>
 53 #include <tina/math/mathDef.h>
 54 #include <tina/math/mathPro.h>
 55 #include <tina/image/imgDef.h>
 56 #include <tina/geometry/geomDef.h>
 57 #include <tina/geometry/geomPro.h>
 58 #include <tina/geometry/geom_IndxDef.h>
 59 #include <tina/geometry/geom_IndxPro.h>
 60 #include <tina/geometry/geom_EdgePro.h>
 61 #include <tina/geometry/geom_PlaneDef.h>
 62 
 63 #define DGRADCURVE 1.2
 64 #define EXTRADATA 2001
 65 
 66 Tstring *es_par_proj_to_plane(Tstring * es, Plane * plane)
 67 {
 68     Tstring *str3;
 69     List *ptr;
 70     List *end;
 71     Vec3    p = {Vec3_id};
 72     Vec3    n = {Vec3_id};
 73 
 74     if (es == NULL || plane == NULL)
 75         return (NULL);
 76 
 77     p = plane->p;
 78     n = plane->n;
 79     str3 = str_clone(es);
 80     ptr = str3->start;
 81     end = str3->end;
 82 
 83     do
 84     {
 85         Vec3    v = {Vec3_id};
 86         Vec3    o = {Vec3_id};
 87         Vec2    pos = {Vec2_id};
 88 
 89         DD_GET_POS2(ptr, pos);
 90         par_proj_ray(pos, &o, &v);
 91         ptr->to = vec3_make(vec3_inter_line_plane(o, v, p, n));
 92         ptr->type = VEC3;
 93         if (ptr == end)
 94             break;
 95         ptr = ptr->next;
 96     } while (1);
 97     return (str3);
 98 }
 99 
100 static Vec3 *disp_pos3(Vec2 p, Vec2 p1, Vec2 p2)
101 {
102     Vec2    v = {Vec2_id};
103     float   x;
104 
105     v = vec2_unit(vec2_diff(p2, p1));
106     x = vec2_x(p1) + (vec2_y(p) - vec2_y(p1)) * vec2_x(v) / vec2_y(v);
107     return (vec3_make(vec3(x, vec2_y(p), vec2_x(p) - x)));
108 }
109 
110 static Vec3 *cv_approx3_next(List * curv, Vec2 pos, List * end)
111 {
112     Vec2    cpos = {Vec2_id};
113     Vec2    npos = {Vec2_id};
114     List *ptr;
115 
116     DD_GET_POS2(curv, cpos);
117 
118     for (ptr = curv;; ptr = ptr->next)
119     {
120         DD_GET_POS2(ptr, npos);
121         if (BETWEEN(vec2_y(pos), vec2_y(cpos), vec2_y(npos)))
122             return (disp_pos3(pos, cpos, npos));
123 
124         if (ptr == end)
125             break;
126     }
127     return (NULL);
128 }
129 
130 static Vec3 *cv_approx3_last(List * curv, Vec2 pos, List * start)
131 {
132     Vec2    cpos = {Vec2_id};
133     Vec2    lpos = {Vec2_id};
134     List *ptr;
135 
136     DD_GET_POS2(curv, cpos);
137 
138     for (ptr = curv;; ptr = ptr->last)
139     {
140         DD_GET_POS2(ptr, lpos);
141         if (BETWEEN(vec2_y(pos), vec2_y(cpos), vec2_y(lpos)))
142             return (disp_pos3(pos, cpos, lpos));
143 
144         if (ptr == start)
145             break;
146     }
147     return (NULL);
148 }
149 
150 Vec3   *cv_approx3(List * curv, Vec2 pos, List * start, List * end)
151 {
152     Vec2    cpos = {Vec2_id};
153     Vec2    npos = {Vec2_id};
154     Vec2    lpos = {Vec2_id};
155 
156     if (curv == NULL || (curv->last == NULL && curv->next == NULL))
157         return (NULL);
158 
159     if (curv == start)
160         return (cv_approx3_next(curv, pos, end));
161     if (curv == end)
162         return (cv_approx3_last(curv, pos, start));
163 
164     DD_GET_POS2(curv, cpos);
165     DD_GET_POS2(curv->last, lpos);
166     DD_GET_POS2(curv->next, npos);
167 
168     if ((vec2_y(cpos) < vec2_y(lpos)) == (vec2_y(cpos) < vec2_y(npos))) /* ambiguous */
169         return (NULL);
170 
171     if ((vec2_y(cpos) < vec2_y(pos)) == (vec2_y(cpos) < vec2_y(npos)))  /* go next */
172         return (cv_approx3_next(curv, pos, end));
173     if ((vec2_y(cpos) < vec2_y(pos)) == (vec2_y(cpos) < vec2_y(lpos)))  /* go last */
174             return (cv_approx3_last(curv, pos, start));
175     return (NULL);              /* strange occurance */
176 }
177 
178 /* build a fast index into the string structure */
179 static Rindex *string_index_get(Tstring * es)
180 {
181     Imregion *roi = es_bounding_region(es);
182     List *end;
183     List *ptr;
184     Rindex *rx;
185     List  **index;
186 
187     if (roi == NULL)
188         return (NULL);
189 
190     rx = rx_alloc(roi, STRING);
191     rfree((void *) roi);
192     index = (List **) rx->index;
193     ptr = es->start;
194     end = es->end;
195 
196     do
197     {
198         Vec2    p = {Vec2_id};
199         int     i;
200 
201         DD_GET_POS2(ptr, p);
202         i = (int)floor(vec2_y(p));
203         index[i] = ref_addtostart(index[i], (void *) ptr, LIST);
204         if (ptr == end)
205             break;
206         ptr = ptr->next;
207     } while (1);
208     return (rx);
209 }
210 
211 #define SEARCH_WINDOW  3
212 #define MAX_ALLOW_DIFF 5.0
213 
214 static List *closest_in_index(Rindex * rx, Vec2 pos)
215 {
216     int     r, i;
217     List *closest = NULL;
218     Imregion *roi;
219     List   *ptr;
220     Vec2    p = {Vec2_id};
221     float   dist;
222     float   min_dist = (float)0.0;
223 
224     if (rx == NULL)
225         return (NULL);
226 
227     roi = rx->region;
228 
229     r = (int)floor(vec2_y(p));
230     i = r - SEARCH_WINDOW;
231     if (i < roi->ly)
232         i = roi->ly;
233     r += SEARCH_WINDOW;
234     if (r > roi->uy)
235         r = roi->uy;
236 
237     while (i < r)
238     {
239         for (ptr = rx->index[i]; ptr != NULL; ptr = ptr->next)
240         {
241             List *dd = (List *) ptr->to;
242 
243             DD_GET_POS2(dd, p);
244 
245             dist = (float)vec2_mod(vec2_diff(pos, p));
246             if (dist < MAX_ALLOW_DIFF && (closest == NULL || dist < min_dist))
247             {
248                 min_dist = dist;
249                 closest = dd;
250             }
251         }
252         ++i;
253     }
254     return (closest);
255 }
256 
257 /***** This next function came from vision/visMatch_es.c PAB *****/
258 
259 /* get the list of matches at a meta string level from the union
260  * implied by matches between epipolar edge subs-trings
261  * 
262  * 
263  * the resulting match list is unique wrt ->to2 field (what they match to)
264  * 
265  * meta edge strings may be a composite made from copies of smaller edge
266  * strings
267  * 
268  * hence it follows that: 1. edge strings and substrings may not share
269  * list elements 2. edge strings and substrings do reference the same
270  * edgels 3. edge strings and substrings may not preserve the local
271  * order of edges */
272 List   *es_get_list_of_matches(Tstring * es)
273 {
274     List *ptr;
275     List *end;
276     Tstring *sub, *sub_last = NULL;
277     List   *matches = NULL;
278 
279     if (es == NULL)
280         return (NULL);
281 
282     end = es->end;
283     for (ptr = es->start;; ptr = ptr->next)
284     {
285         Edgel  *edge;
286         List   *mlist;
287 
288         if (ptr->type != EDGE)
289             continue;
290 
291         edge = DD_EDGE(ptr);
292         sub = (Tstring *) prop_get(edge->props, SINDEX);
293         if (sub != NULL && sub != sub_last)
294         {
295             mlist = (List *) prop_get(sub->props, MLIST);
296 
297             for (; mlist != NULL; mlist = mlist->next)
298             {
299                 Match  *m = (Match *) mlist->to;
300 
301                 if (link_get_by_ref(matches, m->to2) == NULL)
302                     matches = ref_addtostart((List *) matches, (void *) m, MATCH);
303             }
304         }
305         sub_last = sub;
306         if (ptr == end)
307             break;
308     }
309     return (matches);
310 }
311 
312 
313 Tstring *es_build_string3(Tstring * curv, Tstring * es)
314 /* make approximated geometrical string in disparity space relative to curv */
315 {
316     List   *mlist;
317     List   *mptr;
318     List *dispstr = NULL;
319     List *start;
320     List *end;
321     Rindex *rx;
322 
323     if (curv == NULL || es == NULL)
324         return (NULL);
325 
326     /* get list of matches unique in ->to2  ordered along list */
327     mlist = es_get_list_of_matches(es);
328     rx = string_index_get(curv);/* index string */
329 
330     start = curv->start;
331     end = curv->end;
332 
333     for (mptr = mlist; mptr != NULL; mptr = mptr->next)
334     {
335         List *dptr;
336         List *closest;
337         Tstring *s1, *s2;
338         Vec2    p = {Vec2_id};
339 
340         s1 = (Tstring *) ((Match *) mptr->to)->to1;
341         s2 = (Tstring *) ((Match *) mptr->to)->to2;
342 
343         p = DD_EDGE_POS(s1->start);
344         closest = closest_in_index(rx, p);
345         if (closest == NULL)
346             closest = str_get_min(curv, dist_to_pos2, (void *) &p);     /* slow */
347 
348         for (dptr = s2->start;; dptr = dptr->next)
349         {
350             Vec3   *p;
351 
352             p = cv_approx3(closest, DD_EDGE_POS(dptr), start, end);
353             if (p != NULL)
354                 dispstr = dd_ref_addtostart(dispstr, (void *) p, VEC3);
355             if (dptr == s2->end)
356                 break;
357         }
358     }
359     rx_free_links(rx);          /* free sting index */
360     if (dispstr == NULL)
361         return (NULL);
362     return (str_make(STRING, dispstr, dd_get_end(dispstr)));
363 }
364 
365 /* compute disparity vector making allowance for mean orientation NAT 25/10/08 */
366 static Vec3  *disparity_vec(Edgel *edge1, Edgel *edge2)
367 {
368      double orient1, orient2;
369      double deltax, deltay;
370      double tantheta;
371      Vec3 *p;
372 
373      orient1 = edge1->orient;
374      orient2 = edge2->orient;
375 
376      if (orient1-orient2 > PI) orient2 += PI;
377      if (orient1-orient2 < -PI) orient2 -= PI;
378 
379      deltay = edge1->pos.el[1]-edge2->pos.el[1];
380      tantheta = tan((orient1+orient2)/2.0);
381  /* nan and stability trap using modal division with 0.1 angle sigma..... see 2000-011 */
382      if ( tantheta > 0.0)
383          tantheta = (tantheta + sqrt(tantheta*tantheta + 8.0 * 0.01))/2.0;
384      else 
385          tantheta = (tantheta - sqrt(tantheta*tantheta + 8.0 * 0.01))/2.0;
386      deltax = deltay/tantheta;
387 
388      p =vec3_make(vec3(edge1->pos.el[0],(edge1->pos.el[1]+edge2->pos.el[1])/2.0,
389                           edge2->pos.el[0]-edge1->pos.el[0]-deltax));
390      return(p);
391 }
392 
393 Tstring *es_build_string3_rect2(Tstring * curv, Tstring * es)
394 /* string of matched points in disparity space from rectified edges NAT*/
395 {
396     List *dptr;
397     List *dispstr = NULL;
398 
399     if (curv == NULL || es == NULL)
400         return (NULL);
401 
402     for (dptr = es->start; dptr!= NULL; dptr = dptr->next)
403     {
404         Vec3   *p;
405         Edgel *edge1, *edge2;
406         Match *match1;
407 
408         edge1 = dptr->to;
409         /* get the edgel that it matched to */
410         match1 = (Match *)prop_get(edge1->props, MATCH);
411         if (match1 != NULL )
412         {
413             edge2 = (Edgel *)match1->to2;
414             p = disparity_vec(edge1, edge2);
415             dispstr = dd_ref_addtostart(dispstr, (void *) p, VEC3);
416         }
417         if (dptr == es->end)
418            break;
419     }
420 
421     if (dispstr == NULL)
422         return (NULL);
423     return (str_make(STRING, dispstr, dd_get_end(dispstr)));
424 }
425 
426 /* remove high DG points whilst maintaining data order NAT 25/10/08 */
427 static List *es_dgrad_filter(List *dispstr, List **remainder)
428 {
429     int count, count2, split, beginning, i;
430     double dgrad, maxgrad, meangrad;
431     Vec3 *p=NULL, *oldp=NULL;
432     List *ptr=NULL, *rem=NULL, *mer=NULL;
433     Vec3 **oldstr=NULL, **newstr=NULL;
434 
435     count = 0;
436     for (ptr =dispstr; ptr !=NULL; ptr = ptr->next)
437     {
438        count++;
439     }
440     oldstr = (Vec3 **)pvector_alloc(0,count+1);
441     newstr = (Vec3 **)pvector_alloc(0,count+1);
442     count = 0;
443     for (ptr =dispstr; ptr !=NULL; ptr = ptr->next)
444     {
445        oldstr[count] = (Vec3 *)ptr->to;
446        newstr[count] = NULL; 
447        count++;
448     }
449     do
450     {
451         split = 0;
452         meangrad = 0;
453         maxgrad = 0;
454         count2 = 0;
455         for (i=0; i<count; i++)
456         {
457             p = oldstr[i];
458             if (p!=NULL )
459             {
460                 if (count2 >0)
461                 {
462                     dgrad = (oldp->el[2]-p->el[2])/
463                          sqrt((oldp->el[0] - p->el[0])*(oldp->el[0] - p->el[0])+
464                               (oldp->el[1] - p->el[1])*(oldp->el[1] - p->el[1]));
465                     if (fabs(dgrad) > fabs(maxgrad))
466                     {
467                         maxgrad = dgrad;
468                         split = count2;
469                     }
470                     meangrad += dgrad;
471                 }
472                 oldp = p;
473                 count2++;
474             }
475         }
476         if (count2 >= 6)
477         {
478             meangrad = (meangrad - maxgrad)/(count2-1);
479             if (split < (count2/2.0)) beginning = 1;
480             else beginning = 0;
481 
482             if (fabs(maxgrad - meangrad)  > DGRADCURVE)
483             {
484                 count2 = 0;
485                 for (i=0; i<count; i++)
486                 {
487                     p = oldstr[i];
488                     if (p!=NULL)
489                     {
490                         if( (beginning == 1) && count2 < split)
491                         {
492                             newstr[i] = p;
493                             oldstr[i] = NULL;
494                         }
495                         if( (beginning == 0) && count2 >= split) 
496                         {
497                             newstr[i] = p;
498                             oldstr[i] = NULL;
499                         }
500                         count2++;
501                     }
502                 }
503             }
504         }
505     }while( (fabs(maxgrad - meangrad) > DGRADCURVE ) && (count2 >= 6));
506 
507     count = 0;
508     for (ptr =dispstr; ptr !=NULL; ptr = ptr->next)
509     {
510        p = oldstr[count];
511        oldp = newstr[count]; 
512        if (oldp!=NULL) rem = dd_ref_addtostart(rem, (void *) oldp, VEC3); 
513        if (p!=NULL) mer = dd_ref_addtostart(mer, (void *) p, VEC3); 
514        count++;
515     }
516     list_rm(dispstr, NULL);
517     pvector_free(oldstr,0);
518     pvector_free(newstr,0);
519     *remainder = rem;
520     return(mer);
521 } 
522 
523 Tstring *es_build_string3_rect(Tstring * curv, Tstring * es)
524 /* string of matched points in disparity space from rectified edges filtered by gradient */
525 /* take the longest section beyond the disparity discontinuity */
526 /* for segmented curves only NAT 25/10/08 */
527 {
528     List *dptr;
529     List *dispstr = NULL, *dispstr2=NULL, *remainder=NULL;
530     Tstring *tstr;
531 
532     if (curv == NULL || es == NULL)
533         return (NULL);
534 
535     for (dptr = es->start; dptr!= NULL; dptr = dptr->next)
536     {
537         Vec3   *p;
538         Edgel *edge1, *edge2;
539         Match *match1;
540 
541         edge1 = dptr->to;
542         /* get the edgel that it matched to */
543         match1 = (Match *)prop_get(edge1->props, MATCH);
544         if (match1 != NULL )
545         {
546             edge2 = (Edgel *)match1->to2;
547             p = disparity_vec(edge1, edge2);
548             dispstr = dd_ref_addtostart(dispstr, (void *) p, VEC3);
549         }
550         if (dptr == es->end)
551            break;
552     }
553     dispstr = es_dgrad_filter(dispstr, &remainder); 
554     if (remainder!=NULL)
555     {
556         dispstr2 = remainder;
557         remainder = NULL;
558         dispstr2 = es_dgrad_filter(dispstr2, &remainder);
559         list_rm(remainder,vec3_free);
560     }
561     if (dispstr == NULL)
562     {
563         if (dispstr2 !=NULL)
564         {
565             dispstr = dispstr2;
566             dispstr2 = NULL;
567         }
568         else
569             return(NULL);
570     }
571 
572     if (dispstr == NULL)
573         return (NULL);
574 
575     tstr =str_make(STRING, dispstr, dd_get_end(dispstr));
576     if (dispstr2 !=NULL)
577          tstr->props =  proplist_add(tstr->props, 
578                        (void *)str_make(STRING, dispstr2, dd_get_end(dispstr2)), EXTRADATA, NULL);
579     return (tstr);
580 }
581 
582 Tstring *es_build_string4(Tstring * curv, Tstring * es)
583 /* approximated geometrical string (not always edge) */
584 /* associated edge string */
585 {
586     List   *mlist;
587     List   *mptr;
588     List *dispstr = NULL;
589     List *start;
590     List *end;
591     Rindex *rx;
592 
593     if (curv == NULL || es == NULL)
594         return (NULL);
595 
596     /* get list of matches unique in ->to2  ordered along list */
597     mlist = es_get_list_of_matches(es);
598     rx = string_index_get(curv);/* index string */
599 
600     start = curv->start;
601     end = curv->end;
602 
603     for (mptr = mlist; mptr != NULL; mptr = mptr->next)
604     {
605         List *dptr;
606         List *closest;
607         Tstring *s1, *s2;
608         Vec2    p = {Vec2_id};
609 
610         s1 = (Tstring *) ((Match *) mptr->to)->to1;
611         s2 = (Tstring *) ((Match *) mptr->to)->to2;
612 
613         p = DD_EDGE_POS(s1->start);
614         closest = closest_in_index(rx, p);
615         if (closest == NULL)
616             closest = str_get_min(curv, dist_to_pos2, (void *) &p);     /* slow */
617 
618         for (dptr = s2->start;; dptr = dptr->next)
619         {
620             Edgel  *edge = DD_EDGE(dptr);
621             Vec3   *p3;
622             Vec4   *p4;
623 
624             p3 = cv_approx3(closest, edge->pos, start, end);
625             if (p3 != NULL)
626             {
627                 p4 = vec4_alloc();
628                 *(Vec3 *) p4 = *p3;
629                 vec4_w(*p4) = edge->orient;
630                 dispstr = dd_ref_addtostart(dispstr, (void *) p4, VEC4);
631                 rfree((void *) p3);
632             }
633             if (dptr == s2->end)
634                 break;
635         }
636     }
637     rx_free_links(rx);          /* free string index */
638     if (dispstr == NULL)
639         return (NULL);
640     return (str_make(STRING, dispstr, dd_get_end(dispstr)));
641 }
642 

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