tools/clitkEllipse.cxx

   1 /*=========================================================================
   2
   3   Program:   clitk
   4   Language:  C++
   5
   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.
   9
  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.
  13
  14 =========================================================================*/
  15
  16 #include "clitkEllipse.h"
  17
  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;
  22
  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]) {
  27 }
  28 //---------------------------------------------------------------------
  29
  30
  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];
  36 }
  37
  38 //---------------------------------------------------------------------
  39
  40
  41 //---------------------------------------------------------------------
  42 Vector2d clitk::Ellipse::ComputeCenter() {
  43   // See http://mathworld.wolfram.com/Ellipse.html
  44   // see Ruan 2008
  45   Vector2d center;
  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);
  48   return center;
  49 }
  50 //---------------------------------------------------------------------
  51
  52
  53 //---------------------------------------------------------------------
  54 Vector2d clitk::Ellipse::ComputeSemiAxeLengths() {
  55   // See http://www.geometrictools.com/Documentation/InformationAboutEllipses.pdf
  56   Vector2d axis;
  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);
  61   double m11 = mu * a;
  62   double m12 = mu * 0.5 * b;
  63   double m22 = mu * c;
  64   double l1 = ( (m11+m22) + sqrt((m11-m22)*(m11-m22)+4*m12*m12) )/2.0;
  65   assert(l1>0.0);
  66   axis[1] = 1.0/sqrt(l1);
  67   double l2 = ((m11+m22)-sqrt((m11-m22)*(m11-m22)+4*m12*m12))/2.0;
  68   assert(l2>0.0);
  69   axis[0] = 1.0/sqrt(l2);
  70   return axis;
  71 }
  72 //---------------------------------------------------------------------
  73
  74
  75 //---------------------------------------------------------------------
  76 double clitk::Ellipse::ComputeAngleInRad() {
  77   // See http://www.geometrictools.com/Documentation/InformationAboutEllipses.pdf
  78   double theta;
  79   if (b==0) {
  80     if (a<c) { theta = 0; }
  81     else     { theta = 0.5*M_PI; }
  82   }
  83   else {
  84     if (a<c) {
  85       theta = 0.5*atan(-b/(c-a));
  86     }
  87     else {
  88       theta = M_PI/2+0.5*atan(-b/(c-a));
  89     }
  90   }
  91   return theta;
  92 }
  93 //---------------------------------------------------------------------
  94
  95
  96 //---------------------------------------------------------------------
  97 void clitk::Ellipse::InitialiseEllipseFitting(double eta, unsigned int n, clitk::Signal & inputX, clitk::Signal & inputY) {
  98   // Store data
  99   mEta = eta;
 100   mInputX = &inputX;
 101   mInputY = &inputY;
 102
 103   // Initialise ellipse to global fit
 104   std::vector<double> extremaX(2);
 105   std::vector<double> extremaY(2);
 106   double x0 = 0.0;
 107   double y0 = 0.0;
 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];
 115     x0 += inputX[i];
 116     y0 += inputY[i];
 117   }
 118   x0 /= n;
 119   y0 /= n;
 120
 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);
 126
 127   // Initialisation of C
 128   C.Fill(0.0);
 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();
 133
 134   // Compute initial S
 135   UpdateSMatrix(0,n, inputX, inputY);
 136 }
 137 //---------------------------------------------------------------------
 138
 139
 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);
 146     }
 147 }
 148 //---------------------------------------------------------------------
 149
 150
 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);
 157     }
 158 }
 159 //---------------------------------------------------------------------
 160
 161
 162 //---------------------------------------------------------------------
 163 Matrix3x3d clitk::Ellipse::GetBlock3x3(const Matrix6x6d & M, int x, int y) {
 164   Matrix3x3d B;
 165   for(int i=0; i<3; i++) {
 166     for(int j=0; j<3; j++) {
 167       B(i,j) = M(x+i,y+j);
 168     }
 169   }
 170   return B;
 171 }
 172 //---------------------------------------------------------------------
 173
 174
 175 //---------------------------------------------------------------------
 176 double clitk::Ellipse::EllipseFittingNextIteration() {
 177   Vector6d & current = (*this);
 178
 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;
 183
 184   // Temporary parameters
 185   Vector6d an;
 186
 187   // Iterative update
 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());
 194
 195   // return residual
 196   return r;
 197 }
 198 //---------------------------------------------------------------------
 199
 200
 201 //---------------------------------------------------------------------
 202 void clitk::Ellipse::SetCenterAndSemiAxes(double x0, double y0,
 203                                           double r1, double r2) {
 204   a = 1.0/(r1*r1);
 205   b = 0;
 206   c = 1.0/(r2*r2);
 207   if (a>c) {
 208     std::cerr << "Error major axis should be r1, not r2 (r1=" << r1
 209               << " r2=" << r2 << ")" << std::endl;
 210     exit(0);
 211   }
 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();
 216 }
 217 //---------------------------------------------------------------------
 218
 219
 220 //---------------------------------------------------------------------
 221 void clitk::Ellipse::UpdateSMatrix(unsigned int begin, unsigned int n,
 222                                    clitk::Signal & inputX, clitk::Signal & inputY) {
 223   // Initialisation of z
 224   z.resize(n);
 225   int j = 0;
 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];
 230     z[j][3] = inputX[i];
 231     z[j][4] = inputY[i];
 232     z[j][5] = 1;
 233     j++;
 234   }
 235
 236   // Initialisation of S
 237   S.Fill(0.0);
 238   j = 0;
 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];
 243     j++;
 244   }
 245   Sinv = S.GetInverse();
 246   St = S.GetVnlMatrix().transpose();
 247
 248   // Initialisation of W
 249   W.Fill(0.0);
 250   CopyBlock(W, S, 0, 0, 3);
 251   CopyBlock(W, S, 3, 3, 3, -1);
 252   Wt = W.GetVnlMatrix().transpose();
 253
 254   // Automated computation of mEta
 255   if (mEta<0) {
 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());
 261     Matrix3x3d Stilde;
 262     Stilde = E - B*Dinv*Bt;
 263
 264     Matrix3x3d Ctilde;
 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;
 268
 269     // Ctilde is not inonneg-definite, so vnl print error on std cerr
 270     // the following "disableStdCerr" disable it ...
 271     disableStdCerr();
 272     vnl_generalized_eigensystem solver(Stilde.GetVnlMatrix(), Ctilde.GetVnlMatrix());
 273     //vnl_generalized_eigensystem solver(Ctilde.GetVnlMatrix(), Stilde.GetVnlMatrix());
 274     enableStdCerr();
 275     double dmax=0.0, dmin=9999999.9;
 276     dmax = solver.D(2);
 277     dmin = solver.D(1);
 278     mEta = 2.0/(fabs(dmax/dmin)+1);
 279     assert(mEta<1.0);
 280   }
 281 }
 282 //---------------------------------------------------------------------