git.maemo.org Git - opencv/blob - cvaux/src/cvlmeds.cpp

   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