1 /*=========================================================================
6 Copyright (c) CREATIS (Centre de Recherche et d'Applications en Traitement de
7 l'Image). All rights reserved. See Doc/License.txt or
8 http://www.creatis.insa-lyon.fr/Public/Gdcm/License.html for details.
10 This software is distributed WITHOUT ANY WARRANTY; without even
11 the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
12 PURPOSE. See the above copyright notices for more information.
14 =========================================================================*/
16 #include "clitkEllipse.h"
18 typedef itk::Vector<double,2> Vector2d;
19 typedef itk::Vector<double,6> Vector6d;
20 typedef itk::Matrix<double,6,6> Matrix6x6d;
21 typedef itk::Matrix<double,3,3> Matrix3x3d;
23 //---------------------------------------------------------------------
24 clitk::Ellipse::Ellipse():a((*this)[0]), b((*this)[1]),
25 c((*this)[2]), d((*this)[3]),
26 e((*this)[4]), f((*this)[5]) {
28 //---------------------------------------------------------------------
31 //---------------------------------------------------------------------
32 clitk::Ellipse::Ellipse(const Ellipse & e):a((*this)[0]), b((*this)[1]),
33 c((*this)[2]), d((*this)[3]),
34 e((*this)[4]), f((*this)[5]) {
35 for(int i=0; i<7; i++) (*this)[i] = e[i];
38 //---------------------------------------------------------------------
41 //---------------------------------------------------------------------
42 Vector2d clitk::Ellipse::ComputeCenter() {
43 // See http://mathworld.wolfram.com/Ellipse.html
46 center[0] = (2*c*d - b*e)/(b*b-4*a*c);
47 center[1] = (2*a*e - b*d)/(b*b-4*a*c);
50 //---------------------------------------------------------------------
53 //---------------------------------------------------------------------
54 Vector2d clitk::Ellipse::ComputeSemiAxeLengths() {
55 // See http://www.geometrictools.com/Documentation/InformationAboutEllipses.pdf
57 Vector2d center = ComputeCenter();
58 double & k1 = center[0];
59 double & k2 = center[1];
60 double mu = 1.0/(a*k1*k1 + b*k1*k2 + c*k2*k2 - f);
62 double m12 = mu * 0.5 * b;
64 double l1 = ( (m11+m22) + sqrt((m11-m22)*(m11-m22)+4*m12*m12) )/2.0;
66 axis[1] = 1.0/sqrt(l1);
67 double l2 = ((m11+m22)-sqrt((m11-m22)*(m11-m22)+4*m12*m12))/2.0;
69 axis[0] = 1.0/sqrt(l2);
72 //---------------------------------------------------------------------
75 //---------------------------------------------------------------------
76 double clitk::Ellipse::ComputeAngleInRad() {
77 // See http://www.geometrictools.com/Documentation/InformationAboutEllipses.pdf
80 if (a<c) { theta = 0; }
81 else { theta = 0.5*M_PI; }
85 theta = 0.5*atan(-b/(c-a));
88 theta = M_PI/2+0.5*atan(-b/(c-a));
93 //---------------------------------------------------------------------
96 //---------------------------------------------------------------------
97 void clitk::Ellipse::InitialiseEllipseFitting(double eta, unsigned int n, clitk::Signal & inputX, clitk::Signal & inputY) {
103 // Initialise ellipse to global fit
104 std::vector<double> extremaX(2);
105 std::vector<double> extremaY(2);
108 extremaX[0] = extremaY[0] = numeric_limits<double>::max();
109 extremaX[1] = extremaY[1] = -numeric_limits<double>::max();
110 for(unsigned int i=0; i<n; i++) {
111 if (inputX[i] < extremaX[0]) extremaX[0] = inputX[i];
112 if (inputX[i] > extremaX[1]) extremaX[1] = inputX[i];
113 if (inputY[i] < extremaY[0]) extremaY[0] = inputY[i];
114 if (inputY[i] > extremaY[1]) extremaY[1] = inputY[i];
121 // initialisation with an ellipse more small than real points extends
122 double ax1 = (extremaX[1]-extremaX[0])/2.0;
123 double ax2 = (extremaY[1]-extremaY[0])/2.0;
124 if (ax2 >= ax1) ax2 = 0.99*ax1;
125 SetCenterAndSemiAxes(x0, y0, ax1, ax2);
127 // Initialisation of C
129 C(0,0) = 0; C(0,1) = 0; C(0,2) = 2;
130 C(1,0) = 0; C(1,1) = -1; C(1,2) = 0;
131 C(2,0) = 2; C(2,1) = 0; C(2,2) = 0;
132 Ct = C.GetVnlMatrix().transpose();
135 UpdateSMatrix(0,n, inputX, inputY);
137 //---------------------------------------------------------------------
140 //---------------------------------------------------------------------
141 void clitk::Ellipse::CopyBlock(Matrix6x6d & out, const Matrix6x6d & in,
142 int ox, int oy, int l, double factor) {
143 for(int x=ox; x<ox+l; x++)
144 for(int y=oy; y<oy+l; y++) {
145 out(x,y) = factor * in(x,y);
148 //---------------------------------------------------------------------
151 //---------------------------------------------------------------------
152 void clitk::Ellipse::CopyBlock(Matrix6x6d & out, const Matrix3x3d & in,
153 int ox, int oy, double factor) {
154 for(int x=0; x<3; x++)
155 for(int y=0; y<3; y++) {
156 out(ox+x,oy+y) = factor * in(x,y);
159 //---------------------------------------------------------------------
162 //---------------------------------------------------------------------
163 Matrix3x3d clitk::Ellipse::GetBlock3x3(const Matrix6x6d & M, int x, int y) {
165 for(int i=0; i<3; i++) {
166 for(int j=0; j<3; j++) {
172 //---------------------------------------------------------------------
175 //---------------------------------------------------------------------
176 double clitk::Ellipse::EllipseFittingNextIteration() {
177 Vector6d & current = (*this);
179 // Normalize current vector. This allow to have a decreasing
180 // residual r (no need to optimize)
181 GetVnlVector().normalize();
182 double r = (St*current)*current;
184 // Temporary parameters
188 an = Sinv * C * current;
189 double num = (Wt*current)*current; // anT W an = (WT an) an
190 double denom = (Ct*current)*current;
191 an = (mEta * num/denom) * an;
192 an += (1.0-mEta)*current;
193 SetVnlVector(an.GetVnlVector());
198 //---------------------------------------------------------------------
201 //---------------------------------------------------------------------
202 void clitk::Ellipse::SetCenterAndSemiAxes(double x0, double y0,
203 double r1, double r2) {
208 std::cerr << "Error major axis should be r1, not r2 (r1=" << r1
209 << " r2=" << r2 << ")" << std::endl;
212 d = (-2.0*x0)/(r1*r1);
213 e = (-2.0*y0)/(r2*r2);
214 f = (x0*x0)/(r1*r1) + (y0*y0)/(r2*r2) - 1.0;
215 GetVnlVector().normalize();
217 //---------------------------------------------------------------------
220 //---------------------------------------------------------------------
221 void clitk::Ellipse::UpdateSMatrix(unsigned int begin, unsigned int n,
222 clitk::Signal & inputX, clitk::Signal & inputY) {
223 // Initialisation of z
226 for(unsigned int i=begin; i<begin+n; i++) {
227 z[j][0] = inputX[i]*inputX[i];
228 z[j][1] = inputX[i]*inputY[i];
229 z[j][2] = inputY[i]*inputY[i];
236 // Initialisation of S
239 for(unsigned int i=begin; i<begin+n; i++) {
240 for(unsigned int x=0; x<6; x++)
241 for(unsigned int y=0; y<6; y++)
242 S(x,y) += z[j][x]*z[j][y];
245 Sinv = S.GetInverse();
246 St = S.GetVnlMatrix().transpose();
248 // Initialisation of W
250 CopyBlock(W, S, 0, 0, 3);
251 CopyBlock(W, S, 3, 3, 3, -1);
252 Wt = W.GetVnlMatrix().transpose();
254 // Automated computation of mEta
256 Matrix3x3d E = GetBlock3x3(S, 0, 0);
257 Matrix3x3d B = GetBlock3x3(S, 0, 3);
258 Matrix3x3d Bt = GetBlock3x3(S, 3, 0);
259 Matrix3x3d D = GetBlock3x3(S, 3, 3);
260 Matrix3x3d Dinv(D.GetInverse());
262 Stilde = E - B*Dinv*Bt;
265 Ctilde(0,0) = 0; Ctilde(0,1) = 0; Ctilde(0,2) = 2;
266 Ctilde(1,0) = 0; Ctilde(1,1) = -1; Ctilde(1,2) = 0;
267 Ctilde(2,0) = 2; Ctilde(2,1) = 0; Ctilde(2,2) = 0;
269 // Ctilde is not inonneg-definite, so vnl print error on std cerr
270 // the following "disableStdCerr" disable it ...
272 vnl_generalized_eigensystem solver(Stilde.GetVnlMatrix(), Ctilde.GetVnlMatrix());
273 //vnl_generalized_eigensystem solver(Ctilde.GetVnlMatrix(), Stilde.GetVnlMatrix());
275 double dmax=0.0, dmin=9999999.9;
278 mEta = 2.0/(fabs(dmax/dmin)+1);
282 //---------------------------------------------------------------------