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 #include "_cvaux.h" 42 #include "_cvvm.h" 43 #include <stdlib.h> 44 45 #define Sgn(x) ( (x)<0 ? -1:1 ) /* Sgn(0) = 1 ! */ 46 /*===========================================================================*/ 47 CvStatus 48 icvLMedS( int *points1, int *points2, int numPoints, CvMatrix3 * fundamentalMatrix ) 49 { 50 int sample, j, amount_samples, done; 51 int amount_solutions; 52 int ml7[21], mr7[21]; 53 54 double F_try[9 * 3]; 55 double F[9]; 56 double Mj, Mj_new; 57 58 int i, num; 59 60 int *ml; 61 int *mr; 62 int *new_ml; 63 int *new_mr; 64 int new_num; 65 CvStatus error; 66 67 error = CV_NO_ERR; 68 69 if( fundamentalMatrix == 0 ) 70 return CV_BADFACTOR_ERR; 71 72 num = numPoints; 73 74 if( num < 6 ) 75 { 76 return CV_BADFACTOR_ERR; 77 } /* if */ 78 79 ml = (int *) cvAlloc( sizeof( int ) * num * 3 ); 80 mr = (int *) cvAlloc( sizeof( int ) * num * 3 ); 81 82 for( i = 0; i < num; i++ ) 83 { 84 85 ml[i * 3] = points1[i * 2]; 86 ml[i * 3 + 1] = points1[i * 2 + 1]; 87 88 ml[i * 3 + 2] = 1; 89 90 mr[i * 3] = points2[i * 2]; 91 mr[i * 3 + 1] = points2[i * 2 + 1]; 92 93 mr[i * 3 + 2] = 1; 94 } /* for */ 95 96 if( num > 7 ) 97 { 98 99 Mj = -1; 100 amount_samples = 1000; /* ------- Must be changed ! --------- */ 101 102 for( sample = 0; sample < amount_samples; sample++ ) 103 { 104 105 icvChoose7( ml, mr, num, ml7, mr7 ); 106 icvPoint7( ml7, mr7, F_try, &amount_solutions ); 107 108 for( i = 0; i < amount_solutions / 9; i++ ) 109 { 110 111 Mj_new = icvMedian( ml, mr, num, F_try + i * 9 ); 112 113 if( Mj_new >= 0 && (Mj == -1 || Mj_new < Mj) ) 114 { 115 116 for( j = 0; j < 9; j++ ) 117 { 118 119 F[j] = F_try[i * 9 + j]; 120 } /* for */ 121 122 Mj = Mj_new; 123 } /* if */ 124 } /* for */ 125 } /* for */ 126 127 if( Mj == -1 ) 128 return CV_BADFACTOR_ERR; 129 130 done = icvBoltingPoints( ml, mr, num, F, Mj, &new_ml, &new_mr, &new_num ); 131 132 if( done == -1 ) 133 { 134 135 cvFree( &mr ); 136 cvFree( &ml ); 137 return CV_OUTOFMEM_ERR; 138 } /* if */ 139 140 if( done > 7 ) 141 error = icvPoints8( new_ml, new_mr, new_num, F ); 142 143 cvFree( &new_mr ); 144 cvFree( &new_ml ); 145 146 } 147 else 148 { 149 error = icvPoint7( ml, mr, F, &i ); 150 } /* if */ 151 152 if( error == CV_NO_ERR ) 153 error = icvRank2Constraint( F ); 154 155 for( i = 0; i < 3; i++ ) 156 for( j = 0; j < 3; j++ ) 157 fundamentalMatrix->m[i][j] = (float) F[i * 3 + j]; 158 159 return error; 160 161 } /* icvLMedS */ 162 163 /*===========================================================================*/ 164 /*===========================================================================*/ 165 void 166 icvChoose7( int *ml, int *mr, int num, int *ml7, int *mr7 ) 167 { 168 int indexes[7], i, j; 169 170 if( !ml || !mr || num < 7 || !ml7 || !mr7 ) 171 return; 172 173 for( i = 0; i < 7; i++ ) 174 { 175 176 indexes[i] = (int) ((double) rand() / RAND_MAX * num); 177 178 for( j = 0; j < i; j++ ) 179 { 180 181 if( indexes[i] == indexes[j] ) 182 i--; 183 } /* for */ 184 } /* for */ 185 186 for( i = 0; i < 21; i++ ) 187 { 188 189 ml7[i] = ml[3 * indexes[i / 3] + i % 3]; 190 mr7[i] = mr[3 * indexes[i / 3] + i % 3]; 191 } /* for */ 192 } /* cs_Choose7 */ 193 194 /*===========================================================================*/ 195 /*===========================================================================*/ 196 CvStatus 197 icvCubic( double a2, double a1, double a0, double *squares ) 198 { 199 double p, q, D, c1, c2, b1, b2, ro1, ro2, fi1, fi2, tt; 200 double x[6][3]; 201 int i, j, t; 202 203 if( !squares ) 204 return CV_BADFACTOR_ERR; 205 206 p = a1 - a2 * a2 / 3; 207 q = (9 * a1 * a2 - 27 * a0 - 2 * a2 * a2 * a2) / 27; 208 D = q * q / 4 + p * p * p / 27; 209 210 if( D < 0 ) 211 { 212 213 c1 = q / 2; 214 c2 = c1; 215 b1 = sqrt( -D ); 216 b2 = -b1; 217 218 ro1 = sqrt( c1 * c1 - D ); 219 ro2 = ro1; 220 221 fi1 = atan2( b1, c1 ); 222 fi2 = -fi1; 223 } 224 else 225 { 226 227 c1 = q / 2 + sqrt( D ); 228 c2 = q / 2 - sqrt( D ); 229 b1 = 0; 230 b2 = 0; 231 232 ro1 = fabs( c1 ); 233 ro2 = fabs( c2 ); 234 fi1 = CV_PI * (1 - SIGN( c1 )) / 2; 235 fi2 = CV_PI * (1 - SIGN( c2 )) / 2; 236 } /* if */ 237 238 for( i = 0; i < 6; i++ ) 239 { 240 241 x[i][0] = -a2 / 3; 242 x[i][1] = 0; 243 x[i][2] = 0; 244 245 squares[i] = x[i][i % 2]; 246 } /* for */ 247 248 if( !REAL_ZERO( ro1 )) 249 { 250 tt = SIGN( ro1 ) * pow( fabs( ro1 ), 0.333333333333 ); 251 c1 = tt - p / (3. * tt); 252 c2 = tt + p / (3. * tt); 253 } /* if */ 254 255 if( !REAL_ZERO( ro2 )) 256 { 257 tt = SIGN( ro2 ) * pow( fabs( ro2 ), 0.333333333333 ); 258 b1 = tt - p / (3. * tt); 259 b2 = tt + p / (3. * tt); 260 } /* if */ 261 262 for( i = 0; i < 6; i++ ) 263 { 264 265 if( i < 3 ) 266 { 267 268 if( !REAL_ZERO( ro1 )) 269 { 270 271 x[i][0] = cos( fi1 / 3. + 2 * CV_PI * (i % 3) / 3. ) * c1 - a2 / 3; 272 x[i][1] = sin( fi1 / 3. + 2 * CV_PI * (i % 3) / 3. ) * c2; 273 } 274 else 275 { 276 277 x[i][2] = 1; 278 } /* if */ 279 } 280 else 281 { 282 283 if( !REAL_ZERO( ro2 )) 284 { 285 286 x[i][0] = cos( fi2 / 3. + 2 * CV_PI * (i % 3) / 3. ) * b1 - a2 / 3; 287 x[i][1] = sin( fi2 / 3. + 2 * CV_PI * (i % 3) / 3. ) * b2; 288 } 289 else 290 { 291 292 x[i][2] = 1; 293 } /* if */ 294 } /* if */ 295 } /* for */ 296 297 t = 0; 298 299 for( i = 0; i < 6; i++ ) 300 { 301 302 if( !x[i][2] ) 303 { 304 305 squares[t++] = x[i][0]; 306 squares[t++] = x[i][1]; 307 x[i][2] = 1; 308 309 for( j = i + 1; j < 6; j++ ) 310 { 311 312 if( !x[j][2] && REAL_ZERO( x[i][0] - x[j][0] ) 313 && REAL_ZERO( x[i][1] - x[j][1] )) 314 { 315 316 x[j][2] = 1; 317 break; 318 } /* if */ 319 } /* for */ 320 } /* if */ 321 } /* for */ 322 return CV_NO_ERR; 323 } /* icvCubic */ 324 325 /*======================================================================================*/ 326 double 327 icvDet( double *M ) 328 { 329 double value; 330 331 if( !M ) 332 return 0; 333 334 value = M[0] * M[4] * M[8] + M[2] * M[3] * M[7] + M[1] * M[5] * M[6] - 335 M[2] * M[4] * M[6] - M[0] * M[5] * M[7] - M[1] * M[3] * M[8]; 336 337 return value; 338 339 } /* icvDet */ 340 341 /*===============================================================================*/ 342 double 343 icvMinor( double *M, int x, int y ) 344 { 345 int row1, row2, col1, col2; 346 double value; 347 348 if( !M || x < 0 || x > 2 || y < 0 || y > 2 ) 349 return 0; 350 351 row1 = (y == 0 ? 1 : 0); 352 row2 = (y == 2 ? 1 : 2); 353 col1 = (x == 0 ? 1 : 0); 354 col2 = (x == 2 ? 1 : 2); 355 356 value = M[row1 * 3 + col1] * M[row2 * 3 + col2] - M[row2 * 3 + col1] * M[row1 * 3 + col2]; 357 358 value *= 1 - (x + y) % 2 * 2; 359 360 return value; 361 362 } /* icvMinor */ 363 364 /*======================================================================================*/ 365 CvStatus 366 icvGetCoef( double *f1, double *f2, double *a2, double *a1, double *a0 ) 367 { 368 double G[9], a3; 369 int i; 370 371 if( !f1 || !f2 || !a0 || !a1 || !a2 ) 372 return CV_BADFACTOR_ERR; 373 374 for( i = 0; i < 9; i++ ) 375 { 376 377 G[i] = f1[i] - f2[i]; 378 } /* for */ 379 380 a3 = icvDet( G ); 381 382 if( REAL_ZERO( a3 )) 383 return CV_BADFACTOR_ERR; 384 385 *a2 = 0; 386 *a1 = 0; 387 *a0 = icvDet( f2 ); 388 389 for( i = 0; i < 9; i++ ) 390 { 391 392 *a2 += f2[i] * icvMinor( G, (int) (i % 3), (int) (i / 3) ); 393 *a1 += G[i] * icvMinor( f2, (int) (i % 3), (int) (i / 3) ); 394 } /* for */ 395 396 *a0 /= a3; 397 *a1 /= a3; 398 *a2 /= a3; 399 400 return CV_NO_ERR; 401 402 } /* icvGetCoef */ 403 404 /*===========================================================================*/ 405 double 406 icvMedian( int *ml, int *mr, int num, double *F ) 407 { 408 double l1, l2, l3, d1, d2, value; 409 double *deviation; 410 int i, i3; 411 412 if( !ml || !mr || !F ) 413 return -1; 414 415 deviation = (double *) cvAlloc( (num) * sizeof( double )); 416 417 if( !deviation ) 418 return -1; 419 420 for( i = 0, i3 = 0; i < num; i++, i3 += 3 ) 421 { 422 423 l1 = F[0] * mr[i3] + F[1] * mr[i3 + 1] + F[2]; 424 l2 = F[3] * mr[i3] + F[4] * mr[i3 + 1] + F[5]; 425 l3 = F[6] * mr[i3] + F[7] * mr[i3 + 1] + F[8]; 426 427 d1 = (l1 * ml[i3] + l2 * ml[i3 + 1] + l3) / sqrt( l1 * l1 + l2 * l2 ); 428 429 l1 = F[0] * ml[i3] + F[3] * ml[i3 + 1] + F[6]; 430 l2 = F[1] * ml[i3] + F[4] * ml[i3 + 1] + F[7]; 431 l3 = F[2] * ml[i3] + F[5] * ml[i3 + 1] + F[8]; 432 433 d2 = (l1 * mr[i3] + l2 * mr[i3 + 1] + l3) / sqrt( l1 * l1 + l2 * l2 ); 434 435 deviation[i] = (double) (d1 * d1 + d2 * d2); 436 } /* for */ 437 438 if( icvSort( deviation, num ) != CV_NO_ERR ) 439 { 440 441 cvFree( &deviation ); 442 return -1; 443 } /* if */ 444 445 value = deviation[num / 2]; 446 cvFree( &deviation ); 447 return value; 448 449 } /* cs_Median */ 450 451 /*===========================================================================*/ 452 CvStatus 453 icvSort( double *array, int length ) 454 { 455 int i, j, index; 456 double swapd; 457 458 if( !array || length < 1 ) 459 return CV_BADFACTOR_ERR; 460 461 for( i = 0; i < length - 1; i++ ) 462 { 463 464 index = i; 465 466 for( j = i + 1; j < length; j++ ) 467 { 468 469 if( array[j] < array[index] ) 470 index = j; 471 } /* for */ 472 473 if( index - i ) 474 { 475 476 swapd = array[i]; 477 array[i] = array[index]; 478 array[index] = swapd; 479 } /* if */ 480 } /* for */ 481 482 return CV_NO_ERR; 483 484 } /* cs_Sort */ 485 486 /*===========================================================================*/ 487 int 488 icvBoltingPoints( int *ml, int *mr, 489 int num, double *F, double Mj, int **new_ml, int **new_mr, int *new_num ) 490 { 491 double l1, l2, l3, d1, d2, sigma; 492 int i, j, length; 493 int *index; 494 495 if( !ml || !mr || num < 1 || !F || Mj < 0 ) 496 return -1; 497 498 index = (int *) cvAlloc( (num) * sizeof( int )); 499 500 if( !index ) 501 return -1; 502 503 length = 0; 504 sigma = (double) (2.5 * 1.4826 * (1 + 5. / (num - 7)) * sqrt( Mj )); 505 506 for( i = 0; i < num * 3; i += 3 ) 507 { 508 509 l1 = F[0] * mr[i] + F[1] * mr[i + 1] + F[2]; 510 l2 = F[3] * mr[i] + F[4] * mr[i + 1] + F[5]; 511 l3 = F[6] * mr[i] + F[7] * mr[i + 1] + F[8]; 512 513 d1 = (l1 * ml[i] + l2 * ml[i + 1] + l3) / sqrt( l1 * l1 + l2 * l2 ); 514 515 l1 = F[0] * ml[i] + F[3] * ml[i + 1] + F[6]; 516 l2 = F[1] * ml[i] + F[4] * ml[i + 1] + F[7]; 517 l3 = F[2] * ml[i] + F[5] * ml[i + 1] + F[8]; 518 519 d2 = (l1 * mr[i] + l2 * mr[i + 1] + l3) / sqrt( l1 * l1 + l2 * l2 ); 520 521 if( d1 * d1 + d2 * d2 <= sigma * sigma ) 522 { 523 524 index[i / 3] = 1; 525 length++; 526 } 527 else 528 { 529 530 index[i / 3] = 0; 531 } /* if */ 532 } /* for */ 533 534 *new_num = length; 535 536 *new_ml = (int *) cvAlloc( (length * 3) * sizeof( int )); 537 538 if( !new_ml ) 539 { 540 541 cvFree( &index ); 542 return -1; 543 } /* if */ 544 545 *new_mr = (int *) cvAlloc( (length * 3) * sizeof( int )); 546 547 if( !new_mr ) 548 { 549 550 cvFree( &new_ml ); 551 cvFree( &index ); 552 return -1; 553 } /* if */ 554 555 j = 0; 556 557 for( i = 0; i < num * 3; ) 558 { 559 560 if( index[i / 3] ) 561 { 562 563 (*new_ml)[j] = ml[i]; 564 (*new_mr)[j++] = mr[i++]; 565 (*new_ml)[j] = ml[i]; 566 (*new_mr)[j++] = mr[i++]; 567 (*new_ml)[j] = ml[i]; 568 (*new_mr)[j++] = mr[i++]; 569 } 570 else 571 i += 3; 572 } /* for */ 573 574 cvFree( &index ); 575 return length; 576 577 } /* cs_BoltingPoints */ 578 579 /*===========================================================================*/ 580 CvStatus 581 icvPoints8( int *ml, int *mr, int num, double *F ) 582 { 583 double *U; 584 double l1, l2, w, old_norm = -1, new_norm = -2, summ; 585 int i3, i9, j, num3, its = 0, a, t; 586 587 if( !ml || !mr || num < 8 || !F ) 588 return CV_BADFACTOR_ERR; 589 590 U = (double *) cvAlloc( (num * 9) * sizeof( double )); 591 592 if( !U ) 593 return CV_OUTOFMEM_ERR; 594 595 num3 = num * 3; 596 597 while( !REAL_ZERO( new_norm - old_norm )) 598 { 599 600 if( its++ > 1e+2 ) 601 { 602 603 cvFree( &U ); 604 return CV_BADFACTOR_ERR; 605 } /* if */ 606 607 old_norm = new_norm; 608 609 for( i3 = 0, i9 = 0; i3 < num3; i3 += 3, i9 += 9 ) 610 { 611 612 l1 = F[0] * mr[i3] + F[1] * mr[i3 + 1] + F[2]; 613 l2 = F[3] * mr[i3] + F[4] * mr[i3 + 1] + F[5]; 614 615 if( REAL_ZERO( l1 ) && REAL_ZERO( l2 )) 616 { 617 618 cvFree( &U ); 619 return CV_BADFACTOR_ERR; 620 } /* if */ 621 622 w = 1 / (l1 * l1 + l2 * l2); 623 624 l1 = F[0] * ml[i3] + F[3] * ml[i3 + 1] + F[6]; 625 l2 = F[1] * ml[i3] + F[4] * ml[i3 + 1] + F[7]; 626 627 if( REAL_ZERO( l1 ) && REAL_ZERO( l2 )) 628 { 629 630 cvFree( &U ); 631 return CV_BADFACTOR_ERR; 632 } /* if */ 633 634 w += 1 / (l1 * l1 + l2 * l2); 635 w = sqrt( w ); 636 637 for( j = 0; j < 9; j++ ) 638 { 639 640 U[i9 + j] = w * (double) ml[i3 + j / 3] * (double) mr[i3 + j % 3]; 641 } /* for */ 642 } /* for */ 643 644 new_norm = 0; 645 646 for( a = 0; a < num; a++ ) 647 { /* row */ 648 649 summ = 0; 650 651 for( t = 0; t < 9; t++ ) 652 { 653 654 summ += U[a * 9 + t] * F[t]; 655 } /* for */ 656 657 new_norm += summ * summ; 658 } /* for */ 659 660 new_norm = sqrt( new_norm ); 661 662 icvAnalyticPoints8( U, num, F ); 663 } /* while */ 664 665 cvFree( &U ); 666 return CV_NO_ERR; 667 668 } /* cs_Points8 */ 669 670 /*===========================================================================*/ 671 double 672 icvAnalyticPoints8( double *A, int num, double *F ) 673 { 674 double *U; 675 double V[8 * 8]; 676 double W[8]; 677 double *f; 678 double solution[9]; 679 double temp1[8 * 8]; 680 double *temp2; 681 double *A_short; 682 double norm, summ, best_norm; 683 int num8 = num * 8, num9 = num * 9; 684 int i, j, j8, j9, value, a, a8, a9, a_num, b, b8, t; 685 686 /* --------- Initialization data ------------------ */ 687 688 if( !A || num < 8 || !F ) 689 return -1; 690 691 best_norm = -1; 692 U = (double *) cvAlloc( (num8) * sizeof( double )); 693 694 if( !U ) 695 return -1; 696 697 f = (double *) cvAlloc( (num) * sizeof( double )); 698 699 if( !f ) 700 { 701 cvFree( &U ); 702 return -1; 703 } /* if */ 704 705 temp2 = (double *) cvAlloc( (num8) * sizeof( double )); 706 707 if( !temp2 ) 708 { 709 cvFree( &f ); 710 cvFree( &U ); 711 return -1; 712 } /* if */ 713 714 A_short = (double *) cvAlloc( (num8) * sizeof( double )); 715 716 if( !A_short ) 717 { 718 cvFree( &temp2 ); 719 cvFree( &f ); 720 cvFree( &U ); 721 return -1; 722 } /* if */ 723 724 for( i = 0; i < 8; i++ ) 725 { 726 for( j8 = 0, j9 = 0; j9 < num9; j8 += 8, j9 += 9 ) 727 { 728 A_short[j8 + i] = A[j9 + i + 1]; 729 } /* for */ 730 } /* for */ 731 732 for( i = 0; i < 9; i++ ) 733 { 734 735 for( j = 0, j8 = 0, j9 = 0; j < num; j++, j8 += 8, j9 += 9 ) 736 { 737 738 f[j] = -A[j9 + i]; 739 740 if( i ) 741 A_short[j8 + i - 1] = A[j9 + i - 1]; 742 } /* for */ 743 744 value = icvSingularValueDecomposition( num, 8, A_short, W, 1, U, 1, V ); 745 746 if( !value ) 747 { /* ----------- computing the solution ----------- */ 748 749 /* ----------- W = W(-1) ----------- */ 750 for( j = 0; j < 8; j++ ) 751 { 752 if( !REAL_ZERO( W[j] )) 753 W[j] = 1 / W[j]; 754 } /* for */ 755 756 /* ----------- temp1 = V * W(-1) ----------- */ 757 for( a8 = 0; a8 < 64; a8 += 8 ) 758 { /* row */ 759 for( b = 0; b < 8; b++ ) 760 { /* column */ 761 temp1[a8 + b] = V[a8 + b] * W[b]; 762 } /* for */ 763 } /* for */ 764 765 /* ----------- temp2 = V * W(-1) * U(T) ----------- */ 766 for( a8 = 0, a_num = 0; a8 < 64; a8 += 8, a_num += num ) 767 { /* row */ 768 for( b = 0, b8 = 0; b < num; b++, b8 += 8 ) 769 { /* column */ 770 771 temp2[a_num + b] = 0; 772 773 for( t = 0; t < 8; t++ ) 774 { 775 776 temp2[a_num + b] += temp1[a8 + t] * U[b8 + t]; 777 } /* for */ 778 } /* for */ 779 } /* for */ 780 781 /* ----------- solution = V * W(-1) * U(T) * f ----------- */ 782 for( a = 0, a_num = 0; a < 8; a++, a_num += num ) 783 { /* row */ 784 for( b = 0; b < num; b++ ) 785 { /* column */ 786 787 solution[a] = 0; 788 789 for( t = 0; t < num && W[a]; t++ ) 790 { 791 solution[a] += temp2[a_num + t] * f[t]; 792 } /* for */ 793 } /* for */ 794 } /* for */ 795 796 for( a = 8; a > 0; a-- ) 797 { 798 799 if( a == i ) 800 break; 801 solution[a] = solution[a - 1]; 802 } /* for */ 803 804 solution[a] = 1; 805 806 norm = 0; 807 808 for( a9 = 0; a9 < num9; a9 += 9 ) 809 { /* row */ 810 811 summ = 0; 812 813 for( t = 0; t < 9; t++ ) 814 { 815 816 summ += A[a9 + t] * solution[t]; 817 } /* for */ 818 819 norm += summ * summ; 820 } /* for */ 821 822 norm = sqrt( norm ); 823 824 if( best_norm == -1 || norm < best_norm ) 825 { 826 827 for( j = 0; j < 9; j++ ) 828 F[j] = solution[j]; 829 830 best_norm = norm; 831 } /* if */ 832 } /* if */ 833 } /* for */ 834 835 cvFree( &A_short ); 836 cvFree( &temp2 ); 837 cvFree( &f ); 838 cvFree( &U ); 839 840 return best_norm; 841 842 } /* cs_AnalyticPoints8 */ 843 844 /*===========================================================================*/ 845 CvStatus 846 icvRank2Constraint( double *F ) 847 { 848 double U[9], V[9], W[3]; 849 double aW[3]; 850 int i, i3, j, j3, t; 851 852 if( F == 0 ) 853 return CV_BADFACTOR_ERR; 854 855 if( icvSingularValueDecomposition( 3, 3, F, W, 1, U, 1, V )) 856 return CV_BADFACTOR_ERR; 857 858 aW[0] = fabs(W[0]); 859 aW[1] = fabs(W[1]); 860 aW[2] = fabs(W[2]); 861 862 if( aW[0] < aW[1] ) 863 { 864 if( aW[0] < aW[2] ) 865 { 866 867 if( REAL_ZERO( W[0] )) 868 return CV_NO_ERR; 869 else 870 W[0] = 0; 871 } 872 else 873 { 874 875 if( REAL_ZERO( W[2] )) 876 return CV_NO_ERR; 877 else 878 W[2] = 0; 879 } /* if */ 880 } 881 else 882 { 883 884 if( aW[1] < aW[2] ) 885 { 886 887 if( REAL_ZERO( W[1] )) 888 return CV_NO_ERR; 889 else 890 W[1] = 0; 891 } 892 else 893 { 894 if( REAL_ZERO( W[2] )) 895 return CV_NO_ERR; 896 else 897 W[2] = 0; 898 } /* if */ 899 } /* if */ 900 901 for( i = 0; i < 3; i++ ) 902 { 903 for( j3 = 0; j3 < 9; j3 += 3 ) 904 { 905 U[j3 + i] *= W[i]; 906 } /* for */ 907 } /* for */ 908 909 for( i = 0, i3 = 0; i < 3; i++, i3 += 3 ) 910 { 911 for( j = 0, j3 = 0; j < 3; j++, j3 += 3 ) 912 { 913 914 F[i3 + j] = 0; 915 916 for( t = 0; t < 3; t++ ) 917 { 918 F[i3 + j] += U[i3 + t] * V[j3 + t]; 919 } /* for */ 920 } /* for */ 921 } /* for */ 922 923 return CV_NO_ERR; 924 } /* cs_Rank2Constraint */ 925 926 927 /*===========================================================================*/ 928 929 int 930 icvSingularValueDecomposition( int M, 931 int N, 932 double *A, 933 double *W, int get_U, double *U, int get_V, double *V ) 934 { 935 int i = 0, j, k, l = 0, i1, k1, l1 = 0; 936 int iterations, error = 0, jN, iN, kN, lN = 0; 937 double *rv1; 938 double c, f, g, h, s, x, y, z, scale, anorm; 939 double af, ag, ah, t; 940 int MN = M * N; 941 int NN = N * N; 942 943 /* max_iterations - maximum number QR-iterations 944 cc - reduces requirements to number stitch (cc>1) 945 */ 946 947 int max_iterations = 100; 948 double cc = 100; 949 950 if( M < N ) 951 return N; 952 953 rv1 = (double *) cvAlloc( N * sizeof( double )); 954 955 if( rv1 == 0 ) 956 return N; 957 958 for( iN = 0; iN < MN; iN += N ) 959 { 960 for( j = 0; j < N; j++ ) 961 U[iN + j] = A[iN + j]; 962 } /* for */ 963 964 /* Adduction to bidiagonal type (transformations of reflection). 965 Bidiagonal matrix is located in W (diagonal elements) 966 and in rv1 (upperdiagonal elements) 967 */ 968 969 g = 0; 970 scale = 0; 971 anorm = 0; 972 973 for( i = 0, iN = 0; i < N; i++, iN += N ) 974 { 975 976 l = i + 1; 977 lN = iN + N; 978 rv1[i] = scale * g; 979 980 /* Multiplyings on the left */ 981 982 g = 0; 983 s = 0; 984 scale = 0; 985 986 for( kN = iN; kN < MN; kN += N ) 987 scale += fabs( U[kN + i] ); 988 989 if( !REAL_ZERO( scale )) 990 { 991 992 for( kN = iN; kN < MN; kN += N ) 993 { 994 995 U[kN + i] /= scale; 996 s += U[kN + i] * U[kN + i]; 997 } /* for */ 998 999 f = U[iN + i]; 1000 g = -sqrt( s ) * Sgn( f ); 1001 h = f * g - s; 1002 U[iN + i] = f - g; 1003 1004 for( j = l; j < N; j++ ) 1005 { 1006 1007 s = 0; 1008 1009 for( kN = iN; kN < MN; kN += N ) 1010 { 1011 1012 s += U[kN + i] * U[kN + j]; 1013 } /* for */ 1014 1015 f = s / h; 1016 1017 for( kN = iN; kN < MN; kN += N ) 1018 { 1019 1020 U[kN + j] += f * U[kN + i]; 1021 } /* for */ 1022 } /* for */ 1023 1024 for( kN = iN; kN < MN; kN += N ) 1025 U[kN + i] *= scale; 1026 } /* if */ 1027 1028 W[i] = scale * g; 1029 1030 /* Multiplyings on the right */ 1031 1032 g = 0; 1033 s = 0; 1034 scale = 0; 1035 1036 for( k = l; k < N; k++ ) 1037 scale += fabs( U[iN + k] ); 1038 1039 if( !REAL_ZERO( scale )) 1040 { 1041 1042 for( k = l; k < N; k++ ) 1043 { 1044 1045 U[iN + k] /= scale; 1046 s += (U[iN + k]) * (U[iN + k]); 1047 } /* for */ 1048 1049 f = U[iN + l]; 1050 g = -sqrt( s ) * Sgn( f ); 1051 h = f * g - s; 1052 U[i * N + l] = f - g; 1053 1054 for( k = l; k < N; k++ ) 1055 rv1[k] = U[iN + k] / h; 1056 1057 for( jN = lN; jN < MN; jN += N ) 1058 { 1059 1060 s = 0; 1061 1062 for( k = l; k < N; k++ ) 1063 s += U[jN + k] * U[iN + k]; 1064 1065 for( k = l; k < N; k++ ) 1066 U[jN + k] += s * rv1[k]; 1067 1068 } /* for */ 1069 1070 for( k = l; k < N; k++ ) 1071 U[iN + k] *= scale; 1072 } /* if */ 1073 1074 t = fabs( W[i] ); 1075 t += fabs( rv1[i] ); 1076 anorm = MAX( anorm, t ); 1077 } /* for */ 1078 1079 anorm *= cc; 1080 1081 /* accumulation of right transformations, if needed */ 1082 1083 if( get_V ) 1084 { 1085 1086 for( i = N - 1, iN = NN - N; i >= 0; i--, iN -= N ) 1087 { 1088 1089 if( i < N - 1 ) 1090 { 1091 1092 /* pass-by small g */ 1093 if( !REAL_ZERO( g )) 1094 { 1095 1096 for( j = l, jN = lN; j < N; j++, jN += N ) 1097 V[jN + i] = U[iN + j] / U[iN + l] / g; 1098 1099 for( j = l; j < N; j++ ) 1100 { 1101 1102 s = 0; 1103 1104 for( k = l, kN = lN; k < N; k++, kN += N ) 1105 s += U[iN + k] * V[kN + j]; 1106 1107 for( kN = lN; kN < NN; kN += N ) 1108 V[kN + j] += s * V[kN + i]; 1109 } /* for */ 1110 } /* if */ 1111 1112 for( j = l, jN = lN; j < N; j++, jN += N ) 1113 { 1114 V[iN + j] = 0; 1115 V[jN + i] = 0; 1116 } /* for */ 1117 } /* if */ 1118 1119 V[iN + i] = 1; 1120 g = rv1[i]; 1121 l = i; 1122 lN = iN; 1123 } /* for */ 1124 } /* if */ 1125 1126 /* accumulation of left transformations, if needed */ 1127 1128 if( get_U ) 1129 { 1130 1131 for( i = N - 1, iN = NN - N; i >= 0; i--, iN -= N ) 1132 { 1133 1134 l = i + 1; 1135 lN = iN + N; 1136 g = W[i]; 1137 1138 for( j = l; j < N; j++ ) 1139 U[iN + j] = 0; 1140 1141 /* pass-by small g */ 1142 if( !REAL_ZERO( g )) 1143 { 1144 1145 for( j = l; j < N; j++ ) 1146 { 1147 1148 s = 0; 1149 1150 for( kN = lN; kN < MN; kN += N ) 1151 s += U[kN + i] * U[kN + j]; 1152 1153 f = s / U[iN + i] / g; 1154 1155 for( kN = iN; kN < MN; kN += N ) 1156 U[kN + j] += f * U[kN + i]; 1157 } /* for */ 1158 1159 for( jN = iN; jN < MN; jN += N ) 1160 U[jN + i] /= g; 1161 } 1162 else 1163 { 1164 1165 for( jN = iN; jN < MN; jN += N ) 1166 U[jN + i] = 0; 1167 } /* if */ 1168 1169 U[iN + i] += 1; 1170 } /* for */ 1171 } /* if */ 1172 1173 /* Iterations QR-algorithm for bidiagonal matrixes 1174 W[i] - is the main diagonal 1175 rv1[i] - is the top diagonal, rv1[0]=0. 1176 */ 1177 1178 for( k = N - 1; k >= 0; k-- ) 1179 { 1180 1181 k1 = k - 1; 1182 iterations = 0; 1183 1184 for( ;; ) 1185 { 1186 1187 /* Cycle: checking a possibility of fission matrix */ 1188 for( l = k; l >= 0; l-- ) 1189 { 1190 1191 l1 = l - 1; 1192 1193 if( REAL_ZERO( rv1[l] ) || REAL_ZERO( W[l1] )) 1194 break; 1195 } /* for */ 1196 1197 if( !REAL_ZERO( rv1[l] )) 1198 { 1199 1200 /* W[l1] = 0, matrix possible to fission 1201 by clearing out rv1[l] */ 1202 1203 c = 0; 1204 s = 1; 1205 1206 for( i = l; i <= k; i++ ) 1207 { 1208 1209 f = s * rv1[i]; 1210 rv1[i] = c * rv1[i]; 1211 1212 /* Rotations are done before the end of the block, 1213 or when element in the line is finagle. 1214 */ 1215 1216 if( REAL_ZERO( f )) 1217 break; 1218 1219 g = W[i]; 1220 1221 /* Scaling prevents finagling H ( F!=0!) */ 1222 1223 af = fabs( f ); 1224 ag = fabs( g ); 1225 1226 if( af < ag ) 1227 h = ag * sqrt( 1 + (f / g) * (f / g) ); 1228 else 1229 h = af * sqrt( 1 + (f / g) * (f / g) ); 1230 1231 W[i] = h; 1232 c = g / h; 1233 s = -f / h; 1234 1235 if( get_U ) 1236 { 1237 1238 for( jN = 0; jN < MN; jN += N ) 1239 { 1240 1241 y = U[jN + l1]; 1242 z = U[jN + i]; 1243 U[jN + l1] = y * c + z * s; 1244 U[jN + i] = -y * s + z * c; 1245 } /* for */ 1246 } /* if */ 1247 } /* for */ 1248 } /* if */ 1249 1250 1251 /* Output in this place of program means, 1252 that rv1[L] = 0, matrix fissioned 1253 Iterations of the process of the persecution 1254 will be executed always for 1255 the bottom block ( from l before k ), 1256 with increase l possible. 1257 */ 1258 1259 z = W[k]; 1260 1261 if( l == k ) 1262 break; 1263 1264 /* Completion iterations: lower block 1265 became trivial ( rv1[K]=0) */ 1266 1267 if( iterations++ == max_iterations ) 1268 return k; 1269 1270 /* Shift is computed on the lowest order 2 minor. */ 1271 1272 x = W[l]; 1273 y = W[k1]; 1274 g = rv1[k1]; 1275 h = rv1[k]; 1276 1277 /* consequent fission prevents forming a machine zero */ 1278 f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2 * h) / y; 1279 1280 /* prevented overflow */ 1281 if( fabs( f ) > 1 ) 1282 { 1283 g = fabs( f ); 1284 g *= sqrt( 1 + (1 / f) * (1 / f) ); 1285 } 1286 else 1287 g = sqrt( f * f + 1 ); 1288 1289 f = ((x - z) * (x + z) + h * (y / (f + fabs( g ) * Sgn( f )) - h)) / x; 1290 c = 1; 1291 s = 1; 1292 1293 for( i1 = l; i1 <= k1; i1++ ) 1294 { 1295 1296 i = i1 + 1; 1297 g = rv1[i]; 1298 y = W[i]; 1299 h = s * g; 1300 g *= c; 1301 1302 /* Scaling at calculation Z prevents its clearing, 1303 however if F and H both are zero - pass-by of fission on Z. 1304 */ 1305 1306 af = fabs( f ); 1307 ah = fabs( h ); 1308 1309 if( af < ah ) 1310 z = ah * sqrt( 1 + (f / h) * (f / h) ); 1311 1312 else 1313 { 1314 1315 z = 0; 1316 if( !REAL_ZERO( af )) 1317 z = af * sqrt( 1 + (h / f) * (h / f) ); 1318 } /* if */ 1319 1320 rv1[i1] = z; 1321 1322 /* if Z=0, the rotation is free. */ 1323 if( !REAL_ZERO( z )) 1324 { 1325 1326 c = f / z; 1327 s = h / z; 1328 } /* if */ 1329 1330 f = x * c + g * s; 1331 g = -x * s + g * c; 1332 h = y * s; 1333 y *= c; 1334 1335 if( get_V ) 1336 { 1337 1338 for( jN = 0; jN < NN; jN += N ) 1339 { 1340 1341 x = V[jN + i1]; 1342 z = V[jN + i]; 1343 V[jN + i1] = x * c + z * s; 1344 V[jN + i] = -x * s + z * c; 1345 } /* for */ 1346 } /* if */ 1347 1348 af = fabs( f ); 1349 ah = fabs( h ); 1350 1351 if( af < ah ) 1352 z = ah * sqrt( 1 + (f / h) * (f / h) ); 1353 else 1354 { 1355 1356 z = 0; 1357 if( !REAL_ZERO( af )) 1358 z = af * sqrt( 1 + (h / f) * (h / f) ); 1359 } /* if */ 1360 1361 W[i1] = z; 1362 1363 if( !REAL_ZERO( z )) 1364 { 1365 1366 c = f / z; 1367 s = h / z; 1368 } /* if */ 1369 1370 f = c * g + s * y; 1371 x = -s * g + c * y; 1372 1373 if( get_U ) 1374 { 1375 1376 for( jN = 0; jN < MN; jN += N ) 1377 { 1378 1379 y = U[jN + i1]; 1380 z = U[jN + i]; 1381 U[jN + i1] = y * c + z * s; 1382 U[jN + i] = -y * s + z * c; 1383 } /* for */ 1384 } /* if */ 1385 } /* for */ 1386 1387 rv1[l] = 0; 1388 rv1[k] = f; 1389 W[k] = x; 1390 } /* for */ 1391 1392 if( z < 0 ) 1393 { 1394 1395 W[k] = -z; 1396 1397 if( get_V ) 1398 { 1399 1400 for( jN = 0; jN < NN; jN += N ) 1401 V[jN + k] *= -1; 1402 } /* if */ 1403 } /* if */ 1404 } /* for */ 1405 1406 cvFree( &rv1 ); 1407 1408 return error; 1409 1410 } /* vm_SingularValueDecomposition */ 1411 1412 /*========================================================================*/ 1413 1414 /* Obsolete functions. Just for ViewMorping */ 1415 /*=====================================================================================*/ 1416 1417 int 1418 icvGaussMxN( double *A, double *B, int M, int N, double **solutions ) 1419 { 1420 int *variables; 1421 int row, swapi, i, i_best = 0, j, j_best = 0, t; 1422 double swapd, ratio, bigest; 1423 1424 if( !A || !B || !M || !N ) 1425 return -1; 1426 1427 variables = (int *) cvAlloc( (size_t) N * sizeof( int )); 1428 1429 if( variables == 0 ) 1430 return -1; 1431 1432 for( i = 0; i < N; i++ ) 1433 { 1434 variables[i] = i; 1435 } /* for */ 1436 1437 /* ----- Direct way ----- */ 1438 1439 for( row = 0; row < M; row++ ) 1440 { 1441 1442 bigest = 0; 1443 1444 for( j = row; j < M; j++ ) 1445 { /* search non null element */ 1446 for( i = row; i < N; i++ ) 1447 { 1448 double a = fabs( A[j * N + i] ), b = fabs( bigest ); 1449 if( a > b ) 1450 { 1451 bigest = A[j * N + i]; 1452 i_best = i; 1453 j_best = j; 1454 } /* if */ 1455 } /* for */ 1456 } /* for */ 1457 1458 if( REAL_ZERO( bigest )) 1459 break; /* if all shank elements are null */ 1460 1461 if( j_best - row ) 1462 { 1463 1464 for( t = 0; t < N; t++ ) 1465 { /* swap a rows */ 1466 1467 swapd = A[row * N + t]; 1468 A[row * N + t] = A[j_best * N + t]; 1469 A[j_best * N + t] = swapd; 1470 } /* for */ 1471 1472 swapd = B[row]; 1473 B[row] = B[j_best]; 1474 B[j_best] = swapd; 1475 } /* if */ 1476 1477 if( i_best - row ) 1478 { 1479 1480 for( t = 0; t < M; t++ ) 1481 { /* swap a columns */ 1482 1483 swapd = A[t * N + i_best]; 1484 A[t * N + i_best] = A[t * N + row]; 1485 A[t * N + row] = swapd; 1486 } /* for */ 1487 1488 swapi = variables[row]; 1489 variables[row] = variables[i_best]; 1490 variables[i_best] = swapi; 1491 } /* if */ 1492 1493 for( i = row + 1; i < M; i++ ) 1494 { /* recounting A and B */ 1495 1496 ratio = -A[i * N + row] / A[row * N + row]; 1497 B[i] += B[row] * ratio; 1498 1499 for( j = N - 1; j >= row; j-- ) 1500 { 1501 1502 A[i * N + j] += A[row * N + j] * ratio; 1503 } /* for */ 1504 } /* for */ 1505 } /* for */ 1506 1507 if( row < M ) 1508 { /* if rank(A)<M */ 1509 1510 for( j = row; j < M; j++ ) 1511 { 1512 if( !REAL_ZERO( B[j] )) 1513 { 1514 1515 cvFree( &variables ); 1516 return -1; /* if system is antithetic */ 1517 } /* if */ 1518 } /* for */ 1519 1520 M = row; /* decreasing size of the task */ 1521 } /* if */ 1522 1523 /* ----- Reverse way ----- */ 1524 1525 if( M < N ) 1526 { /* if solution are not exclusive */ 1527 1528 *solutions = (double *) cvAlloc( ((N - M + 1) * N) * sizeof( double )); 1529 1530 if( *solutions == 0 ) 1531 { 1532 cvFree( &variables ); 1533 return -1; 1534 } 1535 1536 1537 for( t = M; t <= N; t++ ) 1538 { 1539 for( j = M; j < N; j++ ) 1540 { 1541 1542 (*solutions)[(t - M) * N + variables[j]] = (double) (t == j); 1543 } /* for */ 1544 1545 for( i = M - 1; i >= 0; i-- ) 1546 { /* finding component of solution */ 1547 1548 if( t < N ) 1549 { 1550 (*solutions)[(t - M) * N + variables[i]] = 0; 1551 } 1552 else 1553 { 1554 (*solutions)[(t - M) * N + variables[i]] = B[i] / A[i * N + i]; 1555 } /* if */ 1556 1557 for( j = i + 1; j < N; j++ ) 1558 { 1559 1560 (*solutions)[(t - M) * N + variables[i]] -= 1561 (*solutions)[(t - M) * N + variables[j]] * A[i * N + j] / A[i * N + i]; 1562 } /* for */ 1563 } /* for */ 1564 } /* for */ 1565 1566 cvFree( &variables ); 1567 return N - M; 1568 } /* if */ 1569 1570 *solutions = (double *) cvAlloc( (N) * sizeof( double )); 1571 1572 if( solutions == 0 ) 1573 return -1; 1574 1575 for( i = N - 1; i >= 0; i-- ) 1576 { /* finding exclusive solution */ 1577 1578 (*solutions)[variables[i]] = B[i] / A[i * N + i]; 1579 1580 for( j = i + 1; j < N; j++ ) 1581 { 1582 1583 (*solutions)[variables[i]] -= 1584 (*solutions)[variables[j]] * A[i * N + j] / A[i * N + i]; 1585 } /* for */ 1586 } /* for */ 1587 1588 cvFree( &variables ); 1589 return 0; 1590 1591 } /* icvGaussMxN */ 1592 1593 /*=====================================================================================*/ 1594 /* 1595 static CvStatus 1596 icvGetCoof( double *f1, double *f2, double *a2, double *a1, double *a0 ) 1597 { 1598 double G[9], a3; 1599 int i; 1600 1601 if( !f1 || !f2 || !a0 || !a1 || !a2 ) 1602 return CV_BADFACTOR_ERR; 1603 1604 for( i = 0; i < 9; i++ ) 1605 { 1606 1607 G[i] = f1[i] - f2[i]; 1608 } 1609 1610 a3 = icvDet( G ); 1611 1612 if( REAL_ZERO( a3 )) 1613 return CV_BADFACTOR_ERR; 1614 1615 *a2 = 0; 1616 *a1 = 0; 1617 *a0 = icvDet( f2 ); 1618 1619 for( i = 0; i < 9; i++ ) 1620 { 1621 1622 *a2 += f2[i] * icvMinor( G, (int) (i % 3), (int) (i / 3) ); 1623 *a1 += G[i] * icvMinor( f2, (int) (i % 3), (int) (i / 3) ); 1624 } 1625 1626 *a0 /= a3; 1627 *a1 /= a3; 1628 *a2 /= a3; 1629 1630 return CV_NO_ERR; 1631 1632 }*/ /* icvGetCoof */ 1633 1634 1635 1636 /*======================================================================================*/ 1637 1638 /*F/////////////////////////////////////////////////////////////////////////////////////// 1639 // Name: icvLMedS7 1640 // Purpose: 1641 // 1642 // 1643 // Context: 1644 // Parameters: 1645 // 1646 // 1647 // 1648 // 1649 // 1650 // 1651 // 1652 // Returns: 1653 // CV_NO_ERR if all Ok or error code 1654 // Notes: 1655 //F*/ 1656 1657 CvStatus 1658 icvLMedS7( int *points1, int *points2, CvMatrix3 * matrix ) 1659 { /* Incorrect realization */ 1660 CvStatus error = CV_NO_ERR; 1661 1662 /* int amount; */ 1663 matrix = matrix; 1664 points1 = points1; 1665 points2 = points2; 1666 1667 /* error = cs_Point7( points1, points2, matrix ); */ 1668 /* error = icvPoint7 ( points1, points2, matrix,&amount ); */ 1669 return error; 1670 1671 } /* icvLMedS7 */ 1672 1673 1674 /*======================================================================================*/ 1675 /*F/////////////////////////////////////////////////////////////////////////////////////// 1676 // Name: icvPoint7 1677 // Purpose: 1678 // 1679 // 1680 // Context: 1681 // Parameters: 1682 // 1683 // 1684 // 1685 // 1686 // 1687 // 1688 // 1689 // Returns: 1690 // CV_NO_ERR if all Ok or error code 1691 // Notes: 1692 //F*/ 1693 1694 CvStatus 1695 icvPoint7( int *ml, int *mr, double *F, int *amount ) 1696 { 1697 double A[63], B[7]; 1698 double *solutions; 1699 double a2, a1, a0; 1700 double squares[6]; 1701 int i, j; 1702 1703 /* int amount; */ 1704 /* float* F; */ 1705 1706 CvStatus error = CV_BADFACTOR_ERR; 1707 1708 /* F = (float*)matrix->m; */ 1709 1710 if( !ml || !mr || !F ) 1711 return CV_BADFACTOR_ERR; 1712 1713 for( i = 0; i < 7; i++ ) 1714 { 1715 for( j = 0; j < 9; j++ ) 1716 { 1717 1718 A[i * 9 + j] = (double) ml[i * 3 + j / 3] * (double) mr[i * 3 + j % 3]; 1719 } /* for */ 1720 B[i] = 0; 1721 } /* for */ 1722 1723 *amount = 0; 1724 1725 if( icvGaussMxN( A, B, 7, 9, &solutions ) == 2 ) 1726 { 1727 if( icvGetCoef( solutions, solutions + 9, &a2, &a1, &a0 ) == CV_NO_ERR ) 1728 { 1729 icvCubic( a2, a1, a0, squares ); 1730 1731 for( i = 0; i < 1; i++ ) 1732 { 1733 1734 if( REAL_ZERO( squares[i * 2 + 1] )) 1735 { 1736 1737 for( j = 0; j < 9; j++ ) 1738 { 1739 1740 F[*amount + j] = (float) (squares[i] * solutions[j] + 1741 (1 - squares[i]) * solutions[j + 9]); 1742 } /* for */ 1743 1744 *amount += 9; 1745 1746 error = CV_NO_ERR; 1747 } /* if */ 1748 } /* for */ 1749 1750 cvFree( &solutions ); 1751 return error; 1752 } 1753 else 1754 { 1755 cvFree( &solutions ); 1756 } /* if */ 1757 1758 } 1759 else 1760 { 1761 cvFree( &solutions ); 1762 } /* if */ 1763 1764 return error; 1765 } /* icvPoint7 */ 1766 1767
