| /*M/////////////////////////////////////////////////////////////////////////////////////// |
| // |
| // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. |
| // |
| // By downloading, copying, installing or using the software you agree to this license. |
| // If you do not agree to this license, do not download, install, |
| // copy or use the software. |
| // |
| // |
| // Intel License Agreement |
| // For Open Source Computer Vision Library |
| // |
| // Copyright (C) 2000, Intel Corporation, all rights reserved. |
| // Third party copyrights are property of their respective owners. |
| // |
| // Redistribution and use in source and binary forms, with or without modification, |
| // are permitted provided that the following conditions are met: |
| // |
| // * Redistribution's of source code must retain the above copyright notice, |
| // this list of conditions and the following disclaimer. |
| // |
| // * Redistribution's in binary form must reproduce the above copyright notice, |
| // this list of conditions and the following disclaimer in the documentation |
| // and/or other materials provided with the distribution. |
| // |
| // * The name of Intel Corporation may not be used to endorse or promote products |
| // derived from this software without specific prior written permission. |
| // |
| // This software is provided by the copyright holders and contributors "as is" and |
| // any express or implied warranties, including, but not limited to, the implied |
| // warranties of merchantability and fitness for a particular purpose are disclaimed. |
| // In no event shall the Intel Corporation or contributors be liable for any direct, |
| // indirect, incidental, special, exemplary, or consequential damages |
| // (including, but not limited to, procurement of substitute goods or services; |
| // loss of use, data, or profits; or business interruption) however caused |
| // and on any theory of liability, whether in contract, strict liability, |
| // or tort (including negligence or otherwise) arising in any way out of |
| // the use of this software, even if advised of the possibility of such damage. |
| // |
| //M*/ |
| |
| #include "_cvaux.h" |
| |
| #define PATH_TO_E 1 |
| #define PATH_TO_SE 2 |
| #define PATH_TO_S 3 |
| |
| #define K_S 2 |
| #define E_S 2 |
| #define C_S .01 |
| #define K_Z 5000 |
| #define K_NM 50000 |
| #define K_B 40 |
| #define NULL_EDGE 0.001f |
| #define inf DBL_MAX |
| |
| typedef struct __CvWork |
| { |
| double w_east; |
| double w_southeast; |
| double w_south; |
| char path_e; |
| char path_se; |
| char path_s; |
| }_CvWork; |
| |
| |
| double _cvBendingWork( CvPoint2D32f* B0, |
| CvPoint2D32f* F0, |
| CvPoint2D32f* B1, |
| CvPoint2D32f* F1/*, |
| CvPoint* K */); |
| |
| double _cvStretchingWork(CvPoint2D32f* P1, |
| CvPoint2D32f* P2); |
| |
| void _cvWorkEast (int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2); |
| void _cvWorkSouthEast(int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2); |
| void _cvWorkSouth (int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2); |
| |
| static CvPoint2D32f null_edge = {0,0}; |
| |
| double _cvStretchingWork(CvPoint2D32f* P1, |
| CvPoint2D32f* P2) |
| { |
| double L1,L2, L_min, dL; |
| |
| L1 = sqrt( (double)P1->x*P1->x + P1->y*P1->y); |
| L2 = sqrt( (double)P2->x*P2->x + P2->y*P2->y); |
| |
| L_min = MIN(L1, L2); |
| dL = fabs( L1 - L2 ); |
| |
| return K_S * pow( dL, E_S ) / ( L_min + C_S*dL ); |
| } |
| |
| |
| //////////////////////////////////////////////////////////////////////////////////// |
| double _cvBendingWork( CvPoint2D32f* B0, |
| CvPoint2D32f* F0, |
| CvPoint2D32f* B1, |
| CvPoint2D32f* F1/*, |
| CvPoint* K*/) |
| { |
| CvPoint2D32f Q( CvPoint2D32f q0, CvPoint2D32f q1, CvPoint2D32f q2, double t ); |
| double angle( CvPoint2D32f A, CvPoint2D32f B ); |
| |
| CvPoint2D32f Q0, Q1, Q2; |
| CvPoint2D32f Q1_nm = { 0, 0 }, Q2_nm = { 0, 0 }; |
| double d0, d1, d2, des, t_zero; |
| double k_zero, k_nonmon; |
| CvPoint2D32f center; |
| double check01, check02; |
| char check_origin; |
| double d_angle, d_nm_angle; |
| /* |
| if( (B0->x==0) && (B0->y==0) ) |
| { |
| if( (F0->x==0) && (F0->y==0) ) |
| { |
| B1->x = -B1->x; |
| B1->y = -B1->y; |
| |
| 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) ) ); |
| d_angle = CV_PI - d_angle; |
| |
| B1->x = -B1->x; |
| B1->y = -B1->y; |
| |
| //return d_angle*K_B; |
| return 100; |
| } |
| K->x = -K->x; |
| K->y = -K->y; |
| B1->x = -B1->x; |
| B1->y = -B1->y; |
| |
| 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) ) ); |
| 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) ) ); |
| d_angle = d_angle - CV_PI*0.5; |
| d_angle = fabs(d_angle); |
| |
| |
| K->x = -K->x; |
| K->y = -K->y; |
| B1->x = -B1->x; |
| B1->y = -B1->y; |
| |
| //return d_angle*K_B; |
| return 100; |
| } |
| |
| |
| if( (F0->x==0) && (F0->y==0) ) |
| { |
| K->x = -K->x; |
| K->y = -K->y; |
| B1->x = -B1->x; |
| B1->y = -B1->y; |
| |
| 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) ) ); |
| 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) ) ); |
| d_angle = d_angle - CV_PI*0.5; |
| d_angle = fabs(d_angle); |
| |
| K->x = -K->x; |
| K->y = -K->y; |
| B1->x = -B1->x; |
| B1->y = -B1->y; |
| |
| //return d_angle*K_B; |
| return 100; |
| } |
| /////////////// |
| |
| if( (B1->x==0) && (B1->y==0) ) |
| { |
| if( (F1->x==0) && (F1->y==0) ) |
| { |
| B0->x = -B0->x; |
| B0->y = -B0->y; |
| |
| 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) ) ); |
| d_angle = CV_PI - d_angle; |
| |
| B0->x = -B0->x; |
| B0->y = -B0->y; |
| |
| //return d_angle*K_B; |
| return 100; |
| } |
| K->x = -K->x; |
| K->y = -K->y; |
| B0->x = -B0->x; |
| B0->y = -B0->y; |
| |
| 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) ) ); |
| 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) ) ); |
| d_angle = d_angle - CV_PI*0.5; |
| d_angle = fabs(d_angle); |
| |
| K->x = -K->x; |
| K->y = -K->y; |
| B0->x = -B0->x; |
| B0->y = -B0->y; |
| |
| //return d_angle*K_B; |
| return 100; |
| } |
| |
| |
| if( (F1->x==0) && (F1->y==0) ) |
| { |
| K->x = -K->x; |
| K->y = -K->y; |
| B0->x = -B0->x; |
| B0->y = -B0->y; |
| |
| 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) ) ); |
| 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) ) ); |
| d_angle = d_angle - CV_PI*0.5; |
| d_angle = fabs(d_angle); |
| |
| K->x = -K->x; |
| K->y = -K->y; |
| B0->x = -B0->x; |
| B0->y = -B0->y; |
| |
| //return d_angle*K_B; |
| return 100; |
| } |
| |
| */ |
| |
| /* |
| B0->x = -B0->x; |
| B0->y = -B0->y; |
| B1->x = -B1->x; |
| B1->y = -B1->y; |
| */ |
| Q0.x = F0->x * (-B0->x) + F0->y * (-B0->y); |
| Q0.y = F0->x * (-B0->y) - F0->y * (-B0->x); |
| |
| Q1.x = 0.5f*( (F1->x * (-B0->x) + F1->y * (-B0->y)) + (F0->x * (-B1->x) + F0->y * (-B1->y)) ); |
| Q1.y = 0.5f*( (F1->x * (-B0->y) - F1->y * (-B0->x)) + (F0->x * (-B1->y) - F0->y * (-B1->x)) ); |
| |
| Q2.x = F1->x * (-B1->x) + F1->y * (-B1->y); |
| Q2.y = F1->x * (-B1->y) - F1->y * (-B1->x); |
| |
| d0 = Q0.x * Q1.y - Q0.y * Q1.x; |
| d1 = 0.5f*(Q0.x * Q2.y - Q0.y * Q2.x); |
| d2 = Q1.x * Q2.y - Q1.y * Q2.x; |
| |
| // Check angles goes to zero |
| des = Q1.y*Q1.y - Q0.y*Q2.y; |
| |
| k_zero = 0; |
| |
| if( des >= 0 ) |
| { |
| t_zero = ( Q0.y - Q1.y + sqrt(des) )/( Q0.y - 2*Q1.y + Q2.y ); |
| |
| if( (0 < t_zero) && (t_zero < 1) && ( Q(Q0, Q1, Q2, t_zero).x > 0 ) ) |
| { |
| k_zero = inf; |
| } |
| |
| t_zero = ( Q0.y - Q1.y - sqrt(des) )/( Q0.y - 2*Q1.y + Q2.y ); |
| |
| if( (0 < t_zero) && (t_zero < 1) && ( Q(Q0, Q1, Q2, t_zero).x > 0 ) ) |
| { |
| k_zero = inf; |
| } |
| } |
| |
| // Check nonmonotonic |
| des = d1*d1 - d0*d2; |
| |
| k_nonmon = 0; |
| |
| if( des >= 0 ) |
| { |
| t_zero = ( d0 - d1 - sqrt(des) )/( d0 - 2*d1 + d2 ); |
| |
| if( (0 < t_zero) && (t_zero < 1) ) |
| { |
| k_nonmon = 1; |
| Q1_nm = Q(Q0, Q1, Q2, t_zero); |
| } |
| |
| t_zero = ( d0 - d1 + sqrt(des) )/( d0 - 2*d1 + d2 ); |
| |
| if( (0 < t_zero) && (t_zero < 1) ) |
| { |
| k_nonmon += 2; |
| Q2_nm = Q(Q0, Q1, Q2, t_zero); |
| } |
| } |
| |
| // Finde origin lie in Q0Q1Q2 |
| check_origin = 1; |
| |
| center.x = (Q0.x + Q1.x + Q2.x)/3; |
| center.y = (Q0.y + Q1.y + Q2.y)/3; |
| |
| check01 = (center.x - Q0.x)*(Q1.y - Q0.y) + (center.y - Q0.y)*(Q1.x - Q0.x); |
| check02 = (-Q0.x)*(Q1.y - Q0.y) + (-Q0.y)*(Q1.x - Q0.x); |
| if( check01*check02 > 0 ) |
| { |
| check01 = (center.x - Q1.x)*(Q2.y - Q1.y) + (center.y - Q1.y)*(Q2.x - Q1.x); |
| check02 = (-Q1.x)*(Q2.y - Q1.y) + (-Q1.y)*(Q2.x - Q1.x); |
| if( check01*check02 > 0 ) |
| { |
| check01 = (center.x - Q2.x)*(Q0.y - Q2.y) + (center.y - Q2.y)*(Q0.x - Q2.x); |
| check02 = (-Q2.x)*(Q0.y - Q2.y) + (-Q2.y)*(Q0.x - Q2.x); |
| if( check01*check02 > 0 ) |
| { |
| check_origin = 0; |
| } |
| } |
| } |
| |
| // Calculate angle |
| d_nm_angle = 0; |
| d_angle = angle(Q0,Q2); |
| if( k_nonmon == 0 ) |
| { |
| if( check_origin == 0 ) |
| { |
| } |
| else |
| { |
| d_angle = 2*CV_PI - d_angle; |
| } |
| } |
| else |
| { |
| if( k_nonmon == 1 ) |
| { |
| d_nm_angle = angle(Q0,Q1_nm); |
| if(d_nm_angle > d_angle) |
| { |
| d_nm_angle = d_nm_angle - d_angle; |
| } |
| } |
| |
| if( k_nonmon == 2 ) |
| { |
| d_nm_angle = angle(Q0,Q2_nm); |
| if(d_nm_angle > d_angle) |
| { |
| d_nm_angle = d_nm_angle - d_angle; |
| } |
| } |
| |
| if( k_nonmon == 3 ) |
| { |
| d_nm_angle = angle(Q0,Q1_nm); |
| if(d_nm_angle > d_angle) |
| { |
| d_nm_angle = d_nm_angle - d_angle; |
| d_nm_angle = d_nm_angle + angle(Q0, Q2_nm); |
| } |
| else |
| { |
| d_nm_angle = d_nm_angle + angle(Q2,Q2_nm); |
| } |
| } |
| } |
| /* |
| B0->x = -B0->x; |
| B0->y = -B0->y; |
| B1->x = -B1->x; |
| B1->y = -B1->y; |
| */ |
| return d_angle*K_B + d_nm_angle*K_NM + k_zero*K_Z; |
| //return 0; |
| } |
| |
| |
| ///////////////////////////////////////////////////////////////////////////////// |
| void _cvWorkEast(int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2) |
| { |
| double w1,w2; |
| CvPoint2D32f small_edge; |
| |
| //W[i,j].w_east |
| w1 = W[i-1][j].w_east /*+ _cvBendingWork( &edges1[i-2], |
| &edges1[i-1], |
| &null_edge , |
| &null_edge, |
| NULL)*/; |
| |
| small_edge.x = NULL_EDGE*edges1[i-1].x; |
| small_edge.y = NULL_EDGE*edges1[i-1].y; |
| |
| w2 = W[i-1][j].w_southeast + _cvBendingWork(&edges1[i-2], |
| &edges1[i-1], |
| &edges2[j-1], |
| /*&null_edge*/&small_edge/*, |
| &edges2[j]*/); |
| |
| if(w1<w2) |
| { |
| W[i][j].w_east = w1 + _cvStretchingWork( &edges1[i-1], &null_edge ); |
| W[i][j].path_e = PATH_TO_E; |
| } |
| else |
| { |
| W[i][j].w_east = w2 + _cvStretchingWork( &edges1[i-1], &null_edge ); |
| W[i][j].path_e = PATH_TO_SE; |
| } |
| } |
| |
| |
| |
| |
| |
| //////////////////////////////////////////////////////////////////////////////////// |
| void _cvWorkSouthEast(int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2) |
| { |
| double w1,w2,w3; |
| CvPoint2D32f small_edge; |
| |
| //W[i,j].w_southeast |
| small_edge.x = NULL_EDGE*edges1[i-2].x; |
| small_edge.y = NULL_EDGE*edges1[i-2].y; |
| |
| w1 = W[i-1][j-1].w_east + _cvBendingWork(&edges1[i-2], |
| &edges1[i-1], |
| /*&null_edge*/&small_edge, |
| &edges2[j-1]/*, |
| &edges2[j-2]*/); |
| |
| w2 = W[i-1][j-1].w_southeast + _cvBendingWork( &edges1[i-2], |
| &edges1[i-1], |
| &edges2[j-2], |
| &edges2[j-1]/*, |
| NULL*/); |
| |
| small_edge.x = NULL_EDGE*edges2[j-2].x; |
| small_edge.y = NULL_EDGE*edges2[j-2].y; |
| |
| w3 = W[i-1][j-1].w_south + _cvBendingWork( /*&null_edge*/&small_edge, |
| &edges1[i-1], |
| &edges2[j-2], |
| &edges2[j-1]/*, |
| &edges1[i-2]*/); |
| |
| if( w1<w2 ) |
| { |
| if(w1<w3) |
| { |
| W[i][j].w_southeast = w1 + _cvStretchingWork( &edges1[i-1], &edges2[j-1] ); |
| W[i][j].path_se = PATH_TO_E; |
| } |
| else |
| { |
| W[i][j].w_southeast = w3 + _cvStretchingWork( &edges1[i-1], &edges2[j-1] ); |
| W[i][j].path_se = 3; |
| } |
| } |
| else |
| { |
| if( w2<w3) |
| { |
| W[i][j].w_southeast = w2 + _cvStretchingWork( &edges1[i-1], &edges2[j-1] ); |
| W[i][j].path_se = PATH_TO_SE; |
| } |
| else |
| { |
| W[i][j].w_southeast = w3 + _cvStretchingWork( &edges1[i-1], &edges2[j-1] ); |
| W[i][j].path_se = 3; |
| } |
| } |
| } |
| |
| |
| ////////////////////////////////////////////////////////////////////////////////////// |
| void _cvWorkSouth(int i, int j, _CvWork** W, CvPoint2D32f* edges1, CvPoint2D32f* edges2) |
| { |
| double w1,w2; |
| CvPoint2D32f small_edge; |
| |
| //W[i,j].w_south |
| |
| small_edge.x = NULL_EDGE*edges2[j-1].x; |
| small_edge.y = NULL_EDGE*edges2[j-1].y; |
| |
| w1 = W[i][j-1].w_southeast + _cvBendingWork(&edges1[i-1], |
| /*&null_edge*/&small_edge, |
| &edges2[j-2], |
| &edges2[j-1]/*, |
| &edges1[i]*/); |
| |
| w2 = W[i][j-1].w_south /*+ _cvBendingWork( &null_edge , |
| &null_edge, |
| &edges2[j-2], |
| &edges2[j-1], |
| NULL)*/; |
| |
| if( w1<w2 ) |
| { |
| W[i][j].w_south = w1 + _cvStretchingWork( &null_edge, &edges2[j-1] ); |
| W[i][j].path_s = PATH_TO_SE; |
| } |
| else |
| { |
| W[i][j].w_south = w2 + _cvStretchingWork( &null_edge, &edges2[j-1] ); |
| W[i][j].path_s = 3; |
| } |
| } |
| |
| //=================================================== |
| CvPoint2D32f Q(CvPoint2D32f q0,CvPoint2D32f q1,CvPoint2D32f q2,double t) |
| { |
| CvPoint2D32f q; |
| |
| q.x = (float)(q0.x*(1-t)*(1-t) + 2*q1.x*t*(1-t) + q2.x*t*t); |
| q.y = (float)(q0.y*(1-t)*(1-t) + 2*q1.y*t*(1-t) + q2.y*t*t); |
| |
| return q; |
| } |
| |
| double angle(CvPoint2D32f A, CvPoint2D32f B) |
| { |
| 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) ) ); |
| } |
| |
| /***************************************************************************************\ |
| * |
| * This function compute intermediate polygon between contour1 and contour2 |
| * |
| * Correspondence between points of contours specify by corr |
| * |
| * param = [0,1]; 0 correspondence to contour1, 1 - contour2 |
| * |
| \***************************************************************************************/ |
| CvSeq* icvBlendContours(CvSeq* contour1, |
| CvSeq* contour2, |
| CvSeq* corr, |
| double param, |
| CvMemStorage* storage) |
| { |
| int j; |
| |
| CvSeqWriter writer01; |
| CvSeqReader reader01; |
| |
| int Ni,Nj; // size of contours |
| int i; // counter |
| |
| CvPoint* point1; // array of first contour point |
| CvPoint* point2; // array of second contour point |
| |
| CvPoint point_output; // intermediate storage of ouput point |
| |
| int corr_point; |
| |
| // Create output sequence. |
| CvSeq* output = cvCreateSeq(0, |
| sizeof(CvSeq), |
| sizeof(CvPoint), |
| storage ); |
| |
| // Find size of contours. |
| Ni = contour1->total + 1; |
| Nj = contour2->total + 1; |
| |
| point1 = (CvPoint* )malloc( Ni*sizeof(CvPoint) ); |
| point2 = (CvPoint* )malloc( Nj*sizeof(CvPoint) ); |
| |
| // Initialize arrays of point |
| cvCvtSeqToArray( contour1, point1, CV_WHOLE_SEQ ); |
| cvCvtSeqToArray( contour2, point2, CV_WHOLE_SEQ ); |
| |
| // First and last point mast be equal. |
| point1[Ni-1] = point1[0]; |
| point2[Nj-1] = point2[0]; |
| |
| // Initializes process of writing to sequence. |
| cvStartAppendToSeq( output, &writer01); |
| |
| i = Ni-1; //correspondence to points of contour1 |
| for( ; corr; corr = corr->h_next ) |
| { |
| //Initializes process of sequential reading from sequence |
| cvStartReadSeq( corr, &reader01, 0 ); |
| |
| for(j=0; j < corr->total; j++) |
| { |
| // Read element from sequence. |
| CV_READ_SEQ_ELEM( corr_point, reader01 ); |
| |
| // Compute point of intermediate polygon. |
| point_output.x = cvRound(point1[i].x + param*( point2[corr_point].x - point1[i].x )); |
| point_output.y = cvRound(point1[i].y + param*( point2[corr_point].y - point1[i].y )); |
| |
| // Write element to sequence. |
| CV_WRITE_SEQ_ELEM( point_output, writer01 ); |
| } |
| i--; |
| } |
| // Updates sequence header. |
| cvFlushSeqWriter( &writer01 ); |
| |
| return output; |
| } |
| |
| /************************************************************************************************** |
| * |
| * |
| * |
| * |
| * |
| * |
| * |
| * |
| * |
| * |
| **************************************************************************************************/ |
| |
| |
| void icvCalcContoursCorrespondence(CvSeq* contour1, |
| CvSeq* contour2, |
| CvSeq** corr, |
| CvMemStorage* storage) |
| { |
| int i,j; // counter of cycles |
| int Ni,Nj; // size of contours |
| _CvWork** W; // graph for search minimum of work |
| |
| CvPoint* point1; // array of first contour point |
| CvPoint* point2; // array of second contour point |
| CvPoint2D32f* edges1; // array of first contour edge |
| CvPoint2D32f* edges2; // array of second contour edge |
| |
| //CvPoint null_edge = {0,0}; // |
| CvPoint2D32f small_edge; |
| //double inf; // infinity |
| |
| CvSeq* corr01; |
| CvSeqWriter writer; |
| |
| char path; // |
| |
| // Find size of contours |
| Ni = contour1->total + 1; |
| Nj = contour2->total + 1; |
| |
| // Create arrays |
| W = (_CvWork**)malloc(sizeof(_CvWork*)*Ni); |
| for(i=0; i<Ni; i++) |
| { |
| W[i] = (_CvWork*)malloc(sizeof(_CvWork)*Nj); |
| } |
| |
| point1 = (CvPoint* )malloc( Ni*sizeof(CvPoint) ); |
| point2 = (CvPoint* )malloc( Nj*sizeof(CvPoint) ); |
| edges1 = (CvPoint2D32f* )malloc( (Ni-1)*sizeof(CvPoint2D32f) ); |
| edges2 = (CvPoint2D32f* )malloc( (Nj-1)*sizeof(CvPoint2D32f) ); |
| |
| // Initialize arrays of point |
| cvCvtSeqToArray( contour1, point1, CV_WHOLE_SEQ ); |
| cvCvtSeqToArray( contour2, point2, CV_WHOLE_SEQ ); |
| |
| point1[Ni-1] = point1[0]; |
| point2[Nj-1] = point2[0]; |
| |
| for(i=0;i<Ni-1;i++) |
| { |
| edges1[i].x = (float)( point1[i+1].x - point1[i].x ); |
| edges1[i].y = (float)( point1[i+1].y - point1[i].y ); |
| }; |
| |
| for(i=0;i<Nj-1;i++) |
| { |
| edges2[i].x = (float)( point2[i+1].x - point2[i].x ); |
| edges2[i].y = (float)( point2[i+1].y - point2[i].y ); |
| }; |
| |
| // Find infinity constant |
| //inf=1; |
| ///////////// |
| |
| //Find min path in graph |
| |
| ///////////// |
| W[0][0].w_east = 0; |
| W[0][0].w_south = 0; |
| W[0][0].w_southeast = 0; |
| |
| W[1][1].w_southeast = _cvStretchingWork( &edges1[0], &edges2[0] ); |
| W[1][1].w_east = inf; |
| W[1][1].w_south = inf; |
| W[1][1].path_se = PATH_TO_SE; |
| |
| W[0][1].w_south = _cvStretchingWork( &null_edge, &edges2[0] ); |
| W[0][1].path_s = 3; |
| W[1][0].w_east = _cvStretchingWork( &edges2[0], &null_edge ); |
| W[1][0].path_e = PATH_TO_E; |
| |
| for( i=1; i<Ni; i++ ) |
| { |
| W[i][0].w_south = inf; |
| W[i][0].w_southeast = inf; |
| } |
| |
| for(j=1; j<Nj; j++) |
| { |
| W[0][j].w_east = inf; |
| W[0][j].w_southeast = inf; |
| } |
| |
| for(i=2; i<Ni; i++) |
| { |
| j=0;///////// |
| W[i][j].w_east = W[i-1][j].w_east; |
| W[i][j].w_east = W[i][j].w_east /*+ |
| _cvBendingWork( &edges1[i-2], &edges1[i-1], &null_edge, &null_edge, NULL )*/; |
| W[i][j].w_east = W[i][j].w_east + _cvStretchingWork( &edges2[i-1], &null_edge ); |
| W[i][j].path_e = PATH_TO_E; |
| |
| j=1;////////// |
| W[i][j].w_south = inf; |
| |
| _cvWorkEast (i, j, W, edges1, edges2); |
| |
| W[i][j].w_southeast = W[i-1][j-1].w_east; |
| W[i][j].w_southeast = W[i][j].w_southeast + _cvStretchingWork( &edges1[i-1], &edges2[j-1] ); |
| |
| small_edge.x = NULL_EDGE*edges1[i-2].x; |
| small_edge.y = NULL_EDGE*edges1[i-2].y; |
| |
| W[i][j].w_southeast = W[i][j].w_southeast + |
| _cvBendingWork( &edges1[i-2], &edges1[i-1], /*&null_edge*/&small_edge, &edges2[j-1]/*, &edges2[Nj-2]*/); |
| |
| W[i][j].path_se = PATH_TO_E; |
| } |
| |
| for(j=2; j<Nj; j++) |
| { |
| i=0;////////// |
| W[i][j].w_south = W[i][j-1].w_south; |
| W[i][j].w_south = W[i][j].w_south + _cvStretchingWork( &null_edge, &edges2[j-1] ); |
| W[i][j].w_south = W[i][j].w_south /*+ |
| _cvBendingWork( &null_edge, &null_edge, &edges2[j-2], &edges2[j-1], NULL )*/; |
| W[i][j].path_s = 3; |
| |
| i=1;/////////// |
| W[i][j].w_east= inf; |
| |
| _cvWorkSouth(i, j, W, edges1, edges2); |
| |
| W[i][j].w_southeast = W[i-1][j-1].w_south; |
| W[i][j].w_southeast = W[i][j].w_southeast + _cvStretchingWork( &edges1[i-1], &edges2[j-1] ); |
| |
| small_edge.x = NULL_EDGE*edges2[j-2].x; |
| small_edge.y = NULL_EDGE*edges2[j-2].y; |
| |
| W[i][j].w_southeast = W[i][j].w_southeast + |
| _cvBendingWork( /*&null_edge*/&small_edge, &edges1[i-1], &edges2[j-2], &edges2[j-1]/*, &edges1[Ni-2]*/); |
| W[i][j].path_se = 3; |
| } |
| |
| for(i=2; i<Ni; i++) |
| for(j=2; j<Nj; j++) |
| { |
| _cvWorkEast (i, j, W, edges1, edges2); |
| _cvWorkSouthEast(i, j, W, edges1, edges2); |
| _cvWorkSouth (i, j, W, edges1, edges2); |
| } |
| |
| i=Ni-1;j=Nj-1; |
| |
| *corr = cvCreateSeq(0, |
| sizeof(CvSeq), |
| sizeof(int), |
| storage ); |
| |
| corr01 = *corr; |
| cvStartAppendToSeq( corr01, &writer ); |
| if( W[i][j].w_east > W[i][j].w_southeast ) |
| { |
| if( W[i][j].w_southeast > W[i][j].w_south ) |
| { |
| path = 3; |
| } |
| else |
| { |
| path = PATH_TO_SE; |
| } |
| } |
| else |
| { |
| if( W[i][j].w_east < W[i][j].w_south ) |
| { |
| path = PATH_TO_E; |
| } |
| else |
| { |
| path = 3; |
| } |
| } |
| do |
| { |
| CV_WRITE_SEQ_ELEM( j, writer ); |
| |
| switch( path ) |
| { |
| case PATH_TO_E: |
| path = W[i][j].path_e; |
| i--; |
| cvFlushSeqWriter( &writer ); |
| corr01->h_next = cvCreateSeq( 0, |
| sizeof(CvSeq), |
| sizeof(int), |
| storage ); |
| corr01 = corr01->h_next; |
| cvStartAppendToSeq( corr01, &writer ); |
| break; |
| |
| case PATH_TO_SE: |
| path = W[i][j].path_se; |
| j--; i--; |
| cvFlushSeqWriter( &writer ); |
| corr01->h_next = cvCreateSeq( 0, |
| sizeof(CvSeq), |
| sizeof(int), |
| storage ); |
| corr01 = corr01->h_next; |
| cvStartAppendToSeq( corr01, &writer ); |
| break; |
| |
| case 3: |
| path = W[i][j].path_s; |
| j--; |
| break; |
| } |
| |
| } while( (i>=0) && (j>=0) ); |
| cvFlushSeqWriter( &writer ); |
| |
| // Free memory |
| for(i=1;i<Ni;i++) |
| { |
| free(W[i]); |
| } |
| free(W); |
| free(point1); |
| free(point2); |
| free(edges1); |
| free(edges2); |
| } |
| |