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
icvLMedS(int * points1,int * points2,int numPoints,CvMatrix3 * fundamentalMatrix)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
icvChoose7(int * ml,int * mr,int num,int * ml7,int * mr7)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
icvCubic(double a2,double a1,double a0,double * squares)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
icvDet(double * M)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
icvMinor(double * M,int x,int y)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
icvGetCoef(double * f1,double * f2,double * a2,double * a1,double * a0)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
icvMedian(int * ml,int * mr,int num,double * F)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
icvSort(double * array,int length)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
icvBoltingPoints(int * ml,int * mr,int num,double * F,double Mj,int ** new_ml,int ** new_mr,int * new_num)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
icvPoints8(int * ml,int * mr,int num,double * F)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
icvAnalyticPoints8(double * A,int num,double * F)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
icvRank2Constraint(double * F)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
icvSingularValueDecomposition(int M,int N,double * A,double * W,int get_U,double * U,int get_V,double * V)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
icvGaussMxN(double * A,double * B,int M,int N,double ** solutions)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
icvLMedS7(int * points1,int * points2,CvMatrix3 * matrix)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
icvPoint7(int * ml,int * mr,double * F,int * amount)1695 icvPoint7( int *ml, int *mr, double *F, int *amount )
1696 {
1697 double A[63], B[7];
1698 double *solutions;
1699 double a2, a1, a0;
1700 double squares[6];
1701 int i, j;
1702
1703 /* int amount; */
1704 /* float* F; */
1705
1706 CvStatus error = CV_BADFACTOR_ERR;
1707
1708 /* F = (float*)matrix->m; */
1709
1710 if( !ml || !mr || !F )
1711 return CV_BADFACTOR_ERR;
1712
1713 for( i = 0; i < 7; i++ )
1714 {
1715 for( j = 0; j < 9; j++ )
1716 {
1717
1718 A[i * 9 + j] = (double) ml[i * 3 + j / 3] * (double) mr[i * 3 + j % 3];
1719 } /* for */
1720 B[i] = 0;
1721 } /* for */
1722
1723 *amount = 0;
1724
1725 if( icvGaussMxN( A, B, 7, 9, &solutions ) == 2 )
1726 {
1727 if( icvGetCoef( solutions, solutions + 9, &a2, &a1, &a0 ) == CV_NO_ERR )
1728 {
1729 icvCubic( a2, a1, a0, squares );
1730
1731 for( i = 0; i < 1; i++ )
1732 {
1733
1734 if( REAL_ZERO( squares[i * 2 + 1] ))
1735 {
1736
1737 for( j = 0; j < 9; j++ )
1738 {
1739
1740 F[*amount + j] = (float) (squares[i] * solutions[j] +
1741 (1 - squares[i]) * solutions[j + 9]);
1742 } /* for */
1743
1744 *amount += 9;
1745
1746 error = CV_NO_ERR;
1747 } /* if */
1748 } /* for */
1749
1750 cvFree( &solutions );
1751 return error;
1752 }
1753 else
1754 {
1755 cvFree( &solutions );
1756 } /* if */
1757
1758 }
1759 else
1760 {
1761 cvFree( &solutions );
1762 } /* if */
1763
1764 return error;
1765 } /* icvPoint7 */
1766
1767