common/clitkTransformUtilities.h

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