1 /*=========================================================================
4 Module: $RCSfile: clitkEllipse.cxx,v $
6 Date: $Date: 2010/01/06 13:31:56 $
7 Version: $Revision: 1.1 $
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<7; 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];
63 double mu = 1.0/(a*k1*k1 + b*k1*k2 + c*k2*k2 - f);
65 double m12 = mu * 0.5 * b;
67 double l1 = ( (m11+m22) + sqrt((m11-m22)*(m11-m22)+4*m12*m12) )/2.0;
69 axis[1] = 1.0/sqrt(l1);
70 double l2 = ((m11+m22)-sqrt((m11-m22)*(m11-m22)+4*m12*m12))/2.0;
72 axis[0] = 1.0/sqrt(l2);
75 //---------------------------------------------------------------------
78 //---------------------------------------------------------------------
79 double clitk::Ellipse::ComputeAngleInRad() {
80 // See http://www.geometrictools.com/Documentation/InformationAboutEllipses.pdf
83 if (a<c) { theta = 0; }
84 else { theta = 0.5*M_PI; }
88 theta = 0.5*atan(-b/(c-a));
91 theta = M_PI/2+0.5*atan(-b/(c-a));
96 //---------------------------------------------------------------------
99 //---------------------------------------------------------------------
100 void clitk::Ellipse::InitialiseEllipseFitting(double eta, unsigned int n, clitk::Signal & inputX, clitk::Signal & inputY) {
106 // Initialise ellipse to global fit
107 std::vector<double> extremaX(2);
108 std::vector<double> extremaY(2);
111 extremaX[0] = extremaY[0] = numeric_limits<double>::max();
112 extremaX[1] = extremaY[1] = -numeric_limits<double>::max();
113 for(unsigned int i=0; i<n; i++) {
114 if (inputX[i] < extremaX[0]) extremaX[0] = inputX[i];
115 if (inputX[i] > extremaX[1]) extremaX[1] = inputX[i];
116 if (inputY[i] < extremaY[0]) extremaY[0] = inputY[i];
117 if (inputY[i] > extremaY[1]) extremaY[1] = inputY[i];
124 // initialisation with an ellipse more small than real points extends
125 double ax1 = (extremaX[1]-extremaX[0])/2.0;
126 double ax2 = (extremaY[1]-extremaY[0])/2.0;
127 if (ax2 >= ax1) ax2 = 0.99*ax1;
128 SetCenterAndSemiAxes(x0, y0, ax1, ax2);
130 // Initialisation of C
132 C(0,0) = 0; C(0,1) = 0; C(0,2) = 2;
133 C(1,0) = 0; C(1,1) = -1; C(1,2) = 0;
134 C(2,0) = 2; C(2,1) = 0; C(2,2) = 0;
135 Ct = C.GetVnlMatrix().transpose();
138 UpdateSMatrix(0,n, inputX, inputY);
140 //---------------------------------------------------------------------
143 //---------------------------------------------------------------------
144 void clitk::Ellipse::CopyBlock(Matrix6x6d & out, const Matrix6x6d & in,
145 int ox, int oy, int l, double factor) {
146 for(int x=ox; x<ox+l; x++)
147 for(int y=oy; y<oy+l; y++) {
148 out(x,y) = factor * in(x,y);
151 //---------------------------------------------------------------------
154 //---------------------------------------------------------------------
155 void clitk::Ellipse::CopyBlock(Matrix6x6d & out, const Matrix3x3d & in,
156 int ox, int oy, double factor) {
157 for(int x=0; x<3; x++)
158 for(int y=0; y<3; y++) {
159 out(ox+x,oy+y) = factor * in(x,y);
162 //---------------------------------------------------------------------
165 //---------------------------------------------------------------------
166 Matrix3x3d clitk::Ellipse::GetBlock3x3(const Matrix6x6d & M, int x, int y) {
168 for(int i=0; i<3; i++) {
169 for(int j=0; j<3; j++) {
175 //---------------------------------------------------------------------
178 //---------------------------------------------------------------------
179 double clitk::Ellipse::EllipseFittingNextIteration() {
180 Vector6d & current = (*this);
182 // Normalize current vector. This allow to have a decreasing
183 // residual r (no need to optimize)
184 GetVnlVector().normalize();
185 double r = (St*current)*current;
187 // Temporary parameters
191 an = Sinv * C * current;
192 double num = (Wt*current)*current; // anT W an = (WT an) an
193 double denom = (Ct*current)*current;
194 an = (mEta * num/denom) * an;
195 an += (1.0-mEta)*current;
196 SetVnlVector(an.GetVnlVector());
201 //---------------------------------------------------------------------
204 //---------------------------------------------------------------------
205 void clitk::Ellipse::SetCenterAndSemiAxes(double x0, double y0,
206 double r1, double r2) {
211 std::cerr << "Error major axis should be r1, not r2 (r1=" << r1
212 << " r2=" << r2 << ")" << std::endl;
215 d = (-2.0*x0)/(r1*r1);
216 e = (-2.0*y0)/(r2*r2);
217 f = (x0*x0)/(r1*r1) + (y0*y0)/(r2*r2) - 1.0;
218 GetVnlVector().normalize();
220 //---------------------------------------------------------------------
223 //---------------------------------------------------------------------
224 void clitk::Ellipse::UpdateSMatrix(unsigned int begin, unsigned int n,
225 clitk::Signal & inputX, clitk::Signal & inputY) {
226 // Initialisation of z
229 for(unsigned int i=begin; i<begin+n; i++) {
230 z[j][0] = inputX[i]*inputX[i];
231 z[j][1] = inputX[i]*inputY[i];
232 z[j][2] = inputY[i]*inputY[i];
239 // Initialisation of S
242 for(unsigned int i=begin; i<begin+n; i++) {
243 for(unsigned int x=0; x<6; x++)
244 for(unsigned int y=0; y<6; y++)
245 S(x,y) += z[j][x]*z[j][y];
248 Sinv = S.GetInverse();
249 St = S.GetVnlMatrix().transpose();
251 // Initialisation of W
253 CopyBlock(W, S, 0, 0, 3);
254 CopyBlock(W, S, 3, 3, 3, -1);
255 Wt = W.GetVnlMatrix().transpose();
257 // Automated computation of mEta
259 Matrix3x3d E = GetBlock3x3(S, 0, 0);
260 Matrix3x3d B = GetBlock3x3(S, 0, 3);
261 Matrix3x3d Bt = GetBlock3x3(S, 3, 0);
262 Matrix3x3d D = GetBlock3x3(S, 3, 3);
263 Matrix3x3d Dinv(D.GetInverse());
265 Stilde = E - B*Dinv*Bt;
268 Ctilde(0,0) = 0; Ctilde(0,1) = 0; Ctilde(0,2) = 2;
269 Ctilde(1,0) = 0; Ctilde(1,1) = -1; Ctilde(1,2) = 0;
270 Ctilde(2,0) = 2; Ctilde(2,1) = 0; Ctilde(2,2) = 0;
272 // Ctilde is not inonneg-definite, so vnl print error on std cerr
273 // the following "disableStdCerr" disable it ...
275 vnl_generalized_eigensystem solver(Stilde.GetVnlMatrix(), Ctilde.GetVnlMatrix());
276 //vnl_generalized_eigensystem solver(Ctilde.GetVnlMatrix(), Stilde.GetVnlMatrix());
278 double dmax=0.0, dmin=9999999.9;
281 mEta = 2.0/(fabs(dmax/dmin)+1);
285 //---------------------------------------------------------------------