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