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 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 //////////////////////////////////////////////////////////////////////////////////// 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 ///////////////////////////////////////////////////////////////////////////////// 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 //////////////////////////////////////////////////////////////////////////////////// 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 ////////////////////////////////////////////////////////////////////////////////////// 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 //=================================================== 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 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 \***************************************************************************************/ 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 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
