1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
8 //
9 //
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
15 //
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
18 //
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
21 //
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
25 //
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
28 //
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
39 //
40 //M*/
41
42 #include "_cvaux.h"
43
44 #define PATH_TO_E 1
45 #define PATH_TO_SE 2
46 #define PATH_TO_S 3
47
48 #define K_S 2
49 #define E_S 2
50 #define C_S .01
51 #define K_Z 5000
52 #define K_NM 50000
53 #define K_B 40
54 #define NULL_EDGE 0.001f
55 #define inf DBL_MAX
56
57 typedef struct __CvWork
58 {
59 double w_east;
60 double w_southeast;
61 double w_south;
62 char path_e;
63 char path_se;
64 char path_s;
65 }_CvWork;
66
67
68 double _cvBendingWork( CvPoint2D32f* B0,
69 CvPoint2D32f* F0,
70 CvPoint2D32f* B1,
71 CvPoint2D32f* F1/*,
72 CvPoint* K */);
73
74 double _cvStretchingWork(CvPoint2D32f* P1,
75 CvPoint2D32f* P2);
76
77 void _cvWorkEast (int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2);
78 void _cvWorkSouthEast(int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2);
79 void _cvWorkSouth (int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2);
80
81 static CvPoint2D32f null_edge = {0,0};
82
_cvStretchingWork(CvPoint2D32f * P1,CvPoint2D32f * P2)83 double _cvStretchingWork(CvPoint2D32f* P1,
84 CvPoint2D32f* P2)
85 {
86 double L1,L2, L_min, dL;
87
88 L1 = sqrt( (double)P1->x*P1->x + P1->y*P1->y);
89 L2 = sqrt( (double)P2->x*P2->x + P2->y*P2->y);
90
91 L_min = MIN(L1, L2);
92 dL = fabs( L1 - L2 );
93
94 return K_S * pow( dL, E_S ) / ( L_min + C_S*dL );
95 }
96
97
98 ////////////////////////////////////////////////////////////////////////////////////
_cvBendingWork(CvPoint2D32f * B0,CvPoint2D32f * F0,CvPoint2D32f * B1,CvPoint2D32f * F1)99 double _cvBendingWork( CvPoint2D32f* B0,
100 CvPoint2D32f* F0,
101 CvPoint2D32f* B1,
102 CvPoint2D32f* F1/*,
103 CvPoint* K*/)
104 {
105 CvPoint2D32f Q( CvPoint2D32f q0, CvPoint2D32f q1, CvPoint2D32f q2, double t );
106 double angle( CvPoint2D32f A, CvPoint2D32f B );
107
108 CvPoint2D32f Q0, Q1, Q2;
109 CvPoint2D32f Q1_nm = { 0, 0 }, Q2_nm = { 0, 0 };
110 double d0, d1, d2, des, t_zero;
111 double k_zero, k_nonmon;
112 CvPoint2D32f center;
113 double check01, check02;
114 char check_origin;
115 double d_angle, d_nm_angle;
116 /*
117 if( (B0->x==0) && (B0->y==0) )
118 {
119 if( (F0->x==0) && (F0->y==0) )
120 {
121 B1->x = -B1->x;
122 B1->y = -B1->y;
123
124 d_angle = acos( (B1->x*F1->x + B1->y*F1->y)/sqrt( (B1->x*B1->x + B1->y*B1->y)*(F1->x*F1->x + F1->y*F1->y) ) );
125 d_angle = CV_PI - d_angle;
126
127 B1->x = -B1->x;
128 B1->y = -B1->y;
129
130 //return d_angle*K_B;
131 return 100;
132 }
133 K->x = -K->x;
134 K->y = -K->y;
135 B1->x = -B1->x;
136 B1->y = -B1->y;
137
138 d_angle = acos( (B1->x*F1->x + B1->y*F1->y)/sqrt( (B1->x*B1->x + B1->y*B1->y)*(F1->x*F1->x + F1->y*F1->y) ) );
139 d_angle = d_angle - acos( (F0->x*K->x + F0->y*K->y)/sqrt( (F0->x*F0->x + F0->y*F0->y)*(K->x*K->x + K->y*K->y) ) );
140 d_angle = d_angle - CV_PI*0.5;
141 d_angle = fabs(d_angle);
142
143
144 K->x = -K->x;
145 K->y = -K->y;
146 B1->x = -B1->x;
147 B1->y = -B1->y;
148
149 //return d_angle*K_B;
150 return 100;
151 }
152
153
154 if( (F0->x==0) && (F0->y==0) )
155 {
156 K->x = -K->x;
157 K->y = -K->y;
158 B1->x = -B1->x;
159 B1->y = -B1->y;
160
161 d_angle = acos( (B1->x*F1->x + B1->y*F1->y)/sqrt( (B1->x*B1->x + B1->y*B1->y)*(F1->x*F1->x + F1->y*F1->y) ) );
162 d_angle = d_angle - acos( (B0->x*K->x + B0->y*K->y)/sqrt( (B0->x*B0->x + B0->y*B0->y)*(K->x*K->x + K->y*K->y) ) );
163 d_angle = d_angle - CV_PI*0.5;
164 d_angle = fabs(d_angle);
165
166 K->x = -K->x;
167 K->y = -K->y;
168 B1->x = -B1->x;
169 B1->y = -B1->y;
170
171 //return d_angle*K_B;
172 return 100;
173 }
174 ///////////////
175
176 if( (B1->x==0) && (B1->y==0) )
177 {
178 if( (F1->x==0) && (F1->y==0) )
179 {
180 B0->x = -B0->x;
181 B0->y = -B0->y;
182
183 d_angle = acos( (B0->x*F0->x + B0->y*F0->y)/sqrt( (B0->x*B0->x + B0->y*B0->y)*(F0->x*F0->x + F0->y*F0->y) ) );
184 d_angle = CV_PI - d_angle;
185
186 B0->x = -B0->x;
187 B0->y = -B0->y;
188
189 //return d_angle*K_B;
190 return 100;
191 }
192 K->x = -K->x;
193 K->y = -K->y;
194 B0->x = -B0->x;
195 B0->y = -B0->y;
196
197 d_angle = acos( (B0->x*F0->x + B0->y*F0->y)/sqrt( (B0->x*B0->x + B0->y*B0->y)*(F0->x*F0->x + F0->y*F0->y) ) );
198 d_angle = d_angle - acos( (F1->x*K->x + F1->y*K->y)/sqrt( (F1->x*F1->x + F1->y*F1->y)*(K->x*K->x + K->y*K->y) ) );
199 d_angle = d_angle - CV_PI*0.5;
200 d_angle = fabs(d_angle);
201
202 K->x = -K->x;
203 K->y = -K->y;
204 B0->x = -B0->x;
205 B0->y = -B0->y;
206
207 //return d_angle*K_B;
208 return 100;
209 }
210
211
212 if( (F1->x==0) && (F1->y==0) )
213 {
214 K->x = -K->x;
215 K->y = -K->y;
216 B0->x = -B0->x;
217 B0->y = -B0->y;
218
219 d_angle = acos( (B0->x*F0->x + B0->y*F0->y)/sqrt( (B0->x*B0->x + B0->y*B0->y)*(F0->x*F0->x + F0->y*F0->y) ) );
220 d_angle = d_angle - acos( (B1->x*K->x + B1->y*K->y)/sqrt( (B1->x*B1->x + B1->y*B1->y)*(K->x*K->x + K->y*K->y) ) );
221 d_angle = d_angle - CV_PI*0.5;
222 d_angle = fabs(d_angle);
223
224 K->x = -K->x;
225 K->y = -K->y;
226 B0->x = -B0->x;
227 B0->y = -B0->y;
228
229 //return d_angle*K_B;
230 return 100;
231 }
232
233 */
234
235 /*
236 B0->x = -B0->x;
237 B0->y = -B0->y;
238 B1->x = -B1->x;
239 B1->y = -B1->y;
240 */
241 Q0.x = F0->x * (-B0->x) + F0->y * (-B0->y);
242 Q0.y = F0->x * (-B0->y) - F0->y * (-B0->x);
243
244 Q1.x = 0.5f*( (F1->x * (-B0->x) + F1->y * (-B0->y)) + (F0->x * (-B1->x) + F0->y * (-B1->y)) );
245 Q1.y = 0.5f*( (F1->x * (-B0->y) - F1->y * (-B0->x)) + (F0->x * (-B1->y) - F0->y * (-B1->x)) );
246
247 Q2.x = F1->x * (-B1->x) + F1->y * (-B1->y);
248 Q2.y = F1->x * (-B1->y) - F1->y * (-B1->x);
249
250 d0 = Q0.x * Q1.y - Q0.y * Q1.x;
251 d1 = 0.5f*(Q0.x * Q2.y - Q0.y * Q2.x);
252 d2 = Q1.x * Q2.y - Q1.y * Q2.x;
253
254 // Check angles goes to zero
255 des = Q1.y*Q1.y - Q0.y*Q2.y;
256
257 k_zero = 0;
258
259 if( des >= 0 )
260 {
261 t_zero = ( Q0.y - Q1.y + sqrt(des) )/( Q0.y - 2*Q1.y + Q2.y );
262
263 if( (0 < t_zero) && (t_zero < 1) && ( Q(Q0, Q1, Q2, t_zero).x > 0 ) )
264 {
265 k_zero = inf;
266 }
267
268 t_zero = ( Q0.y - Q1.y - sqrt(des) )/( Q0.y - 2*Q1.y + Q2.y );
269
270 if( (0 < t_zero) && (t_zero < 1) && ( Q(Q0, Q1, Q2, t_zero).x > 0 ) )
271 {
272 k_zero = inf;
273 }
274 }
275
276 // Check nonmonotonic
277 des = d1*d1 - d0*d2;
278
279 k_nonmon = 0;
280
281 if( des >= 0 )
282 {
283 t_zero = ( d0 - d1 - sqrt(des) )/( d0 - 2*d1 + d2 );
284
285 if( (0 < t_zero) && (t_zero < 1) )
286 {
287 k_nonmon = 1;
288 Q1_nm = Q(Q0, Q1, Q2, t_zero);
289 }
290
291 t_zero = ( d0 - d1 + sqrt(des) )/( d0 - 2*d1 + d2 );
292
293 if( (0 < t_zero) && (t_zero < 1) )
294 {
295 k_nonmon += 2;
296 Q2_nm = Q(Q0, Q1, Q2, t_zero);
297 }
298 }
299
300 // Finde origin lie in Q0Q1Q2
301 check_origin = 1;
302
303 center.x = (Q0.x + Q1.x + Q2.x)/3;
304 center.y = (Q0.y + Q1.y + Q2.y)/3;
305
306 check01 = (center.x - Q0.x)*(Q1.y - Q0.y) + (center.y - Q0.y)*(Q1.x - Q0.x);
307 check02 = (-Q0.x)*(Q1.y - Q0.y) + (-Q0.y)*(Q1.x - Q0.x);
308 if( check01*check02 > 0 )
309 {
310 check01 = (center.x - Q1.x)*(Q2.y - Q1.y) + (center.y - Q1.y)*(Q2.x - Q1.x);
311 check02 = (-Q1.x)*(Q2.y - Q1.y) + (-Q1.y)*(Q2.x - Q1.x);
312 if( check01*check02 > 0 )
313 {
314 check01 = (center.x - Q2.x)*(Q0.y - Q2.y) + (center.y - Q2.y)*(Q0.x - Q2.x);
315 check02 = (-Q2.x)*(Q0.y - Q2.y) + (-Q2.y)*(Q0.x - Q2.x);
316 if( check01*check02 > 0 )
317 {
318 check_origin = 0;
319 }
320 }
321 }
322
323 // Calculate angle
324 d_nm_angle = 0;
325 d_angle = angle(Q0,Q2);
326 if( k_nonmon == 0 )
327 {
328 if( check_origin == 0 )
329 {
330 }
331 else
332 {
333 d_angle = 2*CV_PI - d_angle;
334 }
335 }
336 else
337 {
338 if( k_nonmon == 1 )
339 {
340 d_nm_angle = angle(Q0,Q1_nm);
341 if(d_nm_angle > d_angle)
342 {
343 d_nm_angle = d_nm_angle - d_angle;
344 }
345 }
346
347 if( k_nonmon == 2 )
348 {
349 d_nm_angle = angle(Q0,Q2_nm);
350 if(d_nm_angle > d_angle)
351 {
352 d_nm_angle = d_nm_angle - d_angle;
353 }
354 }
355
356 if( k_nonmon == 3 )
357 {
358 d_nm_angle = angle(Q0,Q1_nm);
359 if(d_nm_angle > d_angle)
360 {
361 d_nm_angle = d_nm_angle - d_angle;
362 d_nm_angle = d_nm_angle + angle(Q0, Q2_nm);
363 }
364 else
365 {
366 d_nm_angle = d_nm_angle + angle(Q2,Q2_nm);
367 }
368 }
369 }
370 /*
371 B0->x = -B0->x;
372 B0->y = -B0->y;
373 B1->x = -B1->x;
374 B1->y = -B1->y;
375 */
376 return d_angle*K_B + d_nm_angle*K_NM + k_zero*K_Z;
377 //return 0;
378 }
379
380
381 /////////////////////////////////////////////////////////////////////////////////
_cvWorkEast(int i,int j,_CvWork ** W,CvPoint2D32f * edges1,CvPoint2D32f * edges2)382 void _cvWorkEast(int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2)
383 {
384 double w1,w2;
385 CvPoint2D32f small_edge;
386
387 //W[i,j].w_east
388 w1 = W[i-1][j].w_east /*+ _cvBendingWork( &edges1[i-2],
389 &edges1[i-1],
390 &null_edge ,
391 &null_edge,
392 NULL)*/;
393
394 small_edge.x = NULL_EDGE*edges1[i-1].x;
395 small_edge.y = NULL_EDGE*edges1[i-1].y;
396
397 w2 = W[i-1][j].w_southeast + _cvBendingWork(&edges1[i-2],
398 &edges1[i-1],
399 &edges2[j-1],
400 /*&null_edge*/&small_edge/*,
401 &edges2[j]*/);
402
403 if(w1<w2)
404 {
405 W[i][j].w_east = w1 + _cvStretchingWork( &edges1[i-1], &null_edge );
406 W[i][j].path_e = PATH_TO_E;
407 }
408 else
409 {
410 W[i][j].w_east = w2 + _cvStretchingWork( &edges1[i-1], &null_edge );
411 W[i][j].path_e = PATH_TO_SE;
412 }
413 }
414
415
416
417
418
419 ////////////////////////////////////////////////////////////////////////////////////
_cvWorkSouthEast(int i,int j,_CvWork ** W,CvPoint2D32f * edges1,CvPoint2D32f * edges2)420 void _cvWorkSouthEast(int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2)
421 {
422 double w1,w2,w3;
423 CvPoint2D32f small_edge;
424
425 //W[i,j].w_southeast
426 small_edge.x = NULL_EDGE*edges1[i-2].x;
427 small_edge.y = NULL_EDGE*edges1[i-2].y;
428
429 w1 = W[i-1][j-1].w_east + _cvBendingWork(&edges1[i-2],
430 &edges1[i-1],
431 /*&null_edge*/&small_edge,
432 &edges2[j-1]/*,
433 &edges2[j-2]*/);
434
435 w2 = W[i-1][j-1].w_southeast + _cvBendingWork( &edges1[i-2],
436 &edges1[i-1],
437 &edges2[j-2],
438 &edges2[j-1]/*,
439 NULL*/);
440
441 small_edge.x = NULL_EDGE*edges2[j-2].x;
442 small_edge.y = NULL_EDGE*edges2[j-2].y;
443
444 w3 = W[i-1][j-1].w_south + _cvBendingWork( /*&null_edge*/&small_edge,
445 &edges1[i-1],
446 &edges2[j-2],
447 &edges2[j-1]/*,
448 &edges1[i-2]*/);
449
450 if( w1<w2 )
451 {
452 if(w1<w3)
453 {
454 W[i][j].w_southeast = w1 + _cvStretchingWork( &edges1[i-1], &edges2[j-1] );
455 W[i][j].path_se = PATH_TO_E;
456 }
457 else
458 {
459 W[i][j].w_southeast = w3 + _cvStretchingWork( &edges1[i-1], &edges2[j-1] );
460 W[i][j].path_se = 3;
461 }
462 }
463 else
464 {
465 if( w2<w3)
466 {
467 W[i][j].w_southeast = w2 + _cvStretchingWork( &edges1[i-1], &edges2[j-1] );
468 W[i][j].path_se = PATH_TO_SE;
469 }
470 else
471 {
472 W[i][j].w_southeast = w3 + _cvStretchingWork( &edges1[i-1], &edges2[j-1] );
473 W[i][j].path_se = 3;
474 }
475 }
476 }
477
478
479 //////////////////////////////////////////////////////////////////////////////////////
_cvWorkSouth(int i,int j,_CvWork ** W,CvPoint2D32f * edges1,CvPoint2D32f * edges2)480 void _cvWorkSouth(int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2)
481 {
482 double w1,w2;
483 CvPoint2D32f small_edge;
484
485 //W[i,j].w_south
486
487 small_edge.x = NULL_EDGE*edges2[j-1].x;
488 small_edge.y = NULL_EDGE*edges2[j-1].y;
489
490 w1 = W[i][j-1].w_southeast + _cvBendingWork(&edges1[i-1],
491 /*&null_edge*/&small_edge,
492 &edges2[j-2],
493 &edges2[j-1]/*,
494 &edges1[i]*/);
495
496 w2 = W[i][j-1].w_south /*+ _cvBendingWork( &null_edge ,
497 &null_edge,
498 &edges2[j-2],
499 &edges2[j-1],
500 NULL)*/;
501
502 if( w1<w2 )
503 {
504 W[i][j].w_south = w1 + _cvStretchingWork( &null_edge, &edges2[j-1] );
505 W[i][j].path_s = PATH_TO_SE;
506 }
507 else
508 {
509 W[i][j].w_south = w2 + _cvStretchingWork( &null_edge, &edges2[j-1] );
510 W[i][j].path_s = 3;
511 }
512 }
513
514 //===================================================
Q(CvPoint2D32f q0,CvPoint2D32f q1,CvPoint2D32f q2,double t)515 CvPoint2D32f Q(CvPoint2D32f q0,CvPoint2D32f q1,CvPoint2D32f q2,double t)
516 {
517 CvPoint2D32f q;
518
519 q.x = (float)(q0.x*(1-t)*(1-t) + 2*q1.x*t*(1-t) + q2.x*t*t);
520 q.y = (float)(q0.y*(1-t)*(1-t) + 2*q1.y*t*(1-t) + q2.y*t*t);
521
522 return q;
523 }
524
angle(CvPoint2D32f A,CvPoint2D32f B)525 double angle(CvPoint2D32f A, CvPoint2D32f B)
526 {
527 return acos( (A.x*B.x + A.y*B.y)/sqrt( (double)(A.x*A.x + A.y*A.y)*(B.x*B.x + B.y*B.y) ) );
528 }
529
530 /***************************************************************************************\
531 *
532 * This function compute intermediate polygon between contour1 and contour2
533 *
534 * Correspondence between points of contours specify by corr
535 *
536 * param = [0,1]; 0 correspondence to contour1, 1 - contour2
537 *
538 \***************************************************************************************/
icvBlendContours(CvSeq * contour1,CvSeq * contour2,CvSeq * corr,double param,CvMemStorage * storage)539 CvSeq* icvBlendContours(CvSeq* contour1,
540 CvSeq* contour2,
541 CvSeq* corr,
542 double param,
543 CvMemStorage* storage)
544 {
545 int j;
546
547 CvSeqWriter writer01;
548 CvSeqReader reader01;
549
550 int Ni,Nj; // size of contours
551 int i; // counter
552
553 CvPoint* point1; // array of first contour point
554 CvPoint* point2; // array of second contour point
555
556 CvPoint point_output; // intermediate storage of ouput point
557
558 int corr_point;
559
560 // Create output sequence.
561 CvSeq* output = cvCreateSeq(0,
562 sizeof(CvSeq),
563 sizeof(CvPoint),
564 storage );
565
566 // Find size of contours.
567 Ni = contour1->total + 1;
568 Nj = contour2->total + 1;
569
570 point1 = (CvPoint* )malloc( Ni*sizeof(CvPoint) );
571 point2 = (CvPoint* )malloc( Nj*sizeof(CvPoint) );
572
573 // Initialize arrays of point
574 cvCvtSeqToArray( contour1, point1, CV_WHOLE_SEQ );
575 cvCvtSeqToArray( contour2, point2, CV_WHOLE_SEQ );
576
577 // First and last point mast be equal.
578 point1[Ni-1] = point1[0];
579 point2[Nj-1] = point2[0];
580
581 // Initializes process of writing to sequence.
582 cvStartAppendToSeq( output, &writer01);
583
584 i = Ni-1; //correspondence to points of contour1
585 for( ; corr; corr = corr->h_next )
586 {
587 //Initializes process of sequential reading from sequence
588 cvStartReadSeq( corr, &reader01, 0 );
589
590 for(j=0; j < corr->total; j++)
591 {
592 // Read element from sequence.
593 CV_READ_SEQ_ELEM( corr_point, reader01 );
594
595 // Compute point of intermediate polygon.
596 point_output.x = cvRound(point1[i].x + param*( point2[corr_point].x - point1[i].x ));
597 point_output.y = cvRound(point1[i].y + param*( point2[corr_point].y - point1[i].y ));
598
599 // Write element to sequence.
600 CV_WRITE_SEQ_ELEM( point_output, writer01 );
601 }
602 i--;
603 }
604 // Updates sequence header.
605 cvFlushSeqWriter( &writer01 );
606
607 return output;
608 }
609
610 /**************************************************************************************************
611 *
612 *
613 *
614 *
615 *
616 *
617 *
618 *
619 *
620 *
621 **************************************************************************************************/
622
623
icvCalcContoursCorrespondence(CvSeq * contour1,CvSeq * contour2,CvSeq ** corr,CvMemStorage * storage)624 void icvCalcContoursCorrespondence(CvSeq* contour1,
625 CvSeq* contour2,
626 CvSeq** corr,
627 CvMemStorage* storage)
628 {
629 int i,j; // counter of cycles
630 int Ni,Nj; // size of contours
631 _CvWork** W; // graph for search minimum of work
632
633 CvPoint* point1; // array of first contour point
634 CvPoint* point2; // array of second contour point
635 CvPoint2D32f* edges1; // array of first contour edge
636 CvPoint2D32f* edges2; // array of second contour edge
637
638 //CvPoint null_edge = {0,0}; //
639 CvPoint2D32f small_edge;
640 //double inf; // infinity
641
642 CvSeq* corr01;
643 CvSeqWriter writer;
644
645 char path; //
646
647 // Find size of contours
648 Ni = contour1->total + 1;
649 Nj = contour2->total + 1;
650
651 // Create arrays
652 W = (_CvWork**)malloc(sizeof(_CvWork*)*Ni);
653 for(i=0; i<Ni; i++)
654 {
655 W[i] = (_CvWork*)malloc(sizeof(_CvWork)*Nj);
656 }
657
658 point1 = (CvPoint* )malloc( Ni*sizeof(CvPoint) );
659 point2 = (CvPoint* )malloc( Nj*sizeof(CvPoint) );
660 edges1 = (CvPoint2D32f* )malloc( (Ni-1)*sizeof(CvPoint2D32f) );
661 edges2 = (CvPoint2D32f* )malloc( (Nj-1)*sizeof(CvPoint2D32f) );
662
663 // Initialize arrays of point
664 cvCvtSeqToArray( contour1, point1, CV_WHOLE_SEQ );
665 cvCvtSeqToArray( contour2, point2, CV_WHOLE_SEQ );
666
667 point1[Ni-1] = point1[0];
668 point2[Nj-1] = point2[0];
669
670 for(i=0;i<Ni-1;i++)
671 {
672 edges1[i].x = (float)( point1[i+1].x - point1[i].x );
673 edges1[i].y = (float)( point1[i+1].y - point1[i].y );
674 };
675
676 for(i=0;i<Nj-1;i++)
677 {
678 edges2[i].x = (float)( point2[i+1].x - point2[i].x );
679 edges2[i].y = (float)( point2[i+1].y - point2[i].y );
680 };
681
682 // Find infinity constant
683 //inf=1;
684 /////////////
685
686 //Find min path in graph
687
688 /////////////
689 W[0][0].w_east = 0;
690 W[0][0].w_south = 0;
691 W[0][0].w_southeast = 0;
692
693 W[1][1].w_southeast = _cvStretchingWork( &edges1[0], &edges2[0] );
694 W[1][1].w_east = inf;
695 W[1][1].w_south = inf;
696 W[1][1].path_se = PATH_TO_SE;
697
698 W[0][1].w_south = _cvStretchingWork( &null_edge, &edges2[0] );
699 W[0][1].path_s = 3;
700 W[1][0].w_east = _cvStretchingWork( &edges2[0], &null_edge );
701 W[1][0].path_e = PATH_TO_E;
702
703 for( i=1; i<Ni; i++ )
704 {
705 W[i][0].w_south = inf;
706 W[i][0].w_southeast = inf;
707 }
708
709 for(j=1; j<Nj; j++)
710 {
711 W[0][j].w_east = inf;
712 W[0][j].w_southeast = inf;
713 }
714
715 for(i=2; i<Ni; i++)
716 {
717 j=0;/////////
718 W[i][j].w_east = W[i-1][j].w_east;
719 W[i][j].w_east = W[i][j].w_east /*+
720 _cvBendingWork( &edges1[i-2], &edges1[i-1], &null_edge, &null_edge, NULL )*/;
721 W[i][j].w_east = W[i][j].w_east + _cvStretchingWork( &edges2[i-1], &null_edge );
722 W[i][j].path_e = PATH_TO_E;
723
724 j=1;//////////
725 W[i][j].w_south = inf;
726
727 _cvWorkEast (i, j, W, edges1, edges2);
728
729 W[i][j].w_southeast = W[i-1][j-1].w_east;
730 W[i][j].w_southeast = W[i][j].w_southeast + _cvStretchingWork( &edges1[i-1], &edges2[j-1] );
731
732 small_edge.x = NULL_EDGE*edges1[i-2].x;
733 small_edge.y = NULL_EDGE*edges1[i-2].y;
734
735 W[i][j].w_southeast = W[i][j].w_southeast +
736 _cvBendingWork( &edges1[i-2], &edges1[i-1], /*&null_edge*/&small_edge, &edges2[j-1]/*, &edges2[Nj-2]*/);
737
738 W[i][j].path_se = PATH_TO_E;
739 }
740
741 for(j=2; j<Nj; j++)
742 {
743 i=0;//////////
744 W[i][j].w_south = W[i][j-1].w_south;
745 W[i][j].w_south = W[i][j].w_south + _cvStretchingWork( &null_edge, &edges2[j-1] );
746 W[i][j].w_south = W[i][j].w_south /*+
747 _cvBendingWork( &null_edge, &null_edge, &edges2[j-2], &edges2[j-1], NULL )*/;
748 W[i][j].path_s = 3;
749
750 i=1;///////////
751 W[i][j].w_east= inf;
752
753 _cvWorkSouth(i, j, W, edges1, edges2);
754
755 W[i][j].w_southeast = W[i-1][j-1].w_south;
756 W[i][j].w_southeast = W[i][j].w_southeast + _cvStretchingWork( &edges1[i-1], &edges2[j-1] );
757
758 small_edge.x = NULL_EDGE*edges2[j-2].x;
759 small_edge.y = NULL_EDGE*edges2[j-2].y;
760
761 W[i][j].w_southeast = W[i][j].w_southeast +
762 _cvBendingWork( /*&null_edge*/&small_edge, &edges1[i-1], &edges2[j-2], &edges2[j-1]/*, &edges1[Ni-2]*/);
763 W[i][j].path_se = 3;
764 }
765
766 for(i=2; i<Ni; i++)
767 for(j=2; j<Nj; j++)
768 {
769 _cvWorkEast (i, j, W, edges1, edges2);
770 _cvWorkSouthEast(i, j, W, edges1, edges2);
771 _cvWorkSouth (i, j, W, edges1, edges2);
772 }
773
774 i=Ni-1;j=Nj-1;
775
776 *corr = cvCreateSeq(0,
777 sizeof(CvSeq),
778 sizeof(int),
779 storage );
780
781 corr01 = *corr;
782 cvStartAppendToSeq( corr01, &writer );
783 if( W[i][j].w_east > W[i][j].w_southeast )
784 {
785 if( W[i][j].w_southeast > W[i][j].w_south )
786 {
787 path = 3;
788 }
789 else
790 {
791 path = PATH_TO_SE;
792 }
793 }
794 else
795 {
796 if( W[i][j].w_east < W[i][j].w_south )
797 {
798 path = PATH_TO_E;
799 }
800 else
801 {
802 path = 3;
803 }
804 }
805 do
806 {
807 CV_WRITE_SEQ_ELEM( j, writer );
808
809 switch( path )
810 {
811 case PATH_TO_E:
812 path = W[i][j].path_e;
813 i--;
814 cvFlushSeqWriter( &writer );
815 corr01->h_next = cvCreateSeq( 0,
816 sizeof(CvSeq),
817 sizeof(int),
818 storage );
819 corr01 = corr01->h_next;
820 cvStartAppendToSeq( corr01, &writer );
821 break;
822
823 case PATH_TO_SE:
824 path = W[i][j].path_se;
825 j--; i--;
826 cvFlushSeqWriter( &writer );
827 corr01->h_next = cvCreateSeq( 0,
828 sizeof(CvSeq),
829 sizeof(int),
830 storage );
831 corr01 = corr01->h_next;
832 cvStartAppendToSeq( corr01, &writer );
833 break;
834
835 case 3:
836 path = W[i][j].path_s;
837 j--;
838 break;
839 }
840
841 } while( (i>=0) && (j>=0) );
842 cvFlushSeqWriter( &writer );
843
844 // Free memory
845 for(i=1;i<Ni;i++)
846 {
847 free(W[i]);
848 }
849 free(W);
850 free(point1);
851 free(point2);
852 free(edges1);
853 free(edges2);
854 }
855
856