|
1 #====================================================================== |
|
2 # L I B L O O P F I T . P L |
|
3 # doc: Thu Mar 12 08:05:26 2015 |
|
4 # dlm: Sun Mar 15 19:46:49 2015 |
|
5 # (c) 2015 A.M. Thurnherr |
|
6 # uE-Info: 47 22 NIL 0 0 72 2 2 4 NIL ofnI |
|
7 #====================================================================== |
|
8 |
|
9 # HISTORY: |
|
10 # Mar 12, 2015: - created |
|
11 # Mar 15, 2015: - added [libQZ.pl] |
|
12 |
|
13 require "$ANTS/nrutil.pl"; |
|
14 require "$ANTS/libSVD.pl"; # for circular fit |
|
15 require "$ANTS/libQZ.pl"; # for ellipse fit |
|
16 |
|
17 #---------------------------------------------------------------------- |
|
18 # fitCircle(\@x,\@y) => ($r,$cx,$cy) |
|
19 # \@x,\@y references to list of data values |
|
20 # $r circle radius |
|
21 # $cx,$cy circle center |
|
22 #---------------------------------------------------------------------- |
|
23 |
|
24 sub fitCircle($$) |
|
25 { |
|
26 my($xR,$yR) = @_; |
|
27 my(@A,@V,@W); |
|
28 my($nSamp) = scalar(@{$xR}); |
|
29 my($nParam) = 3; |
|
30 |
|
31 matrix(\@A,1,$nSamp,1,$nParam); # rows, then columns |
|
32 matrix(\@V,1,$nParam,1,$nParam); # 3x3 matrix for circle fitting |
|
33 vector(\@W,1,$nParam); # diag matrix of singular values output as vector |
|
34 vector(\@b,1,$nSamp); # rhs |
|
35 vector(\@coeff,1,$nParam); # fit coefficients |
|
36 |
|
37 for ($r=1; $r<=$nSamp; $r++) { # circle equation c1*x + c2*y + c3 = -(x^2 + y^2) |
|
38 $A[$r][1] = $xR->[$r-1]; # r = sqrt((c1^2 + c2^2)/4 - c3) |
|
39 $A[$r][2] = $yR->[$r-1]; # cx = -c1/2 |
|
40 $A[$r][3] = 1; # cy = -c2/2 |
|
41 $b[$r] = -($xR->[$r-1]**2+$yR->[$r-1]**2); |
|
42 } |
|
43 |
|
44 svdcmp(\@A,\@W,\@V); # solve Ax = b, with x == coeff |
|
45 svbksb(\@A,\@W,\@V,\@b,\@coeff); |
|
46 |
|
47 return ($coeff[1],$coeff[2],$coeff[3]); |
|
48 my($r) = sqrt(($coeff[1]**2+$coeff[2]**2)/4-$coeff[3]); |
|
49 my($cx) = -0.5*$coeff[1]; |
|
50 my($cy) = -0.5*$coeff[2]; |
|
51 return ($r,$cx,$cy); |
|
52 } |
|
53 |
|
54 #---------------------------------------------------------------------- |
|
55 # fitEllipse(\@x,\@y) => ? |
|
56 # \@x,\@y references to list of data values |
|
57 # |
|
58 # Direct Least Square Fitting of Ellipses |
|
59 # Andrew Fitzgibbon, Maurizio Pilu, and Robert B. Fisher, MAY 1999 |
|
60 # IEEE transactions on pattern analysis and machine intelligence 21(5): 476-480 |
|
61 #---------------------------------------------------------------------- |
|
62 |
|
63 sub fitEllipse($$) |
|
64 { |
|
65 my($xR,$yR) = @_; |
|
66 my($nSamp) = scalar(@{$xR}); |
|
67 |
|
68 # design matrix D = [ x.*x x.*y y.*y x y ones(size(x)) ]; |
|
69 my(@D); |
|
70 for (my($r)=0; $r<$nSamp; $r++) { |
|
71 $D[$r][0] = $xR->[$r]**2; |
|
72 $D[$r][1] = $xR->[$r] * $yR->[$r]; |
|
73 $D[$r][2] = $yR->[$r]**2; |
|
74 $D[$r][3] = $xR->[$r]; |
|
75 $D[$r][4] = $yR->[$r]; |
|
76 $D[$r][5] = 1; |
|
77 } |
|
78 |
|
79 # scatter matrix S = D' * D; |
|
80 my(@S); |
|
81 for (my($i)=0; $i<6; $i++) { |
|
82 for (my($j)=0; $j<6; $j++) { |
|
83 for (my($k)=0; $k<$nSamp; $k++) { |
|
84 $S[$i][$j] += $D[$k][$j] * $D[$k][$i]; |
|
85 } |
|
86 } |
|
87 } |
|
88 |
|
89 # 6x6 constraint matrix C |
|
90 my(@C); |
|
91 for (my($r)=0; $r<6; $r++) { |
|
92 for (my($c)=0; $c<6; $c++) { |
|
93 $C[$r][$c] = 0; |
|
94 } |
|
95 } |
|
96 $C[0][2] = -2; $C[1][1] = 1; $C[2][0] = -2; |
|
97 |
|
98 # solve generalized eigensystem |
|
99 my(@geRe,@geImg,@geVec); |
|
100 eig(\@S,\@C,\@geRe,\@geImg,\@geVec); |
|
101 |
|
102 # find only negative eigenvalue |
|
103 my($i); |
|
104 for ($i=0; $i<@geRe; $i++) { |
|
105 last if (numberp($geRe[$i]) && ($geRe[$i] < 0)); |
|
106 } |
|
107 croak("internal error") unless ($geRe[$i] < 0); |
|
108 |
|
109 # get fitted parameters |
|
110 return @{$geVec[$i]}; |
|
111 |
|
112 } |
|
113 |
|
114 1; |