1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
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.
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
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.
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.
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.
42 #include "precomp.hpp"
54 #define NULL_EDGE 0.001f
57 typedef struct __CvWork
68 double _cvBendingWork( CvPoint2D32f* B0,
74 double _cvStretchingWork(CvPoint2D32f* P1,
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);
81 static CvPoint2D32f null_edge;
83 double _cvStretchingWork(CvPoint2D32f* P1,
86 double L1,L2, L_min, dL;
88 L1 = sqrt( (double)P1->x*P1->x + P1->y*P1->y);
89 L2 = sqrt( (double)P2->x*P2->x + P2->y*P2->y);
94 return K_S * pow( dL, E_S ) / ( L_min + C_S*dL );
98 ////////////////////////////////////////////////////////////////////////////////////
99 CvPoint2D32f Q( CvPoint2D32f q0, CvPoint2D32f q1, CvPoint2D32f q2, double t );
100 double angle( CvPoint2D32f A, CvPoint2D32f B );
102 double _cvBendingWork( CvPoint2D32f* B0,
108 CvPoint2D32f Q0, Q1, Q2;
109 CvPoint2D32f Q1_nm, Q2_nm;
110 double d0, d1, d2, des, t_zero;
111 double k_zero, k_nonmon;
113 double check01, check02;
115 double d_angle, d_nm_angle;
117 if( (B0->x==0) && (B0->y==0) )
119 if( (F0->x==0) && (F0->y==0) )
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;
130 //return d_angle*K_B;
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);
149 //return d_angle*K_B;
154 if( (F0->x==0) && (F0->y==0) )
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);
171 //return d_angle*K_B;
176 if( (B1->x==0) && (B1->y==0) )
178 if( (F1->x==0) && (F1->y==0) )
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;
189 //return d_angle*K_B;
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);
207 //return d_angle*K_B;
212 if( (F1->x==0) && (F1->y==0) )
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);
229 //return d_angle*K_B;
241 Q0.x = F0->x * (-B0->x) + F0->y * (-B0->y);
242 Q0.y = F0->x * (-B0->y) - F0->y * (-B0->x);
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)) );
247 Q2.x = F1->x * (-B1->x) + F1->y * (-B1->y);
248 Q2.y = F1->x * (-B1->y) - F1->y * (-B1->x);
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;
254 // Check angles goes to zero
255 des = Q1.y*Q1.y - Q0.y*Q2.y;
261 t_zero = ( Q0.y - Q1.y + sqrt(des) )/( Q0.y - 2*Q1.y + Q2.y );
263 if( (0 < t_zero) && (t_zero < 1) && ( Q(Q0, Q1, Q2, t_zero).x > 0 ) )
268 t_zero = ( Q0.y - Q1.y - sqrt(des) )/( Q0.y - 2*Q1.y + Q2.y );
270 if( (0 < t_zero) && (t_zero < 1) && ( Q(Q0, Q1, Q2, t_zero).x > 0 ) )
276 // Check nonmonotonic
283 t_zero = ( d0 - d1 - sqrt(des) )/( d0 - 2*d1 + d2 );
285 if( (0 < t_zero) && (t_zero < 1) )
288 Q1_nm = Q(Q0, Q1, Q2, t_zero);
291 t_zero = ( d0 - d1 + sqrt(des) )/( d0 - 2*d1 + d2 );
293 if( (0 < t_zero) && (t_zero < 1) )
296 Q2_nm = Q(Q0, Q1, Q2, t_zero);
300 // Finde origin lie in Q0Q1Q2
303 center.x = (Q0.x + Q1.x + Q2.x)/3;
304 center.y = (Q0.y + Q1.y + Q2.y)/3;
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 )
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 )
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 )
325 d_angle = angle(Q0,Q2);
328 if( check_origin == 0 )
333 d_angle = 2*CV_PI - d_angle;
340 d_nm_angle = angle(Q0,Q1_nm);
341 if(d_nm_angle > d_angle)
343 d_nm_angle = d_nm_angle - d_angle;
349 d_nm_angle = angle(Q0,Q2_nm);
350 if(d_nm_angle > d_angle)
352 d_nm_angle = d_nm_angle - d_angle;
358 d_nm_angle = angle(Q0,Q1_nm);
359 if(d_nm_angle > d_angle)
361 d_nm_angle = d_nm_angle - d_angle;
362 d_nm_angle = d_nm_angle + angle(Q0, Q2_nm);
366 d_nm_angle = d_nm_angle + angle(Q2,Q2_nm);
376 return d_angle*K_B + d_nm_angle*K_NM + k_zero*K_Z;
381 /////////////////////////////////////////////////////////////////////////////////
382 void _cvWorkEast(int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2)
385 CvPoint2D32f small_edge;
388 w1 = W[i-1][j].w_east /*+ _cvBendingWork( &edges1[i-2],
394 small_edge.x = NULL_EDGE*edges1[i-1].x;
395 small_edge.y = NULL_EDGE*edges1[i-1].y;
397 w2 = W[i-1][j].w_southeast + _cvBendingWork(&edges1[i-2],
400 /*&null_edge*/&small_edge/*,
405 W[i][j].w_east = w1 + _cvStretchingWork( &edges1[i-1], &null_edge );
406 W[i][j].path_e = PATH_TO_E;
410 W[i][j].w_east = w2 + _cvStretchingWork( &edges1[i-1], &null_edge );
411 W[i][j].path_e = PATH_TO_SE;
419 ////////////////////////////////////////////////////////////////////////////////////
420 void _cvWorkSouthEast(int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2)
423 CvPoint2D32f small_edge;
426 small_edge.x = NULL_EDGE*edges1[i-2].x;
427 small_edge.y = NULL_EDGE*edges1[i-2].y;
429 w1 = W[i-1][j-1].w_east + _cvBendingWork(&edges1[i-2],
431 /*&null_edge*/&small_edge,
435 w2 = W[i-1][j-1].w_southeast + _cvBendingWork( &edges1[i-2],
441 small_edge.x = NULL_EDGE*edges2[j-2].x;
442 small_edge.y = NULL_EDGE*edges2[j-2].y;
444 w3 = W[i-1][j-1].w_south + _cvBendingWork( /*&null_edge*/&small_edge,
454 W[i][j].w_southeast = w1 + _cvStretchingWork( &edges1[i-1], &edges2[j-1] );
455 W[i][j].path_se = PATH_TO_E;
459 W[i][j].w_southeast = w3 + _cvStretchingWork( &edges1[i-1], &edges2[j-1] );
467 W[i][j].w_southeast = w2 + _cvStretchingWork( &edges1[i-1], &edges2[j-1] );
468 W[i][j].path_se = PATH_TO_SE;
472 W[i][j].w_southeast = w3 + _cvStretchingWork( &edges1[i-1], &edges2[j-1] );
479 //////////////////////////////////////////////////////////////////////////////////////
480 void _cvWorkSouth(int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2)
483 CvPoint2D32f small_edge;
487 small_edge.x = NULL_EDGE*edges2[j-1].x;
488 small_edge.y = NULL_EDGE*edges2[j-1].y;
490 w1 = W[i][j-1].w_southeast + _cvBendingWork(&edges1[i-1],
491 /*&null_edge*/&small_edge,
496 w2 = W[i][j-1].w_south /*+ _cvBendingWork( &null_edge ,
504 W[i][j].w_south = w1 + _cvStretchingWork( &null_edge, &edges2[j-1] );
505 W[i][j].path_s = PATH_TO_SE;
509 W[i][j].w_south = w2 + _cvStretchingWork( &null_edge, &edges2[j-1] );
515 //===================================================
516 CvPoint2D32f Q(CvPoint2D32f q0,CvPoint2D32f q1,CvPoint2D32f q2,double t)
520 q.x = (float)(q0.x*(1-t)*(1-t) + 2*q1.x*t*(1-t) + q2.x*t*t);
521 q.y = (float)(q0.y*(1-t)*(1-t) + 2*q1.y*t*(1-t) + q2.y*t*t);
526 double angle(CvPoint2D32f A, CvPoint2D32f B)
528 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) ) );
531 /***************************************************************************************\
533 * This function compute intermediate polygon between contour1 and contour2
535 * Correspondence between points of contours specify by corr
537 * param = [0,1]; 0 correspondence to contour1, 1 - contour2
539 \***************************************************************************************/
540 static CvSeq* icvBlendContours(CvSeq* contour1,
544 CvMemStorage* storage)
548 CvSeqWriter writer01;
549 CvSeqReader reader01;
551 int Ni,Nj; // size of contours
554 CvPoint* point1; // array of first contour point
555 CvPoint* point2; // array of second contour point
557 CvPoint point_output; // intermediate storage of ouput point
561 // Create output sequence.
562 CvSeq* output = cvCreateSeq(0,
567 // Find size of contours.
568 Ni = contour1->total + 1;
569 Nj = contour2->total + 1;
571 point1 = (CvPoint* )malloc( Ni*sizeof(CvPoint) );
572 point2 = (CvPoint* )malloc( Nj*sizeof(CvPoint) );
574 // Initialize arrays of point
575 cvCvtSeqToArray( contour1, point1, CV_WHOLE_SEQ );
576 cvCvtSeqToArray( contour2, point2, CV_WHOLE_SEQ );
578 // First and last point mast be equal.
579 point1[Ni-1] = point1[0];
580 point2[Nj-1] = point2[0];
582 // Initializes process of writing to sequence.
583 cvStartAppendToSeq( output, &writer01);
585 i = Ni-1; //correspondence to points of contour1
586 for( ; corr; corr = corr->h_next )
588 //Initializes process of sequential reading from sequence
589 cvStartReadSeq( corr, &reader01, 0 );
591 for(j=0; j < corr->total; j++)
593 // Read element from sequence.
594 CV_READ_SEQ_ELEM( corr_point, reader01 );
596 // Compute point of intermediate polygon.
597 point_output.x = cvRound(point1[i].x + param*( point2[corr_point].x - point1[i].x ));
598 point_output.y = cvRound(point1[i].y + param*( point2[corr_point].y - point1[i].y ));
600 // Write element to sequence.
601 CV_WRITE_SEQ_ELEM( point_output, writer01 );
605 // Updates sequence header.
606 cvFlushSeqWriter( &writer01 );
611 /**************************************************************************************************
622 **************************************************************************************************/
625 static void icvCalcContoursCorrespondence(CvSeq* contour1,
628 CvMemStorage* storage)
630 int i,j; // counter of cycles
631 int Ni,Nj; // size of contours
632 _CvWork** W; // graph for search minimum of work
634 CvPoint* point1; // array of first contour point
635 CvPoint* point2; // array of second contour point
636 CvPoint2D32f* edges1; // array of first contour edge
637 CvPoint2D32f* edges2; // array of second contour edge
639 //CvPoint null_edge = {0,0}; //
640 CvPoint2D32f small_edge;
641 //double inf; // infinity
648 // Find size of contours
649 Ni = contour1->total + 1;
650 Nj = contour2->total + 1;
653 W = (_CvWork**)malloc(sizeof(_CvWork*)*Ni);
656 W[i] = (_CvWork*)malloc(sizeof(_CvWork)*Nj);
659 point1 = (CvPoint* )malloc( Ni*sizeof(CvPoint) );
660 point2 = (CvPoint* )malloc( Nj*sizeof(CvPoint) );
661 edges1 = (CvPoint2D32f* )malloc( (Ni-1)*sizeof(CvPoint2D32f) );
662 edges2 = (CvPoint2D32f* )malloc( (Nj-1)*sizeof(CvPoint2D32f) );
664 // Initialize arrays of point
665 cvCvtSeqToArray( contour1, point1, CV_WHOLE_SEQ );
666 cvCvtSeqToArray( contour2, point2, CV_WHOLE_SEQ );
668 point1[Ni-1] = point1[0];
669 point2[Nj-1] = point2[0];
673 edges1[i].x = (float)( point1[i+1].x - point1[i].x );
674 edges1[i].y = (float)( point1[i+1].y - point1[i].y );
679 edges2[i].x = (float)( point2[i+1].x - point2[i].x );
680 edges2[i].y = (float)( point2[i+1].y - point2[i].y );
683 // Find infinity constant
687 //Find min path in graph
692 W[0][0].w_southeast = 0;
694 W[1][1].w_southeast = _cvStretchingWork( &edges1[0], &edges2[0] );
695 W[1][1].w_east = inf;
696 W[1][1].w_south = inf;
697 W[1][1].path_se = PATH_TO_SE;
699 W[0][1].w_south = _cvStretchingWork( &null_edge, &edges2[0] );
701 W[1][0].w_east = _cvStretchingWork( &edges2[0], &null_edge );
702 W[1][0].path_e = PATH_TO_E;
704 for( i=1; i<Ni; i++ )
706 W[i][0].w_south = inf;
707 W[i][0].w_southeast = inf;
712 W[0][j].w_east = inf;
713 W[0][j].w_southeast = inf;
719 W[i][j].w_east = W[i-1][j].w_east;
720 W[i][j].w_east = W[i][j].w_east /*+
721 _cvBendingWork( &edges1[i-2], &edges1[i-1], &null_edge, &null_edge, NULL )*/;
722 W[i][j].w_east = W[i][j].w_east + _cvStretchingWork( &edges2[i-1], &null_edge );
723 W[i][j].path_e = PATH_TO_E;
726 W[i][j].w_south = inf;
728 _cvWorkEast (i, j, W, edges1, edges2);
730 W[i][j].w_southeast = W[i-1][j-1].w_east;
731 W[i][j].w_southeast = W[i][j].w_southeast + _cvStretchingWork( &edges1[i-1], &edges2[j-1] );
733 small_edge.x = NULL_EDGE*edges1[i-2].x;
734 small_edge.y = NULL_EDGE*edges1[i-2].y;
736 W[i][j].w_southeast = W[i][j].w_southeast +
737 _cvBendingWork( &edges1[i-2], &edges1[i-1], /*&null_edge*/&small_edge, &edges2[j-1]/*, &edges2[Nj-2]*/);
739 W[i][j].path_se = PATH_TO_E;
745 W[i][j].w_south = W[i][j-1].w_south;
746 W[i][j].w_south = W[i][j].w_south + _cvStretchingWork( &null_edge, &edges2[j-1] );
747 W[i][j].w_south = W[i][j].w_south /*+
748 _cvBendingWork( &null_edge, &null_edge, &edges2[j-2], &edges2[j-1], NULL )*/;
754 _cvWorkSouth(i, j, W, edges1, edges2);
756 W[i][j].w_southeast = W[i-1][j-1].w_south;
757 W[i][j].w_southeast = W[i][j].w_southeast + _cvStretchingWork( &edges1[i-1], &edges2[j-1] );
759 small_edge.x = NULL_EDGE*edges2[j-2].x;
760 small_edge.y = NULL_EDGE*edges2[j-2].y;
762 W[i][j].w_southeast = W[i][j].w_southeast +
763 _cvBendingWork( /*&null_edge*/&small_edge, &edges1[i-1], &edges2[j-2], &edges2[j-1]/*, &edges1[Ni-2]*/);
770 _cvWorkEast (i, j, W, edges1, edges2);
771 _cvWorkSouthEast(i, j, W, edges1, edges2);
772 _cvWorkSouth (i, j, W, edges1, edges2);
777 *corr = cvCreateSeq(0,
783 cvStartAppendToSeq( corr01, &writer );
784 if( W[i][j].w_east > W[i][j].w_southeast )
786 if( W[i][j].w_southeast > W[i][j].w_south )
797 if( W[i][j].w_east < W[i][j].w_south )
808 CV_WRITE_SEQ_ELEM( j, writer );
813 path = W[i][j].path_e;
815 cvFlushSeqWriter( &writer );
816 corr01->h_next = cvCreateSeq( 0,
820 corr01 = corr01->h_next;
821 cvStartAppendToSeq( corr01, &writer );
825 path = W[i][j].path_se;
827 cvFlushSeqWriter( &writer );
828 corr01->h_next = cvCreateSeq( 0,
832 corr01 = corr01->h_next;
833 cvStartAppendToSeq( corr01, &writer );
837 path = W[i][j].path_s;
842 } while( (i>=0) && (j>=0) );
843 cvFlushSeqWriter( &writer );