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

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

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