common/clitkTransformUtilities.h

   1 #ifndef CLITKTRANSFORMUTILITIES_H
   2 #define CLITKTRANSFORMUTILITIES_H
   3 #include "itkMatrix.h"
   4 #include "itkArray.h"
   5 #include "itkPoint.h"
   6 #include "clitkImageCommon.h"
   7 #include "clitkCommon.h"
   8
   9
  10 namespace clitk
  11 {
  12   //============================================================================
  13   //Declarations
  14   //============================================================================
  15   itk::Matrix<double, 3, 3> GetForwardAffineMatrix2D(itk::Array<double> transformParameters);
  16   itk::Matrix<double, 3, 3> GetBackwardAffineMatrix2D(itk::Array<double> transformParameters);
  17   itk::Matrix<double, 4, 4> GetForwardAffineMatrix3D(itk::Array<double> transformParameters);
  18   itk::Matrix<double, 4, 4> GetBackwardAffineMatrix3D(itk::Array<double> transformParameters);
  19   itk::Matrix<double, 3, 3> GetRotationMatrix3D(itk::Array<double> rotationParameters);
  20   itk::Point<double, 3> GetRotatedPoint3D(itk::Array<double> rotationParameters, itk::Point<double, 3> input);
  21   itk::Matrix<double, 4, 4> GetCenteredRotationMatrix3D(itk::Array<double> rotationParameters,itk::Point<double,3> centerOfRotation);
  22   //   itk::Matrix<double, 4, 4> GetComposedMatrix3D(itk::Matrix<double, 4, 4> firstTransform, itk::Matrix<double, 4, 4> secondTransform);
  23
  24   itk::Matrix<double, 5, 5> ReadMatrix4D(std::string fileName);
  25   itk::Matrix<double, 4, 4> ReadMatrix3D(std::string fileName);
  26   itk::Matrix<double, 3, 3> ReadMatrix2D(std::string fileName);
  27   template <unsigned int Dimension > itk::Matrix<double, Dimension+1 , Dimension+1> ReadMatrix(std::string fileName);
  28
  29   itk::Matrix<double, 3, 3> GetRotationalPartMatrix3D(itk::Matrix<double, 4, 4> input);
  30   itk::Matrix<double, 3, 3> GetRotationalPartMatrix(itk::Matrix<double, 4, 4> input);
  31   itk::Matrix<double, 2, 2> GetRotationalPartMatrix2D(itk::Matrix<double, 3, 3> input);
  32   itk::Matrix<double, 2, 2> GetRotationalPartMatrix(itk::Matrix<double, 3, 3> input);
  33
  34   itk::Vector<double,3> GetTranslationPartMatrix3D(itk::Matrix<double, 4, 4> input);
  35   itk::Vector<double,3> GetTranslationPartMatrix(itk::Matrix<double, 4, 4> input);
  36   itk::Vector<double,2> GetTranslationPartMatrix2D(itk::Matrix<double, 3, 3> input);
  37   itk::Vector<double,2> GetTranslationPartMatrix(itk::Matrix<double, 3, 3> input);
  38
  39
  40   //============================================================================
  41   //Inline functions definition in header file, otherwise linker errors
  42   //============================================================================
  43
  44   //========================================================================================
  45   inline  itk::Matrix<double, 3, 3> GetForwardAffineMatrix2D(itk::Array<double> transformParameters)
  46   {
  47     itk::Matrix<double, 3, 3> matrix;
  48     //rotation part
  49     matrix[0][0]=cos(transformParameters[0]);
  50     matrix[0][1]=-sin(transformParameters[0]);
  51     matrix[1][0]=sin(transformParameters[0]);
  52     matrix[1][1]=cos(transformParameters[0]);
  53     //translation part
  54     matrix[0][2]=transformParameters[1];
  55     matrix[1][2]=transformParameters[2];
  56     //homogenize
  57     matrix[2][0]=0.;
  58     matrix[2][1]=0.;
  59     matrix[2][2]=1.;
  60     return matrix;
  61   }
  62
  63   inline  itk::Matrix<double, 3, 3> GetBackwardAffineMatrix2D(itk::Array<double> transformParameters)
  64   {
  65     itk::Matrix<double, 3, 3> matrix;
  66     //rotation part
  67     matrix[0][0]=cos(transformParameters[0]);
  68     matrix[0][1]=sin(transformParameters[0]);
  69     matrix[1][0]=-sin(transformParameters[0]);
  70     matrix[1][1]=cos(transformParameters[0]);
  71     //translation part
  72     matrix[0][2]=transformParameters[1];
  73     matrix[1][2]=transformParameters[2];
  74     //homogenize
  75     matrix[2][0]=0.;
  76     matrix[2][1]=0.;
  77     matrix[2][2]=1.;
  78     return matrix;
  79   }
  80
  81
  82   inline  itk::Matrix<double, 4, 4> GetForwardAffineMatrix3D(itk::Array<double> transformParameters)
  83   {
  84     itk::Matrix<double, 4, 4> matrix;
  85     //rotational part
  86     matrix[0][0]= cos(transformParameters[1])*cos(transformParameters[2]);
  87     matrix[0][1]= sin(transformParameters[0])*sin(transformParameters[1])*cos(transformParameters[2])+ sin(transformParameters[2])*cos(transformParameters[0]);
  88     matrix[0][2]= -cos(transformParameters[0])*sin(transformParameters[1])*cos(transformParameters[2])+sin(transformParameters[0])*sin(transformParameters[2]);
  89     matrix[1][0]= -cos(transformParameters[1])*sin(transformParameters[2]);
  90     matrix[1][1]= -sin(transformParameters[0])*sin(transformParameters[1])*sin(transformParameters[2])+cos(transformParameters[0])*cos(transformParameters[2]);
  91     matrix[1][2]= cos(transformParameters[0])*sin(transformParameters[1])*sin(transformParameters[2])+sin(transformParameters[0])*cos(transformParameters[2]);
  92     matrix[2][0]= sin(transformParameters[1]);
  93     matrix[2][1]= -sin(transformParameters[0])*cos(transformParameters[1]);
  94     matrix[2][2]= cos(transformParameters[0])*cos(transformParameters[1]);
  95     //translational part
  96     matrix[0][3]=transformParameters[3];
  97     matrix[1][3]=transformParameters[4];
  98     matrix[2][3]=transformParameters[5];
  99     //homogenize
 100     matrix[3][0]=0.;
 101     matrix[3][1]=0.;
 102     matrix[3][2]=0.;
 103     matrix[3][3]=1.;
 104     return matrix;
 105   }
 106
 107
 108   inline  itk::Matrix<double, 4, 4> GetBackwardAffineMatrix3D(itk::Array<double> transformParameters)
 109   {
 110     itk::Matrix<double, 4, 4> matrix;
 111     //rotational part
 112     matrix[0][0]= cos(transformParameters[1])*cos(transformParameters[2]);
 113     matrix[0][1]= sin(transformParameters[0])*sin(transformParameters[1])*cos(transformParameters[2])- sin(transformParameters[2])*cos(transformParameters[0]);
 114     matrix[0][2]= cos(transformParameters[0])*sin(transformParameters[1])*cos(transformParameters[2])+sin(transformParameters[0])*sin(transformParameters[2]);
 115     matrix[1][0]= cos(transformParameters[1])*sin(transformParameters[2]);
 116     matrix[1][1]= sin(transformParameters[0])*sin(transformParameters[1])*sin(transformParameters[2])+cos(transformParameters[0])*cos(transformParameters[2]);
 117     matrix[1][2]= cos(transformParameters[0])*sin(transformParameters[1])*sin(transformParameters[2])-sin(transformParameters[0])*cos(transformParameters[2]);
 118     matrix[2][0]= -sin(transformParameters[1]);
 119     matrix[2][1]= sin(transformParameters[0])*cos(transformParameters[1]);
 120     matrix[2][2]= cos(transformParameters[0])*cos(transformParameters[1]);
 121     //translational part
 122     matrix[0][3]=transformParameters[3];
 123     matrix[1][3]=transformParameters[4];
 124     matrix[2][3]=transformParameters[5];
 125     //homogenize
 126     matrix[3][0]=0.;
 127     matrix[3][1]=0.;
 128     matrix[3][2]=0.;
 129     matrix[3][3]=1.;
 130     return matrix;
 131   }
 132
 133   inline itk::Matrix<double, 3, 3> GetRotationMatrix3D(itk::Array<double> rotationParameters)
 134   {
 135     itk::Matrix<double, 3, 3> matrix;
 136     //rotational part
 137     matrix[0][0]= cos(rotationParameters[1])*cos(rotationParameters[2]);
 138     matrix[0][1]= sin(rotationParameters[0])*sin(rotationParameters[1])*cos(rotationParameters[2])+ sin(rotationParameters[2])*cos(rotationParameters[0]);
 139     matrix[0][2]= -cos(rotationParameters[0])*sin(rotationParameters[1])*cos(rotationParameters[2])+sin(rotationParameters[0])*sin(rotationParameters[2]);
 140     matrix[1][0]= -cos(rotationParameters[1])*sin(rotationParameters[2]);
 141     matrix[1][1]= -sin(rotationParameters[0])*sin(rotationParameters[1])*sin(rotationParameters[2])+cos(rotationParameters[0])*cos(rotationParameters[2]);
 142     matrix[1][2]= cos(rotationParameters[0])*sin(rotationParameters[1])*sin(rotationParameters[2])+sin(rotationParameters[0])*cos(rotationParameters[2]);
 143     matrix[2][0]= sin(rotationParameters[1]);
 144     matrix[2][1]= -sin(rotationParameters[0])*cos(rotationParameters[1]);
 145     matrix[2][2]= cos(rotationParameters[0])*cos(rotationParameters[1]);
 146     return matrix;
 147   }
 148
 149
 150
 151   //========================================================================================
 152   inline  itk::Point<double, 3> GetRotatedPoint3D(itk::Array<double> rotationParameters, itk::Point<double, 3> input)
 153   {
 154     itk::Matrix<double, 3, 3> matrix = GetRotationMatrix3D(rotationParameters);
 155     itk::Point<double, 3> output;
 156     for (unsigned int i=0;i<3;i++)
 157       {
 158         output[i]=0.0;
 159         for (unsigned int j=0;j<3;j++)
 160           output[i]+=matrix(i,j)*input[j];
 161       }
 162     return output;
 163   }
 164
 165
 166   inline itk::Matrix<double, 4, 4> GetCenteredRotationMatrix3D(itk::Array<double> rotationParameters,itk::Point<double,3> centerOfRotation )
 167   {
 168     //rotational part is identical as affine matrix, translations change
 169     itk::Array<double> parameters(6);
 170     for(unsigned int i=0; i<3;i++) parameters[i]=rotationParameters[i];
 171     for(unsigned int i=3; i<6;i++) parameters[i]=centerOfRotation[i-3];
 172     itk::Matrix<double, 4, 4> matrix=GetForwardAffineMatrix3D(parameters);
 173
 174     //Get the rotation of the centerOfRotation
 175     itk::Matrix<double,3,3> rotation = GetRotationalPartMatrix3D(matrix);
 176     itk::Point<double,3> rotatedCenter=rotation*centerOfRotation; //GetRotatedPoint3D(rotationParameters, centerOfRotation);
 177
 178     //Substract this point to the translational part
 179     matrix(0,3)-=rotatedCenter[0];
 180     matrix(1,3)-=rotatedCenter[1];
 181     matrix(2,3)-=rotatedCenter[2];
 182     return matrix;
 183   }
 184
 185
 186   //   inline  itk::Matrix<double, 4, 4> GetComposedMatrix3D(itk::Matrix<double, 4, 4> firstAppliedTransform, itk::Matrix<double, 4, 4> secondAppliedTransform)
 187   //   {
 188   //     itk::Matrix<double, 4, 4> matrix;
 189   //     for (unsigned int i=0;i<4;i++)
 190   //       for (unsigned int j=0;j<4;j++)
 191   //    {
 192   //      matrix[i][j]=0.0;
 193   //      for (unsigned int k=0;k<4;k++)
 194   //        matrix[i][j]+=firstAppliedTransform[i][k]*secondAppliedTransform[k][j];
 195   //    }
 196   //     return matrix;
 197   //   }
 198
 199
 200   //========================================================================================
 201   inline itk::Matrix<double, 5, 5> ReadMatrix4D(std::string fileName)
 202   {
 203     // read input matrix
 204     std::ifstream is;
 205     openFileForReading(is, fileName);
 206     std::vector<double> nb;
 207     double x;
 208     skipComment(is);
 209     is >> x;
 210     while (!is.eof()) {
 211       nb.push_back(x);
 212       skipComment(is);
 213       is >> x;
 214     }
 215
 216     //copy it to the matrix
 217     itk::Matrix<double, 5, 5> matrix;
 218     unsigned int index=0;
 219     for (unsigned int i=0;i<5;i++)
 220       for (unsigned int j=0;j<5;j++)
 221         matrix[i][j]=nb[index++];
 222     return matrix;
 223   }
 224
 225   inline itk::Matrix<double, 4, 4> ReadMatrix3D(std::string fileName)
 226   {
 227     // read input matrix
 228     std::ifstream is;
 229     openFileForReading(is, fileName);
 230     std::vector<double> nb;
 231     double x;
 232     skipComment(is);
 233     is >> x;
 234     while (!is.eof()) {
 235       nb.push_back(x);
 236       skipComment(is);
 237       is >> x;
 238     }
 239
 240     //copy it to the matrix
 241     itk::Matrix<double, 4, 4> matrix;
 242     unsigned int index=0;
 243     for (unsigned int i=0;i<4;i++)
 244       for (unsigned int j=0;j<4;j++)
 245         matrix[i][j]=nb[index++];
 246     return matrix;
 247   }
 248
 249   inline itk::Matrix<double, 3, 3> ReadMatrix2D(std::string fileName)
 250   {
 251     // read input matrix
 252     std::ifstream is;
 253     openFileForReading(is, fileName);
 254     std::vector<double> nb;
 255     double x;
 256     skipComment(is);
 257     is >> x;
 258     while (!is.eof()) {
 259       nb.push_back(x);
 260       skipComment(is);
 261       is >> x;
 262     }
 263
 264     //copy it to the matrix
 265     itk::Matrix<double, 3, 3> matrix;
 266     unsigned int index=0;
 267     for (unsigned int i=0;i<3;i++)
 268       for (unsigned int j=0;j<3;j++)
 269         matrix[i][j]=nb[index++];
 270     return matrix;
 271   }
 272
 273   template <unsigned int Dimension > inline itk::Matrix<double, Dimension+1 , Dimension+1> ReadMatrix(std::string fileName)
 274   {
 275
 276     // read input matrix
 277     std::ifstream is;
 278     openFileForReading(is, fileName);
 279     std::vector<double> nb;
 280     double x;
 281     skipComment(is);
 282     is >> x;
 283     while (!is.eof()) {
 284       nb.push_back(x);
 285       skipComment(is);
 286       is >> x;
 287     }
 288
 289     //copy it to the matrix
 290     itk::Matrix<double, Dimension+1, Dimension+1> matrix;
 291     unsigned int index=0;
 292     for (unsigned int i=0;i<Dimension+1;i++)
 293       for (unsigned int j=0;j<Dimension+1;j++)
 294         matrix[i][j]=nb[index++];
 295     return matrix;
 296   }
 297
 298
 299   //    template<> inline itk::Matrix<double, 3, 3>  ReadMatrix<2> (std::string fileName)
 300   //    {
 301   //      return  ReadMatrix2D(fileName);
 302   //    }
 303   //    template<> inline itk::Matrix<double, 4, 4>  ReadMatrix<3> (std::string fileName)
 304   //    {
 305   //      return  ReadMatrix3D(fileName);
 306   //    }
 307   //  template<> inline itk::Matrix<double, 5, 5>  ReadMatrix<4> (std::string fileName)
 308   //    {
 309   //      return  ReadMatrix4D(fileName);
 310   //    }
 311
 312
 313   //========================================================================================
 314   inline  itk::Matrix<double, 4, 4> GetRotationalPartMatrix4D(itk::Matrix<double, 5, 5> input)
 315   {
 316     itk::Matrix<double,4,4> matrix;
 317     matrix[0][0]= input[0][0];
 318     matrix[0][1]= input[0][1];
 319     matrix[0][2]= input[0][2];
 320     matrix[0][3]= input[0][3];
 321     matrix[1][0]= input[1][0];
 322     matrix[1][1]= input[1][1];
 323     matrix[1][2]= input[1][2];
 324     matrix[1][3]= input[1][3];
 325     matrix[2][0]= input[2][0];
 326     matrix[2][1]= input[2][1];
 327     matrix[2][2]= input[2][2];
 328     matrix[2][2]= input[2][2];
 329     matrix[2][3]= input[2][3];
 330     matrix[3][0]= input[3][0];
 331     matrix[3][1]= input[3][1];
 332     matrix[3][2]= input[3][2];
 333     matrix[3][2]= input[3][2];
 334     matrix[3][3]= input[3][3];
 335
 336     return matrix;
 337   }
 338
 339
 340   inline  itk::Matrix<double, 3, 3> GetRotationalPartMatrix3D(itk::Matrix<double, 4, 4> input)
 341   {
 342     itk::Matrix<double,3,3> matrix;
 343     matrix[0][0]= input[0][0];
 344     matrix[0][1]= input[0][1];
 345     matrix[0][2]= input[0][2];
 346     matrix[1][0]= input[1][0];
 347     matrix[1][1]= input[1][1];
 348     matrix[1][2]= input[1][2];
 349     matrix[2][0]= input[2][0];
 350     matrix[2][1]= input[2][1];
 351     matrix[2][2]= input[2][2];
 352     return matrix;
 353   }
 354
 355   inline  itk::Matrix<double, 2, 2> GetRotationalPartMatrix2D(itk::Matrix<double, 3, 3> input)
 356   {
 357     itk::Matrix<double,2,2> matrix;
 358     matrix[0][0]= input[0][0];
 359     matrix[0][1]= input[0][1];
 360     matrix[0][2]= input[0][2];
 361     matrix[1][0]= input[1][0];
 362     matrix[1][1]= input[1][1];
 363     return matrix;
 364   }
 365
 366   inline itk::Matrix<double, 4, 4> GetRotationalPartMatrix(itk::Matrix<double, 5, 5> input)
 367   {
 368     return GetRotationalPartMatrix4D(input);
 369   }
 370
 371   inline itk::Matrix<double, 3, 3> GetRotationalPartMatrix(itk::Matrix<double, 4, 4> input)
 372   {
 373     return GetRotationalPartMatrix3D(input);
 374   }
 375
 376   inline itk::Matrix<double, 2, 2> GetRotationalPartMatrix(itk::Matrix<double, 3, 3> input)
 377   {
 378     return GetRotationalPartMatrix2D(input);
 379   }
 380
 381
 382   //========================================================================================
 383   inline  itk::Vector<double,4> GetTranslationPartMatrix4D(itk::Matrix<double, 5, 5> input)
 384   {
 385     itk::Vector<double,4> vec;
 386     vec[0]= input[0][4];
 387     vec[1]= input[1][4];
 388     vec[2]= input[2][4];
 389     vec[3]= input[3][4];
 390     return vec;
 391
 392   }
 393
 394   inline  itk::Vector<double,3> GetTranslationPartMatrix3D(itk::Matrix<double, 4, 4> input)
 395   {
 396     itk::Vector<double,3> vec;
 397     vec[0]= input[0][3];
 398     vec[1]= input[1][3];
 399     vec[2]= input[2][3];
 400     return vec;
 401
 402   }
 403   inline  itk::Vector<double,2> GetTranslationPartMatrix2D(itk::Matrix<double, 3, 3> input)
 404   {
 405     itk::Vector<double,2> vec;
 406     vec[0]= input[0][2];
 407     vec[1]= input[1][2];
 408     return vec;
 409
 410   }
 411
 412   inline itk::Vector<double,4> GetTranslationPartMatrix(itk::Matrix<double, 5, 5> input)
 413   {
 414
 415     return GetTranslationPartMatrix4D(input);
 416   }
 417
 418   inline itk::Vector<double,3> GetTranslationPartMatrix(itk::Matrix<double, 4, 4> input)
 419   {
 420
 421     return GetTranslationPartMatrix3D(input);
 422   }
 423
 424   inline itk::Vector<double,2> GetTranslationPartMatrix(itk::Matrix<double, 3, 3> input)
 425   {
 426
 427     return GetTranslationPartMatrix2D(input);
 428   }
 429 }
 430
 431 #endif //#define CLITKTRANSFORMUTILITIES_H