17
|
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;
|