tools/clitkEllipse.cxx

   1 /*=========================================================================
   2
   3   Program:   clitk
   4   Module:    $RCSfile: clitkEllipse.cxx,v $
   5   Language:  C++
   6   Date:      $Date: 2010/03/03 10:47:48 $
   7   Version:   $Revision: 1.3 $
   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<6; 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   // DD(f);
  64   double mu = 1.0/(a*k1*k1 + b*k1*k2 + c*k2*k2 - f);
  65   // DD(a*k1*k1);
  66   //   DD(b*k1*k2);
  67   //   DD(c*k2*k2);
  68   //   DD(a*k1*k1 + b*k1*k2 + c*k2*k2 - f);
  69   //   DD(mu);
  70   double m11 = mu * a;
  71   double m12 = mu * 0.5 * b;
  72   double m22 = mu * c;
  73   double l1 = ( (m11+m22) + sqrt((m11-m22)*(m11-m22)+4*m12*m12) )/2.0;
  74   // DD(k1);
  75   //   DD(k2);
  76   //   DD(mu);
  77   //   DD(m11);
  78   //   DD(m12);
  79   //   DD(m22);
  80   //   DD(l1);
  81   assert(l1>=0.0);
  82   axis[1] = 1.0/sqrt(l1);
  83   double l2 = ((m11+m22)-sqrt((m11-m22)*(m11-m22)+4*m12*m12))/2.0;
  84   // DD(l2);
  85   assert(l2>0.0);
  86   axis[0] = 1.0/sqrt(l2);
  87   return axis;
  88 }
  89 //---------------------------------------------------------------------
  90
  91
  92 //---------------------------------------------------------------------
  93 double clitk::Ellipse::ComputeAngleInRad() {
  94   // See http://www.geometrictools.com/Documentation/InformationAboutEllipses.pdf
  95   double theta;
  96   if (b==0) {
  97     if (a<c) { theta = 0; }
  98     else     { theta = 0.5*M_PI; }
  99   }
 100   else {
 101     if (a<c) {
 102       theta = 0.5*atan(-b/(c-a));
 103     }
 104     else {
 105       theta = M_PI/2+0.5*atan(-b/(c-a));
 106     }
 107   }
 108   return theta;
 109 }
 110 //---------------------------------------------------------------------
 111
 112
 113 //---------------------------------------------------------------------
 114 void clitk::Ellipse::InitialiseEllipseFitting(double eta, unsigned int n, clitk::Signal & inputX, clitk::Signal & inputY) {
 115   // Store data
 116   mEta = eta;
 117   mInputX = &inputX;
 118   mInputY = &inputY;
 119
 120   // Initialise ellipse to global fit
 121   std::vector<double> extremaX(2);
 122   std::vector<double> extremaY(2);
 123   double x0 = 0.0;
 124   double y0 = 0.0;
 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];
 132     x0 += inputX[i];
 133     y0 += inputY[i];
 134   }
 135   x0 /= n;
 136   y0 /= n;
 137
 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);
 143
 144   // Initialisation of C
 145   C.Fill(0.0);
 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();
 150
 151   // Compute initial S
 152   UpdateSMatrix(0,n, inputX, inputY);
 153 }
 154 //---------------------------------------------------------------------
 155
 156
 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);
 163     }
 164 }
 165 //---------------------------------------------------------------------
 166
 167
 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);
 174     }
 175 }
 176 //---------------------------------------------------------------------
 177
 178
 179 //---------------------------------------------------------------------
 180 Matrix3x3d clitk::Ellipse::GetBlock3x3(const Matrix6x6d & M, int x, int y) {
 181   Matrix3x3d B;
 182   for(int i=0; i<3; i++) {
 183     for(int j=0; j<3; j++) {
 184       B(i,j) = M(x+i,y+j);
 185     }
 186   }
 187   return B;
 188 }
 189 //---------------------------------------------------------------------
 190
 191
 192 //---------------------------------------------------------------------
 193 double clitk::Ellipse::EllipseFittingNextIteration() {
 194   Vector6d & current = (*this);
 195
 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;
 200   // DD(r);
 201
 202   // Temporary parameters
 203   Vector6d an;
 204
 205   // Iterative update
 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());
 212
 213   // return residual
 214   return r;
 215 }
 216 //---------------------------------------------------------------------
 217
 218
 219 //---------------------------------------------------------------------
 220 void clitk::Ellipse::SetCenterAndSemiAxes(double x0, double y0,
 221                                           double r1, double r2) {
 222   a = 1.0/(r1*r1);
 223   b = 0;
 224   c = 1.0/(r2*r2);
 225   if (a>c) {
 226     std::cerr << "Error major axis should be r1, not r2 (r1=" << r1
 227               << " r2=" << r2 << ")" << std::endl;
 228     exit(0);
 229   }
 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();
 234 }
 235 //---------------------------------------------------------------------
 236
 237
 238 //---------------------------------------------------------------------
 239 void clitk::Ellipse::UpdateSMatrix(unsigned int begin, unsigned int n,
 240                                    clitk::Signal & inputX, clitk::Signal & inputY) {
 241   // Initialisation of z
 242   z.resize(n);
 243   int j = 0;
 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];
 248     z[j][3] = inputX[i];
 249     z[j][4] = inputY[i];
 250     z[j][5] = 1;
 251     j++;
 252   }
 253
 254   // Initialisation of S
 255   S.Fill(0.0);
 256   // j = 0;
 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];
 261     // j++;
 262   }
 263   Sinv = S.GetInverse();
 264   St = S.GetVnlMatrix().transpose();
 265
 266   // Initialisation of W
 267   W.Fill(0.0);
 268   CopyBlock(W, S, 0, 0, 3);
 269   CopyBlock(W, S, 3, 3, 3, -1);
 270   Wt = W.GetVnlMatrix().transpose();
 271
 272   // Automated computation of mEta
 273   if (mEta<0) {
 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());
 279     Matrix3x3d Stilde;
 280     Stilde = E - B*Dinv*Bt;
 281
 282     Matrix3x3d Ctilde;
 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;
 286
 287     // Ctilde is not inonneg-definite, so vnl print error on std cerr
 288     // the following "disableStdCerr" disable it ...
 289     disableStdCerr();
 290     vnl_generalized_eigensystem solver(Stilde.GetVnlMatrix(), Ctilde.GetVnlMatrix());
 291     //vnl_generalized_eigensystem solver(Ctilde.GetVnlMatrix(), Stilde.GetVnlMatrix());
 292     enableStdCerr();
 293     double dmax=0.0, dmin=9999999.9;
 294     dmax = solver.D(2);
 295     dmin = solver.D(1);
 296     mEta = 2.0/(fabs(dmax/dmin)+1);
 297     assert(mEta<1.0);
 298   }
 299 }
 300 //---------------------------------------------------------------------
 301
 302
 303 //---------------------------------------------------------------------
 304 void clitk::ComputeIsoPhaseFromListOfEllipses(std::vector<clitk::Ellipse*> & l,
 305                                               clitk::Signal & sx,
 306                                               clitk::Signal & sy,
 307                                               int nbIsoPhase,
 308                                               int delay,
 309                                               int L,
 310                                               clitk::Signal & phase) {
 311   // Init
 312   DD(nbIsoPhase);
 313   std::vector<double> mIsoPhaseRefAngle(nbIsoPhase);
 314   phase.resize(l.size());
 315   double refphaseangle=0;
 316   double previousangle=0;
 317
 318   // Loop on ellipses
 319   // DD(l.size());
 320   for (unsigned int i=0; i<l.size(); i++) {
 321     // DD("=================================");
 322     //     DD(i);
 323     clitk::Ellipse & An = *l[i];
 324     // DD(An);
 325     //     DD(An.ComputeCenter());
 326     //     DD(An.ComputeSemiAxeLengths());
 327     //     DD(rad2deg(An.ComputeAngleInRad()));
 328
 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);
 334     Vector2d x3(x1);
 335     Vector2d x4; x4[0] = sx[i+L]; x4[1] = sy[i+L];
 336     Vector2d A(x2-x1);
 337     Vector2d B(x4-x3);
 338     // DD(sx[i+L]);
 339     //     DD(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;
 343
 344     // First time : set the angle
 345     if (i==0) {
 346       // DD(signed_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];
 354       }
 355       int a=0;
 356       while ((a<nbIsoPhase) && (signed_angle>=mIsoPhaseRefAngle[a])) { a++; }
 357       phase[i] = a-1;
 358       if (nbIsoPhase == 1) phase[i] = 1;
 359     }
 360     else {
 361       phase[i] = phase[i-1];
 362
 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) {
 368           // DD("cas1");
 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]))
 373             {
 374               // DD(a);
 375               if (nbIsoPhase == 1) { // single phase, alternate 0 and 1
 376                 phase[i] = -phase[i-1];
 377               }
 378               else phase[i] = a;
 379             }
 380         }
 381         else { // previousangle >= signed_angle (we turn around 0)
 382           // DD("cas2");
 383           if ((mIsoPhaseRefAngle[a] > previousangle) ||
 384               (mIsoPhaseRefAngle[a] < signed_angle)) {
 385             // DD(a);
 386             if (nbIsoPhase == 1) { // single phase, alternate 0 and 1
 387               phase[i] = -phase[i-1];
 388             }
 389             else phase[i] = a;
 390           }
 391         }
 392       }
 393     }
 394     previousangle = signed_angle;
 395   }
 396 }
 397 //---------------------------------------------------------------------
 398