tools/clitkEllipse.cxx

   1 /*=========================================================================
   2
   3   Program:   clitk
   4   Module:    $RCSfile: clitkEllipse.cxx,v $
   5   Language:  C++
   6   Date:      $Date: 2010/01/06 13:31:56 $
   7   Version:   $Revision: 1.1 $
   8
   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.
  12
  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.
  16
  17 =========================================================================*/
  18
  19 #include "clitkEllipse.h"
  20
  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;
  25
  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]) {
  30 }
  31 //---------------------------------------------------------------------
  32
  33
  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];
  39 }
  40
  41 //---------------------------------------------------------------------
  42
  43
  44 //---------------------------------------------------------------------
  45 Vector2d clitk::Ellipse::ComputeCenter() {
  46   // See http://mathworld.wolfram.com/Ellipse.html
  47   // see Ruan 2008
  48   Vector2d center;
  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);
  51   return center;
  52 }
  53 //---------------------------------------------------------------------
  54
  55
  56 //---------------------------------------------------------------------
  57 Vector2d clitk::Ellipse::ComputeSemiAxeLengths() {
  58   // See http://www.geometrictools.com/Documentation/InformationAboutEllipses.pdf
  59   Vector2d axis;
  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);
  64   double m11 = mu * a;
  65   double m12 = mu * 0.5 * b;
  66   double m22 = mu * c;
  67   double l1 = ( (m11+m22) + sqrt((m11-m22)*(m11-m22)+4*m12*m12) )/2.0;
  68   assert(l1>0.0);
  69   axis[1] = 1.0/sqrt(l1);
  70   double l2 = ((m11+m22)-sqrt((m11-m22)*(m11-m22)+4*m12*m12))/2.0;
  71   assert(l2>0.0);
  72   axis[0] = 1.0/sqrt(l2);
  73   return axis;
  74 }
  75 //---------------------------------------------------------------------
  76
  77
  78 //---------------------------------------------------------------------
  79 double clitk::Ellipse::ComputeAngleInRad() {
  80   // See http://www.geometrictools.com/Documentation/InformationAboutEllipses.pdf
  81   double theta;
  82   if (b==0) {
  83     if (a<c) { theta = 0; }
  84     else     { theta = 0.5*M_PI; }
  85   }
  86   else {
  87     if (a<c) {
  88       theta = 0.5*atan(-b/(c-a));
  89     }
  90     else {
  91       theta = M_PI/2+0.5*atan(-b/(c-a));
  92     }
  93   }
  94   return theta;
  95 }
  96 //---------------------------------------------------------------------
  97
  98
  99 //---------------------------------------------------------------------
 100 void clitk::Ellipse::InitialiseEllipseFitting(double eta, unsigned int n, clitk::Signal & inputX, clitk::Signal & inputY) {
 101   // Store data
 102   mEta = eta;
 103   mInputX = &inputX;
 104   mInputY = &inputY;
 105
 106   // Initialise ellipse to global fit
 107   std::vector<double> extremaX(2);
 108   std::vector<double> extremaY(2);
 109   double x0 = 0.0;
 110   double y0 = 0.0;
 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];
 118     x0 += inputX[i];
 119     y0 += inputY[i];
 120   }
 121   x0 /= n;
 122   y0 /= n;
 123
 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);
 129
 130   // Initialisation of C
 131   C.Fill(0.0);
 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();
 136
 137   // Compute initial S
 138   UpdateSMatrix(0,n, inputX, inputY);
 139 }
 140 //---------------------------------------------------------------------
 141
 142
 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);
 149     }
 150 }
 151 //---------------------------------------------------------------------
 152
 153
 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);
 160     }
 161 }
 162 //---------------------------------------------------------------------
 163
 164
 165 //---------------------------------------------------------------------
 166 Matrix3x3d clitk::Ellipse::GetBlock3x3(const Matrix6x6d & M, int x, int y) {
 167   Matrix3x3d B;
 168   for(int i=0; i<3; i++) {
 169     for(int j=0; j<3; j++) {
 170       B(i,j) = M(x+i,y+j);
 171     }
 172   }
 173   return B;
 174 }
 175 //---------------------------------------------------------------------
 176
 177
 178 //---------------------------------------------------------------------
 179 double clitk::Ellipse::EllipseFittingNextIteration() {
 180   Vector6d & current = (*this);
 181
 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;
 186
 187   // Temporary parameters
 188   Vector6d an;
 189
 190   // Iterative update
 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());
 197
 198   // return residual
 199   return r;
 200 }
 201 //---------------------------------------------------------------------
 202
 203
 204 //---------------------------------------------------------------------
 205 void clitk::Ellipse::SetCenterAndSemiAxes(double x0, double y0,
 206                                           double r1, double r2) {
 207   a = 1.0/(r1*r1);
 208   b = 0;
 209   c = 1.0/(r2*r2);
 210   if (a>c) {
 211     std::cerr << "Error major axis should be r1, not r2 (r1=" << r1
 212               << " r2=" << r2 << ")" << std::endl;
 213     exit(0);
 214   }
 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();
 219 }
 220 //---------------------------------------------------------------------
 221
 222
 223 //---------------------------------------------------------------------
 224 void clitk::Ellipse::UpdateSMatrix(unsigned int begin, unsigned int n,
 225                                    clitk::Signal & inputX, clitk::Signal & inputY) {
 226   // Initialisation of z
 227   z.resize(n);
 228   int j = 0;
 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];
 233     z[j][3] = inputX[i];
 234     z[j][4] = inputY[i];
 235     z[j][5] = 1;
 236     j++;
 237   }
 238
 239   // Initialisation of S
 240   S.Fill(0.0);
 241   j = 0;
 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];
 246     j++;
 247   }
 248   Sinv = S.GetInverse();
 249   St = S.GetVnlMatrix().transpose();
 250
 251   // Initialisation of W
 252   W.Fill(0.0);
 253   CopyBlock(W, S, 0, 0, 3);
 254   CopyBlock(W, S, 3, 3, 3, -1);
 255   Wt = W.GetVnlMatrix().transpose();
 256
 257   // Automated computation of mEta
 258   if (mEta<0) {
 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());
 264     Matrix3x3d Stilde;
 265     Stilde = E - B*Dinv*Bt;
 266
 267     Matrix3x3d Ctilde;
 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;
 271
 272     // Ctilde is not inonneg-definite, so vnl print error on std cerr
 273     // the following "disableStdCerr" disable it ...
 274     disableStdCerr();
 275     vnl_generalized_eigensystem solver(Stilde.GetVnlMatrix(), Ctilde.GetVnlMatrix());
 276     //vnl_generalized_eigensystem solver(Ctilde.GetVnlMatrix(), Stilde.GetVnlMatrix());
 277     enableStdCerr();
 278     double dmax=0.0, dmin=9999999.9;
 279     dmax = solver.D(2);
 280     dmin = solver.D(1);
 281     mEta = 2.0/(fabs(dmax/dmin)+1);
 282     assert(mEta<1.0);
 283   }
 284 }
 285 //---------------------------------------------------------------------