1 /*=========================================================================
4 Module: $RCSfile: clitkEllipse.cxx,v $
6 Date: $Date: 2010/03/03 10:47:48 $
7 Version: $Revision: 1.3 $
9 Copyright (c) CREATIS (Centre de Recherche et d'Applications en Traitement de
10 l'Image). All rights reserved. See Doc/License.txt or
11 http://www.creatis.insa-lyon.fr/Public/Gdcm/License.html for details.
13 This software is distributed WITHOUT ANY WARRANTY; without even
14 the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
15 PURPOSE. See the above copyright notices for more information.
17 =========================================================================*/
19 #include "clitkEllipse.h"
21 typedef itk::Vector<double,2> Vector2d;
22 typedef itk::Vector<double,6> Vector6d;
23 typedef itk::Matrix<double,6,6> Matrix6x6d;
24 typedef itk::Matrix<double,3,3> Matrix3x3d;
26 //---------------------------------------------------------------------
27 clitk::Ellipse::Ellipse():a((*this)[0]), b((*this)[1]),
28 c((*this)[2]), d((*this)[3]),
29 e((*this)[4]), f((*this)[5]) {
31 //---------------------------------------------------------------------
34 //---------------------------------------------------------------------
35 clitk::Ellipse::Ellipse(const Ellipse & e):a((*this)[0]), b((*this)[1]),
36 c((*this)[2]), d((*this)[3]),
37 e((*this)[4]), f((*this)[5]) {
38 for(int i=0; i<6; i++) (*this)[i] = e[i];
41 //---------------------------------------------------------------------
44 //---------------------------------------------------------------------
45 Vector2d clitk::Ellipse::ComputeCenter() {
46 // See http://mathworld.wolfram.com/Ellipse.html
49 center[0] = (2*c*d - b*e)/(b*b-4*a*c);
50 center[1] = (2*a*e - b*d)/(b*b-4*a*c);
53 //---------------------------------------------------------------------
56 //---------------------------------------------------------------------
57 Vector2d clitk::Ellipse::ComputeSemiAxeLengths() {
58 // See http://www.geometrictools.com/Documentation/InformationAboutEllipses.pdf
60 Vector2d center = ComputeCenter();
61 double & k1 = center[0];
62 double & k2 = center[1];
64 double mu = 1.0/(a*k1*k1 + b*k1*k2 + c*k2*k2 - f);
68 // DD(a*k1*k1 + b*k1*k2 + c*k2*k2 - f);
71 double m12 = mu * 0.5 * b;
73 double l1 = ( (m11+m22) + sqrt((m11-m22)*(m11-m22)+4*m12*m12) )/2.0;
82 axis[1] = 1.0/sqrt(l1);
83 double l2 = ((m11+m22)-sqrt((m11-m22)*(m11-m22)+4*m12*m12))/2.0;
86 axis[0] = 1.0/sqrt(l2);
89 //---------------------------------------------------------------------
92 //---------------------------------------------------------------------
93 double clitk::Ellipse::ComputeAngleInRad() {
94 // See http://www.geometrictools.com/Documentation/InformationAboutEllipses.pdf
97 if (a<c) { theta = 0; }
98 else { theta = 0.5*M_PI; }
102 theta = 0.5*atan(-b/(c-a));
105 theta = M_PI/2+0.5*atan(-b/(c-a));
110 //---------------------------------------------------------------------
113 //---------------------------------------------------------------------
114 void clitk::Ellipse::InitialiseEllipseFitting(double eta, unsigned int n, clitk::Signal & inputX, clitk::Signal & inputY) {
120 // Initialise ellipse to global fit
121 std::vector<double> extremaX(2);
122 std::vector<double> extremaY(2);
125 extremaX[0] = extremaY[0] = numeric_limits<double>::max();
126 extremaX[1] = extremaY[1] = -numeric_limits<double>::max();
127 for(unsigned int i=0; i<n; i++) {
128 if (inputX[i] < extremaX[0]) extremaX[0] = inputX[i];
129 if (inputX[i] > extremaX[1]) extremaX[1] = inputX[i];
130 if (inputY[i] < extremaY[0]) extremaY[0] = inputY[i];
131 if (inputY[i] > extremaY[1]) extremaY[1] = inputY[i];
138 // initialisation with an ellipse more small than real points extends
139 double ax1 = (extremaX[1]-extremaX[0])/2.0;
140 double ax2 = (extremaY[1]-extremaY[0])/2.0;
141 if (ax2 >= ax1) ax2 = 0.99*ax1;
142 SetCenterAndSemiAxes(x0, y0, ax1, ax2);
144 // Initialisation of C
146 C(0,0) = 0; C(0,1) = 0; C(0,2) = 2;
147 C(1,0) = 0; C(1,1) = -1; C(1,2) = 0;
148 C(2,0) = 2; C(2,1) = 0; C(2,2) = 0;
149 Ct = C.GetVnlMatrix().transpose();
152 UpdateSMatrix(0,n, inputX, inputY);
154 //---------------------------------------------------------------------
157 //---------------------------------------------------------------------
158 void clitk::Ellipse::CopyBlock(Matrix6x6d & out, const Matrix6x6d & in,
159 int ox, int oy, int l, double factor) {
160 for(int x=ox; x<ox+l; x++)
161 for(int y=oy; y<oy+l; y++) {
162 out(x,y) = factor * in(x,y);
165 //---------------------------------------------------------------------
168 //---------------------------------------------------------------------
169 void clitk::Ellipse::CopyBlock(Matrix6x6d & out, const Matrix3x3d & in,
170 int ox, int oy, double factor) {
171 for(int x=0; x<3; x++)
172 for(int y=0; y<3; y++) {
173 out(ox+x,oy+y) = factor * in(x,y);
176 //---------------------------------------------------------------------
179 //---------------------------------------------------------------------
180 Matrix3x3d clitk::Ellipse::GetBlock3x3(const Matrix6x6d & M, int x, int y) {
182 for(int i=0; i<3; i++) {
183 for(int j=0; j<3; j++) {
189 //---------------------------------------------------------------------
192 //---------------------------------------------------------------------
193 double clitk::Ellipse::EllipseFittingNextIteration() {
194 Vector6d & current = (*this);
196 // Normalize current vector. This allow to have a decreasing
197 // residual r (no need to optimize)
198 GetVnlVector().normalize();
199 double r = (St*current)*current;
202 // Temporary parameters
206 an = Sinv * C * current;
207 double num = (Wt*current)*current; // anT W an = (WT an) an
208 double denom = (Ct*current)*current;
209 an = (mEta * num/denom) * an;
210 an += (1.0-mEta)*current;
211 SetVnlVector(an.GetVnlVector());
216 //---------------------------------------------------------------------
219 //---------------------------------------------------------------------
220 void clitk::Ellipse::SetCenterAndSemiAxes(double x0, double y0,
221 double r1, double r2) {
226 std::cerr << "Error major axis should be r1, not r2 (r1=" << r1
227 << " r2=" << r2 << ")" << std::endl;
230 d = (-2.0*x0)/(r1*r1);
231 e = (-2.0*y0)/(r2*r2);
232 f = (x0*x0)/(r1*r1) + (y0*y0)/(r2*r2) - 1.0;
233 GetVnlVector().normalize();
235 //---------------------------------------------------------------------
238 //---------------------------------------------------------------------
239 void clitk::Ellipse::UpdateSMatrix(unsigned int begin, unsigned int n,
240 clitk::Signal & inputX, clitk::Signal & inputY) {
241 // Initialisation of z
244 for(unsigned int i=begin; i<begin+n; i++) {
245 z[j][0] = inputX[i]*inputX[i];
246 z[j][1] = inputX[i]*inputY[i];
247 z[j][2] = inputY[i]*inputY[i];
254 // Initialisation of S
257 for(unsigned int i=0; i<n; i++) {
258 for(unsigned int x=0; x<6; x++)
259 for(unsigned int y=0; y<6; y++)
260 S(x,y) += z[i][x]*z[i][y];
263 Sinv = S.GetInverse();
264 St = S.GetVnlMatrix().transpose();
266 // Initialisation of W
268 CopyBlock(W, S, 0, 0, 3);
269 CopyBlock(W, S, 3, 3, 3, -1);
270 Wt = W.GetVnlMatrix().transpose();
272 // Automated computation of mEta
274 Matrix3x3d E = GetBlock3x3(S, 0, 0);
275 Matrix3x3d B = GetBlock3x3(S, 0, 3);
276 Matrix3x3d Bt = GetBlock3x3(S, 3, 0);
277 Matrix3x3d D = GetBlock3x3(S, 3, 3);
278 Matrix3x3d Dinv(D.GetInverse());
280 Stilde = E - B*Dinv*Bt;
283 Ctilde(0,0) = 0; Ctilde(0,1) = 0; Ctilde(0,2) = 2;
284 Ctilde(1,0) = 0; Ctilde(1,1) = -1; Ctilde(1,2) = 0;
285 Ctilde(2,0) = 2; Ctilde(2,1) = 0; Ctilde(2,2) = 0;
287 // Ctilde is not inonneg-definite, so vnl print error on std cerr
288 // the following "disableStdCerr" disable it ...
290 vnl_generalized_eigensystem solver(Stilde.GetVnlMatrix(), Ctilde.GetVnlMatrix());
291 //vnl_generalized_eigensystem solver(Ctilde.GetVnlMatrix(), Stilde.GetVnlMatrix());
293 double dmax=0.0, dmin=9999999.9;
296 mEta = 2.0/(fabs(dmax/dmin)+1);
300 //---------------------------------------------------------------------
303 //---------------------------------------------------------------------
304 void clitk::ComputeIsoPhaseFromListOfEllipses(std::vector<clitk::Ellipse*> & l,
310 clitk::Signal & phase) {
313 std::vector<double> mIsoPhaseRefAngle(nbIsoPhase);
314 phase.resize(l.size());
315 double refphaseangle=0;
316 double previousangle=0;
320 for (unsigned int i=0; i<l.size(); i++) {
321 // DD("=================================");
323 clitk::Ellipse & An = *l[i];
325 // DD(An.ComputeCenter());
326 // DD(An.ComputeSemiAxeLengths());
327 // DD(rad2deg(An.ComputeAngleInRad()));
329 // Compute current signed angle
330 Vector2d x1(An.ComputeCenter());
331 double a = l[0]->ComputeSemiAxeLengths()[0];
332 double theta = l[0]->ComputeAngleInRad();
333 Vector2d x2; x2[0] = x1[0]+a * cos(theta); x2[1] = x1[1]+a * sin(theta);
335 Vector2d x4; x4[0] = sx[i+L]; x4[1] = sy[i+L];
340 // http://www.euclideanspace.com/maths/algebra/vectors/angleBetween/index.htm
341 double signed_angle = atan2(B[1], B[0]) - atan2(A[1], A[0]);
342 if (signed_angle<0) signed_angle = 2*M_PI+signed_angle;
344 // First time : set the angle
347 refphaseangle = signed_angle;
348 for(int a=0; a<nbIsoPhase; a++) {
349 if (a==0) mIsoPhaseRefAngle[a] = refphaseangle;//signed_angle;
350 else mIsoPhaseRefAngle[a] = (refphaseangle //signed_angle
351 + (2*M_PI)*a/nbIsoPhase);
352 if (mIsoPhaseRefAngle[a] > 2*M_PI) mIsoPhaseRefAngle[a] -= 2*M_PI;
353 if (mIsoPhaseRefAngle[a] < 0) mIsoPhaseRefAngle[a] = 2*M_PI-mIsoPhaseRefAngle[a];
356 while ((a<nbIsoPhase) && (signed_angle>=mIsoPhaseRefAngle[a])) { a++; }
358 if (nbIsoPhase == 1) phase[i] = 1;
361 phase[i] = phase[i-1];
363 // Check if angle cross a ref angle
364 for(int a=0; a<nbIsoPhase; a++) {
365 // std::cout << "a=" << rad2deg(signed_angle) << " prev=" << rad2deg(previousangle)
366 // << " ref=" << rad2deg(mIsoPhaseRefAngle[a]) << " " << phase[i] << std::endl;
367 if (signed_angle > previousangle) {
369 // (((signed_angle > mIsoPhaseRefAngle[a]) && (previousangle < mIsoPhaseRefAngle[a]))) ||
370 // ((mIsoPhaseRefAngle[a]==0) && (signed_angle < previousangle)))
371 if ((previousangle < mIsoPhaseRefAngle[a]) &&
372 (signed_angle >= mIsoPhaseRefAngle[a]))
375 if (nbIsoPhase == 1) { // single phase, alternate 0 and 1
376 phase[i] = -phase[i-1];
381 else { // previousangle >= signed_angle (we turn around 0)
383 if ((mIsoPhaseRefAngle[a] > previousangle) ||
384 (mIsoPhaseRefAngle[a] < signed_angle)) {
386 if (nbIsoPhase == 1) { // single phase, alternate 0 and 1
387 phase[i] = -phase[i-1];
394 previousangle = signed_angle;
397 //---------------------------------------------------------------------