MNE-CPP  0.1.9
A Framework for Electrophysiology
fiff_coord_trans_old.cpp
Go to the documentation of this file.
1 //=============================================================================================================
37 //=============================================================================================================
38 // INCLUDES
39 //=============================================================================================================
40 
41 #include "fiff_coord_trans_old.h"
42 
43 #include <fiff/fiff_tag.h>
44 
45 #include <QFile>
46 
47 #include <Eigen/Dense>
48 
49 //=============================================================================================================
50 // USED NAMESPACES
51 //=============================================================================================================
52 
53 using namespace Eigen;
54 using namespace FIFFLIB;
55 using namespace FIFFLIB;
56 
57 #ifndef TRUE
58 #define TRUE 1
59 #endif
60 
61 #ifndef FALSE
62 #define FALSE 0
63 #endif
64 
65 #ifndef FAIL
66 #define FAIL -1
67 #endif
68 
69 #ifndef OK
70 #define OK 0
71 #endif
72 
73 #define X_20 0
74 #define Y_20 1
75 #define Z_20 2
76 
77 #define FREE_20(x) if ((char *)(x) != NULL) free((char *)(x))
78 
79 #define MALLOC_20(x,t) (t *)malloc((x)*sizeof(t))
80 
81 static void matrix_error_20(int kind, int nr, int nc)
82 
83 {
84  if (kind == 1)
85  printf("Failed to allocate memory pointers for a %d x %d matrix\n",nr,nc);
86  else if (kind == 2)
87  printf("Failed to allocate memory for a %d x %d matrix\n",nr,nc);
88  else
89  printf("Allocation error for a %d x %d matrix\n",nr,nc);
90  if (sizeof(void *) == 4) {
91  printf("This is probably because you seem to be using a computer with 32-bit architecture.\n");
92  printf("Please consider moving to a 64-bit platform.");
93  }
94  printf("Cannot continue. Sorry.\n");
95  exit(1);
96 }
97 
98 float **mne_cmatrix_20(int nr,int nc)
99 
100 {
101  int i;
102  float **m;
103  float *whole;
104 
105  m = MALLOC_20(nr,float *);
106  if (!m) matrix_error_20(1,nr,nc);
107  whole = MALLOC_20(nr*nc,float);
108  if (!whole) matrix_error_20(2,nr,nc);
109 
110  for(i=0;i<nr;i++)
111  m[i] = whole + i*nc;
112  return m;
113 }
114 
115 #define VEC_DIFF_20(from,to,diff) {\
116  (diff)[X_20] = (to)[X_20] - (from)[X_20];\
117  (diff)[Y_20] = (to)[Y_20] - (from)[Y_20];\
118  (diff)[Z_20] = (to)[Z_20] - (from)[Z_20];\
119  }
120 
121 #define ALLOC_CMATRIX_20(x,y) mne_cmatrix_20((x),(y))
122 
123 #define MAXWORD 1000
124 
125 #define VEC_DOT_20(x,y) ((x)[X_20]*(y)[X_20] + (x)[Y_20]*(y)[Y_20] + (x)[Z_20]*(y)[Z_20])
126 
127 #define VEC_LEN_20(x) sqrt(VEC_DOT_20(x,x))
128 
129 #define CROSS_PRODUCT_20(x,y,xy) {\
130  (xy)[X_20] = (x)[Y_20]*(y)[Z_20]-(y)[Y_20]*(x)[Z_20];\
131  (xy)[Y_20] = -((x)[X_20]*(y)[Z_20]-(y)[X_20]*(x)[Z_20]);\
132  (xy)[Z_20] = (x)[X_20]*(y)[Y_20]-(y)[X_20]*(x)[Y_20];\
133  }
134 
135 #define FREE_CMATRIX_20(m) mne_free_cmatrix_20((m))
136 
137 void mne_free_cmatrix_20(float **m)
138 {
139  if (m) {
140  FREE_20(*m);
141  FREE_20(m);
142  }
143 }
144 
145 #define MIN_20(a,b) ((a) < (b) ? (a) : (b))
146 
147 //float
148 Eigen::MatrixXf toFloatEigenMatrix_20(float **mat, const int m, const int n)
149 {
150  Eigen::MatrixXf eigen_mat(m,n);
151 
152  for ( int i = 0; i < m; ++i)
153  for ( int j = 0; j < n; ++j)
154  eigen_mat(i,j) = mat[i][j];
155 
156  return eigen_mat;
157 }
158 
159 void fromFloatEigenVector_20(const Eigen::VectorXf& from_vec, float *to_vec, const int n)
160 {
161  for ( int i = 0; i < n; ++i)
162  to_vec[i] = from_vec[i];
163 }
164 
165 void fromFloatEigenMatrix_20(const Eigen::MatrixXf& from_mat, float **& to_mat, const int m, const int n)
166 {
167  for ( int i = 0; i < m; ++i)
168  for ( int j = 0; j < n; ++j)
169  to_mat[i][j] = from_mat(i,j);
170 }
171 
172 int mne_svd_20(float **mat, /* The matrix */
173  int m,int n, /* m rows n columns */
174  float *sing, /* Singular values (must have size
175  * MIN(m,n)+1 */
176  float **uu, /* Left eigenvectors */
177  float **vv) /* Right eigenvectors */
178 /*
179  * Compute the SVD of mat.
180  * The singular vector calculations depend on whether
181  * or not u and v are given.
182  *
183  * The allocations should be done as follows
184  *
185  * mat = ALLOC_CMATRIX_3(m,n);
186  * vv = ALLOC_CMATRIX_3(MIN(m,n),n);
187  * uu = ALLOC_CMATRIX_3(MIN(m,n),m);
188  * sing = MALLOC_3(MIN(m,n),float);
189  *
190  * mat is modified by this operation
191  *
192  * This simply allocates the workspace and calls the
193  * LAPACK Fortran routine
194  */
195 
196 {
197  int udim = MIN_20(m,n);
198 
199  Eigen::MatrixXf eigen_mat = toFloatEigenMatrix_20(mat, m, n);
200 
201  //ToDo Optimize computation depending of whether uu or vv are defined
202  Eigen::JacobiSVD< Eigen::MatrixXf > svd(eigen_mat ,Eigen::ComputeFullU | Eigen::ComputeFullV);
203 
204  fromFloatEigenVector_20(svd.singularValues(), sing, svd.singularValues().size());
205 
206  if (uu != NULL)
207  fromFloatEigenMatrix_20(svd.matrixU().transpose(), uu, udim, m);
208 
209  if (vv != NULL)
210  fromFloatEigenMatrix_20(svd.matrixV().transpose(), vv, m, n);
211 
212  return 0;
213  // return info;
214 }
215 
216 void mne_matt_mat_mult2_20 (float **m1,float **m2,float **result,
217  int d1,int d2,int d3)
218  /* Matrix multiplication
219  * result(d1 x d3) = m1(d2 x d1)^T * m2(d2 x d3) */
220 
221 {
222  #ifdef BLAS
223  char *transa = "N";
224  char *transb = "T";
225  float zero = 0.0;
226  float one = 1.0;
227 
228  sgemm (transa,transb,&d3,&d1,&d2,
229  &one,m2[0],&d3,m1[0],&d1,&zero,result[0],&d3);
230 
231  return;
232  #else
233  int j,k,p;
234  float sum;
235  int one = 1;
236 
237  for (j = 0; j < d1; j++)
238  for (k = 0; k < d3; k++) {
239  sum = 0.0;
240  for (p = 0; p < d2; p++)
241  sum = sum + m1[p][j]*m2[p][k];
242  result[j][k] = sum;
243  }
244  return;
245  #endif
246 }
247 
248 float **mne_matt_mat_mult_20 (float **m1,float **m2,int d1,int d2,int d3)
249 
250 {
251  float **result = ALLOC_CMATRIX_20(d1,d3);
252  mne_matt_mat_mult2_20(m1,m2,result,d1,d2,d3);
253  return result;
254 }
255 
256 static void skip_comments(FILE *in)
257 
258 {
259  int c;
260 
261  while (1) {
262  c = fgetc(in);
263  if (c == '#') {
264  for (c = fgetc(in); c != EOF && c != '\n'; c = fgetc(in))
265  ;
266  }
267  else {
268  ungetc(c,in);
269  return;
270  }
271  }
272 }
273 
274 static int whitespace(int c)
275 
276 {
277  if (c == '\t' || c == '\n' || c == ' ')
278  return TRUE;
279  else
280  return FALSE;
281 }
282 
283 static int whitespace_quote(int c, int inquote)
284 
285 {
286  if (inquote)
287  return (c == '"');
288  else
289  return (c == '\t' || c == '\n' || c == ' ');
290 }
291 
292 static char *next_word_20(FILE *in)
293 
294 {
295  char *next = MALLOC_20(MAXWORD,char);
296  int c;
297  int p,k;
298  int inquote;
299 
300  skip_comments(in);
301 
302  inquote = FALSE;
303  for (k = 0, p = 0, c = fgetc(in); c != EOF && !whitespace_quote(c,inquote) ; c = fgetc(in), k++) {
304  if (k == 0 && c == '"')
305  inquote = TRUE;
306  else
307  next[p++] = c;
308  }
309  if (c == EOF && k == 0) {
310  FREE_20(next);
311  return NULL;
312  }
313  else
314  next[p] = '\0';
315  if (c != EOF) {
316  for (k = 0, c = fgetc(in); whitespace(c) ; c = fgetc(in), k++)
317  ;
318  if (c != EOF)
319  ungetc(c,in);
320  }
321 #ifdef DEBUG
322  if (next)
323  printf("<%s>\n",next);
324 #endif
325  return next;
326 }
327 
328 static int get_fval_20(FILE *in, float *fval)
329 {
330  char *next = next_word_20(in);
331  if (next == NULL) {
332  qWarning("bad integer");
333  return FAIL;
334  }
335  else if (sscanf(next,"%g",fval) != 1) {
336  qWarning("bad floating point number : %s",next);
337  FREE_20(next);
338  return FAIL;
339  }
340  FREE_20(next);
341  return OK;
342 }
343 
344 //=============================================================================================================
345 // DEFINE MEMBER METHODS
346 //=============================================================================================================
347 
348 FiffCoordTransOld::FiffCoordTransOld()
349 {
350 }
351 
352 //=============================================================================================================
353 
354 FiffCoordTransOld::FiffCoordTransOld(const FiffCoordTransOld &p_FiffCoordTransOld)
355 {
356  this->from = p_FiffCoordTransOld.from;
357  this->to = p_FiffCoordTransOld.to;
358 
359  for (int j = 0; j < 3; j++) {
360  this->move[j] = p_FiffCoordTransOld.move[j];
361  this->invmove[j] = p_FiffCoordTransOld.invmove[j];
362  for (int k = 0; k < 3; k++) {
363  this->rot(j,k) = p_FiffCoordTransOld.rot(j,k);
364  this->invrot(j,k) = p_FiffCoordTransOld.invrot(j,k);
365  }
366  }
367 }
368 
369 //=============================================================================================================
370 
371 FiffCoordTransOld *FiffCoordTransOld::catenate(FiffCoordTransOld *t1, FiffCoordTransOld *t2)
372 {
374  int j,k,p;
375 
376  t->to = t1->to;
377  t->from = t2->from;
378 
379  for (j = 0; j < 3; j++) {
380  t->move[j] = t1->move[j];
381  for (k = 0; k < 3; k++) {
382  t->rot(j,k) = 0.0;
383  t->move[j] += t1->rot(j,k)*t2->move[k];
384  for (p = 0; p < 3; p++)
385  t->rot(j,k) += t1->rot(j,p)*t2->rot(p,k);
386  }
387  }
388  add_inverse(t);
389  return (t);
390 }
391 
392 //=============================================================================================================
393 
394 FiffCoordTransOld::FiffCoordTransOld(int from, int to, float rot[3][3], float move[3])
395 {
396  this->from = from;
397  this->to = to;
398 
399  for (int j = 0; j < 3; j++) {
400  this->move[j] = move[j];
401  for (int k = 0; k < 3; k++)
402  this->rot(j,k) = rot[j][k];
403  }
404  add_inverse(this);
405 }
406 
407 //=============================================================================================================
408 
409 FiffCoordTransOld::~FiffCoordTransOld()
410 {
411 }
412 
413 //=============================================================================================================
414 
415 FiffCoordTrans FiffCoordTransOld::toNew()
416 {
418  MatrixXf trans = MatrixXf::Identity(4,4);
419  trans.block(0,0,3,3) = this->rot;
420  trans.block(0,3,3,1) = this->move;
421  t.to = this->to;
422  t.from = this->from;
423  t.trans = trans;
424  t.addInverse(t);
425  return t;
426 }
427 
428 //=============================================================================================================
429 
430 int FiffCoordTransOld::add_inverse(FiffCoordTransOld *t)
431 {
432  Matrix4f m = Matrix4f::Identity(4,4);
433 
434  m.block(0,0,3,3) = t->rot;
435  m.block(0,3,3,1) = t->move;
436 
437  m = m.inverse().eval();
438 
439  t->invrot = m.block(0,0,3,3);
440  t->invmove = m.block(0,3,3,1);
441 
442  return OK;
443 }
444 
445 //=============================================================================================================
446 
447 FiffCoordTransOld *FiffCoordTransOld::fiff_invert_transform() const
448 {
450  ti->move = this->invmove;
451  ti->invmove = this ->move;
452  ti->rot = this->invrot;
453  ti->invrot = this ->rot;
454  ti->from = this->to;
455  ti->to = this->from;
456  return (ti);
457 }
458 
459 //=============================================================================================================
460 
461 void FiffCoordTransOld::fiff_coord_trans(float r[], const FiffCoordTransOld *t, int do_move)
462 /*
463  * Apply coordinate transformation
464  */
465 {
466  int j,k;
467  float res[3];
468 
469  for (j = 0; j < 3; j++) {
470  res[j] = (do_move ? t->move[j] : 0.0);
471  for (k = 0; k < 3; k++)
472  res[j] += t->rot(j,k)*r[k];
473  }
474  for (j = 0; j < 3; j++)
475  r[j] = res[j];
476 }
477 
478 //=============================================================================================================
479 
480 FiffCoordTransOld *FiffCoordTransOld::fiff_combine_transforms(int from, int to, FiffCoordTransOld *t1, FiffCoordTransOld *t2)
481 /*
482  * Combine two coordinate transformations
483  * to yield a transform from 'from' system
484  * to 'to' system.
485  *
486  * Return NULL if this fails
487  *
488  */
489 {
490  FiffCoordTransOld* t = NULL;
491  int swapped = 0;
492  FiffCoordTransOld* temp;
493  /*
494  * We have a total of eight possibilities:
495  * four without swapping and four with
496  */
497  while (1) {
498  if (t1->to == to && t2->from == from) {
499  t1 = new FiffCoordTransOld(*t1);
500  t2 = new FiffCoordTransOld(*t2);
501  break;
502  }
503  else if (t1->from == to && t2->from == from) {
504  FiffCoordTransOld* tmp_t1 = t1;
505  t1 = tmp_t1->fiff_invert_transform();//Memory leak here!!
506  delete tmp_t1;
507  t2 = new FiffCoordTransOld(*t2);
508  break;
509  }
510  else if (t1->to == to && t2->to == from) {
511  t1 = new FiffCoordTransOld(*t1);
512  FiffCoordTransOld* tmp_t2 = t2;
513  t2 = tmp_t2->fiff_invert_transform();//Memory leak here!!
514  delete tmp_t2;
515  break;
516  }
517  else if (t1->from == to && t2->to == from) {
518  FiffCoordTransOld* tmp_t1 = t1;
519  t1 = tmp_t1->fiff_invert_transform();//Memory leak here!!
520  delete tmp_t1;
521  FiffCoordTransOld* tmp_t2 = t2;
522  t2 = tmp_t2->fiff_invert_transform();//Memory leak here!!
523  delete tmp_t2;
524  break;
525  }
526  if (swapped) { /* Already swapped and not found */
527  qCritical("Cannot combine coordinate transforms");
528  return (t);
529  }
530  temp = t1; /* Try it swapped as well */
531  t1 = t2;
532  t2 = temp;
533  swapped = 1;
534  }
535  if (t1->from != t2->to) /* Transforms must match on the other side as well */
536  qCritical ("Cannot combine coordinate transforms");
537  else /* We can do it directly */
538  t = catenate(t1,t2);
539  FREE_20(t1); FREE_20(t2);
540  return (t);
541 }
542 
543 //=============================================================================================================
544 
545 void FiffCoordTransOld::fiff_coord_trans_inv(float r[], FiffCoordTransOld *t, int do_move)
546 /*
547  * Apply inverse coordinate transformation
548  */
549 {
550  int j,k;
551  float res[3];
552 
553  for (j = 0; j < 3; j++) {
554  res[j] = (do_move ? t->invmove[j] : 0.0);
555  for (k = 0; k < 3; k++)
556  res[j] += t->invrot(j,k)*r[k];
557  }
558  for (j = 0; j < 3; j++)
559  r[j] = res[j];
560 }
561 
562 //=============================================================================================================
563 
564 namespace FIFFLIB
565 {
566 
567 typedef struct {
568  int frame;
569  const char *name;
570 } frameNameRec;
571 
572 }
573 
574 const char *FiffCoordTransOld::mne_coord_frame_name(int frame)
575 {
576  static frameNameRec frames[] = {
577  {FIFFV_COORD_UNKNOWN,"unknown"},
578  {FIFFV_COORD_DEVICE,"MEG device"},
579  {FIFFV_COORD_ISOTRAK,"isotrak"},
580  {FIFFV_COORD_HPI,"hpi"},
581  {FIFFV_COORD_HEAD,"head"},
582  {FIFFV_COORD_MRI,"MRI (surface RAS)"},
583  {FIFFV_MNE_COORD_MRI_VOXEL, "MRI voxel"},
584  {FIFFV_COORD_MRI_SLICE,"MRI slice"},
585  {FIFFV_COORD_MRI_DISPLAY,"MRI display"},
586  {FIFFV_MNE_COORD_CTF_DEVICE,"CTF MEG device"},
587  {FIFFV_MNE_COORD_CTF_HEAD,"CTF/4D/KIT head"},
588  {FIFFV_MNE_COORD_RAS,"RAS (non-zero origin)"},
589  {FIFFV_MNE_COORD_MNI_TAL,"MNI Talairach"},
590  {FIFFV_MNE_COORD_FS_TAL_GTZ,"Talairach (MNI z > 0)"},
591  {FIFFV_MNE_COORD_FS_TAL_LTZ,"Talairach (MNI z < 0)"},
592  {-1,"unknown"}
593  };
594  int k;
595  for (k = 0; frames[k].frame != -1; k++) {
596  if (frame == frames[k].frame)
597  return frames[k].name;
598  }
599  return frames[k].name;
600 }
601 
602 //=============================================================================================================
603 
604 void FiffCoordTransOld::mne_print_coord_transform_label(FILE *log, char *label, FiffCoordTransOld *t)
605 {
606  int k,p;
607  int frame;
608  if (!label || strlen(label) == 0)
609  fprintf(log,"Coordinate transformation: ");
610  else
611  fprintf(log,"%s",label);
612  for (frame = t->from, k = 0; k < 2; k++) {
613  if (k == 0) {
614  fprintf(log,"%s -> ",mne_coord_frame_name(frame));
615  frame = t->to;
616  }
617  else {
618  fprintf(log,"%s\n",mne_coord_frame_name(frame));
619  for (p = 0; p < 3; p++)
620  fprintf(log,"\t% 8.6f % 8.6f % 8.6f\t% 7.2f mm\n",
621  t->rot(p,X_20),t->rot(p,Y_20),t->rot(p,Z_20),1000*t->move[p]);
622  fprintf(log,"\t% 8.6f % 8.6f % 8.6f % 7.2f\n",0.0,0.0,0.0,1.0);
623  }
624  }
625 }
626 
627 //=============================================================================================================
628 
629 void FiffCoordTransOld::mne_print_coord_transform(FILE *log, FiffCoordTransOld *t)
630 {
631  mne_print_coord_transform_label(log,NULL,t);
632 }
633 
634 //=============================================================================================================
635 
636 FiffCoordTransOld *FiffCoordTransOld::mne_read_transform(const QString &name, int from, int to)
637 /*
638  * Read the specified coordinate transformation
639  */
640 {
641  QFile file(name);
642  FiffStream::SPtr stream(new FiffStream(&file));
643 
644  FiffCoordTransOld* res = NULL;
645  // fiffFile in = NULL;
646  FiffTag::SPtr t_pTag;
647  // fiffTagRec tag;
648  // fiffDirEntry dir;
649  fiff_int_t kind, pos;
650  int k;
651 
652  // tag.data = NULL;
653  // if ((in = fiff_open(name.toUtf8().data())) == NULL)
654  // goto out;
655  if(!stream->open())
656  goto out;
657 
658  for (k = 0; k < stream->dir().size(); k++) {
659  kind = stream->dir()[k]->kind;
660  pos = stream->dir()[k]->pos;
661  if (kind == FIFF_COORD_TRANS) {
662  // if (fiff_read_this_tag (in->fd,dir->pos,&tag) == FIFF_FAIL)
663  // goto out;
664  // res = (fiffCoordTrans)tag.data;
665  if (!stream->read_tag(t_pTag,pos))
666  goto out;
667 
668  res = FiffCoordTransOld::read_helper( t_pTag );
669  if (res->from == from && res->to == to) {
670  // tag.data = NULL;
671  goto out;
672  }
673  else if (res->from == to && res->to == from) {
674  FiffCoordTransOld* tmp_res = res;
675  res = tmp_res->fiff_invert_transform();//Memory leak here!!
676  delete tmp_res;
677  goto out;
678  }
679  res = NULL;
680  }
681  }
682  qCritical("No suitable coordinate transformation found in %s.",name.toUtf8().data());
683  goto out;
684 
685 out : {
686  // FREE(tag.data);
687  // fiff_close(in);
688  stream->close();
689  return res;
690  }
691 
692  return res;
693 }
694 
695 //=============================================================================================================
696 
697 FiffCoordTransOld *FiffCoordTransOld::mne_read_transform_from_node(FiffStream::SPtr &stream, const FiffDirNode::SPtr &node, int from, int to)
698 /*
699  * Read the specified coordinate transformation
700  */
701 {
702  FiffCoordTransOld* res = NULL;
703  FiffTag::SPtr t_pTag;
704  // fiffTagRec tag;
705  // fiffDirEntry dir;
706  fiff_int_t kind, pos;
707  int k;
708 
709  // tag.data = NULL;
710  for (k = 0; k < node->nent(); k++)
711  kind = node->dir[k]->kind;
712  pos = node->dir[k]->pos;
713  if (kind == FIFF_COORD_TRANS) {
714  // if (fiff_read_this_tag (in->fd,dir->pos,&tag) == FIFF_FAIL)
715  // goto out;
716  // res = (fiffCoordTrans)tag.data;
717  if (!stream->read_tag(t_pTag,pos))
718  goto out;
719  res = FiffCoordTransOld::read_helper( t_pTag );
720  if (res->from == from && res->to == to) {
721  // tag.data = NULL;
722  goto out;
723  }
724  else if (res->from == to && res->to == from) {
725  FiffCoordTransOld* tmp_res = res;
726  res = tmp_res->fiff_invert_transform();//Memory leak here!!
727  delete tmp_res;
728  goto out;
729  }
730  res = NULL;
731  }
732  printf("No suitable coordinate transformation found");
733  goto out;
734 
735 out : {
736  // FREE(tag.data);
737  return res;
738  }
739 }
740 
741 //=============================================================================================================
742 
743 FiffCoordTransOld *FiffCoordTransOld::mne_read_mri_transform(const QString &name)
744 /*
745  * Read the MRI -> HEAD coordinate transformation
746  */
747 {
748  return mne_read_transform(name,FIFFV_COORD_MRI,FIFFV_COORD_HEAD);
749 }
750 
751 //=============================================================================================================
752 
753 FiffCoordTransOld *FiffCoordTransOld::mne_read_meas_transform(const QString &name)
754 /*
755  * Read the MEG device -> HEAD coordinate transformation
756  */
757 {
758  return mne_read_transform(name,FIFFV_COORD_DEVICE,FIFFV_COORD_HEAD);
759 }
760 
761 //=============================================================================================================
762 
763 FiffCoordTransOld *FiffCoordTransOld::mne_read_transform_ascii(char *name, int from, int to)
764 /*
765  * Read the Neuromag -> FreeSurfer transformation matrix
766  */
767 {
768  FILE *in = NULL;
769  FiffCoordTransOld* t = NULL;
770  float rot[3][3];
771  float move[3];
772  int k;
773  float dum;
774 
775  if ((in = fopen(name,"r")) == NULL) {
776  qCritical(name);
777  goto bad;
778  }
779  for (k = 0; k < 3; k++) {
780  if (get_fval_20(in,rot[k]+X_20) == FAIL)
781  goto noread;
782  if (get_fval_20(in,rot[k]+Y_20) == FAIL)
783  goto noread;
784  if (get_fval_20(in,rot[k]+Z_20) == FAIL)
785  goto noread;
786  if (get_fval_20(in,move+k) == FAIL)
787  goto noread;
788  }
789  for (k = 0; k < 4; k++) {
790  if (get_fval_20(in,&dum) == FAIL)
791  goto noread;
792  }
793  fclose(in);
794  for (k = 0; k < 3; k++)
795  move[k] = move[k]/1000.0;
796  t = new FiffCoordTransOld(from, to, rot, move );
797  return t;
798 
799 noread : {
800  qCritical("Cannot read the coordinate transformation");
801  goto bad;
802  }
803 
804 bad : {
805  if(t)
806  delete t;
807  if (in != NULL)
808  fclose(in);
809  return NULL;
810  }
811 }
812 
813 //=============================================================================================================
814 
815 FiffCoordTransOld *FiffCoordTransOld::mne_read_FShead2mri_transform(char *name)
816 {
817  return mne_read_transform_ascii(name,FIFFV_COORD_HEAD,FIFFV_COORD_MRI);
818 }
819 
820 //=============================================================================================================
821 
822 FiffCoordTransOld *FiffCoordTransOld::mne_identity_transform(int from, int to)
823 {
824  float rot[3][3] = { { 1.0, 0.0, 0.0 },
825  { 0.0, 1.0, 0.0 },
826  { 0.0, 0.0, 1.0 } };
827  float move[] = { 0.0, 0.0, 0.0 };
828  return new FiffCoordTransOld(from,to,rot,move);
829 }
830 
831 //=============================================================================================================
832 
833 FiffCoordTransOld * FiffCoordTransOld::fiff_make_transform_card (int from,int to,
834  float *rL,
835  float *rN,
836  float *rR)
837 /* 'from' coordinate system
838  * cardinal points expressed in
839  * the 'to' system */
840 
841 {
843  float ex[3],ey[3],ez[3]; /* The unit vectors */
844  float alpha,alpha1,len;
845  float diff1[3],diff2[3];
846  int k;
847  float r0[3];
848 
849  t->from = from;
850  t->to = to;
851  for (k = 0; k < 3; k++) {
852  diff1[k] = rN[k] - rL[k];
853  diff2[k] = rR[k] - rL[k];
854  }
855  alpha = VEC_DOT_20(diff1,diff2)/VEC_DOT_20(diff2,diff2);
856  len = VEC_LEN_20(diff2);
857  alpha1 = 1.0 - alpha;
858 
859  for (k = 0; k < 3; k++) {
860  r0[k] = alpha1*rL[k] + alpha*rR[k];
861  ex[k] = diff2[k]/len;
862  ey[k] = rN[k] - r0[k];
863  t->move[k] = r0[k];
864  }
865 
866  len = VEC_LEN_20(ey);
867 
868  for (k = 0; k < 3; k++)
869  ey[k] = ey[k]/len;
870 
871  CROSS_PRODUCT_20 (ex,ey,ez);
872 
873  for (k = 0; k < 3; k++) {
874  t->rot(k,X_20) = ex[k];
875  t->rot(k,Y_20) = ey[k];
876  t->rot(k,Z_20) = ez[k];
877  }
878 
879  add_inverse (t);
880 
881  return (t);
882 }
883 
884 //=============================================================================================================
885 
886 FiffCoordTransOld* FiffCoordTransOld::procrustes_align(int from_frame, /* The coordinate frames */
887  int to_frame,
888  float **fromp, /* Point locations in these two coordinate frames */
889  float **top,
890  float *w, /* Optional weights */
891  int np, /* How many points */
892  float max_diff) /* Maximum allowed difference */
893 /*
894  * Perform an alignment using the the solution of the orthogonal (weighted) Procrustes problem
895  */
896 {
897  float **from = ALLOC_CMATRIX_20(np,3);
898  float **to = ALLOC_CMATRIX_20(np,3);
899  float from0[3],to0[3],rr[3],diff[3];
900  int j,k,c,p;
901  float rot[3][3];
902  float move[3];
903 
904  /*
905  * Calculate the centroids and subtract;
906  */
907  for (c = 0; c < 3; c++)
908  from0[c] = to0[c] = 0.0;
909  for (j = 0; j < np; j++) {
910  for (c = 0; c < 3; c++) {
911  from0[c] += fromp[j][c];
912  to0[c] += top[j][c];
913  }
914  }
915  for (c = 0; c < 3; c++) {
916  from0[c] = from0[c]/np;
917  to0[c] = to0[c]/np;
918  }
919  for (j = 0; j < np; j++) {
920  for (c = 0; c < 3; c++) {
921  from[j][c] = fromp[j][c] - from0[c];
922  to[j][c] = top[j][c] - to0[c];
923  }
924  }
925  /*
926  * Compute the solution of the orthogonal Proscrustes problem
927  */
928  {
929  float **S;
930  float **uu = ALLOC_CMATRIX_20(3,3);
931  float **vv = ALLOC_CMATRIX_20(3,3);
932  float **R = NULL;
933  float sing[3];
934 
935  if (w) {
936  /*
937  * This is the weighted version which allows multiplicity of points
938  */
939  S = ALLOC_CMATRIX_20(3,3);
940  for (j = 0; j < 3; j++) {
941  for (k = 0; k < 3; k++) {
942  S[j][k] = 0.0;
943  for (p = 0; p < np; p++)
944  S[j][k] += w[p]*from[p][j]*to[p][k];
945  }
946  }
947  }
948  else
949  S = mne_matt_mat_mult_20(from,to,3,np,3);
950  if (mne_svd_20(S,3,3,sing,uu,vv) != 0) {
951  FREE_CMATRIX_20(S);
952  FREE_CMATRIX_20(uu);
953  FREE_CMATRIX_20(vv);
954  goto bad;
955  }
956  R = mne_matt_mat_mult_20(vv,uu,3,3,3);
957  for (j = 0; j < 3; j++)
958  for (k = 0; k < 3; k++)
959  rot[j][k] = R[j][k];
960  FREE_CMATRIX_20(R);
961  FREE_CMATRIX_20(S);
962  FREE_CMATRIX_20(uu);
963  FREE_CMATRIX_20(vv);
964  }
965  /*
966  * Now we need to generate a transformed translation vector
967  */
968  for (j = 0; j < 3; j++) {
969  move[j] = to0[j];
970  for (k = 0; k < 3; k++)
971  move[j] = move[j] - rot[j][k]*from0[k];
972  }
973  /*
974  * Test the transformation and print the results
975  */
976  #ifdef DEBUG
977  printf("Procrustes matching (desired vs. transformed) :\n");
978  #endif
979  for (p = 0; p < np; p++) {
980  for (j = 0; j < 3; j++) {
981  rr[j] = move[j];
982  for (k = 0; k < 3; k++)
983  rr[j] += rot[j][k]*fromp[p][k];
984  }
985  VEC_DIFF_20(top[p],rr,diff);
986  #ifdef DEBUG
987  printf("\t%7.2f %7.2f %7.2f mm <-> %7.2f %7.2f %7.2f mm diff = %8.3f mm\n",
988  1000*top[p][0],1000*top[p][1],1000*top[p][2],
989  1000*rr[0],1000*rr[1],1000*rr[2],1000*VEC_LEN(diff));
990  #endif
991  if (VEC_LEN_20(diff) > max_diff) {
992  printf("To large difference in matching : %7.1f > %7.1f mm", 1000*VEC_LEN_20(diff),1000*max_diff);
993  goto bad;
994  }
995  }
996  #ifdef DEBUG
997  printf("\n");
998  #endif
999 
1000  FREE_CMATRIX_20(from);
1001  FREE_CMATRIX_20(to);
1002 
1003  return new FiffCoordTransOld(from_frame,to_frame,rot,move);
1004 
1005  bad : {
1006  FREE_CMATRIX_20(from);
1007  FREE_CMATRIX_20(to);
1008  return NULL;
1009  }
1010 }
1011 
1012 //=============================================================================================================
1013 
1014 FiffCoordTransOld *FiffCoordTransOld::read_helper( FIFFLIB::FiffTag::SPtr& tag)
1015 {
1016  FiffCoordTransOld* p_FiffCoordTrans = NULL;
1017  if(tag->isMatrix() || tag->getType() != FIFFT_COORD_TRANS_STRUCT || tag->data() == NULL)
1018  return p_FiffCoordTrans;
1019  else
1020  {
1021  p_FiffCoordTrans = new FiffCoordTransOld;
1022  qint32* t_pInt32 = (qint32*)tag->data();
1023  p_FiffCoordTrans->from = t_pInt32[0];
1024  p_FiffCoordTrans->to = t_pInt32[1];
1025 
1026  float* t_pFloat = (float*)tag->data();
1027  int count = 0;
1028  int r, c;
1029  for (r = 0; r < 3; ++r) {
1030  p_FiffCoordTrans->move[r] = t_pFloat[11+r];
1031  for (c = 0; c < 3; ++c) {
1032  p_FiffCoordTrans->rot(r,c) = t_pFloat[2+count];
1033  ++count;
1034  }
1035  }
1036 
1037  count = 0;
1038  for (r = 0; r < 3; ++r) {
1039  p_FiffCoordTrans->invmove[r] = t_pFloat[23+r];
1040  for (c = 0; c < 3; ++c) {
1041  p_FiffCoordTrans->invrot(r,c) = t_pFloat[14+count];
1042  ++count;
1043  }
1044  }
1045 
1046  return p_FiffCoordTrans;
1047  }
1048 }
#define FIFFV_MNE_COORD_MRI_VOXEL
#define FIFFV_MNE_COORD_CTF_DEVICE
QSharedPointer< FiffDirNode > SPtr
Definition: fiff_dir_node.h:77
Coordinate transformation description.
#define FIFFV_MNE_COORD_FS_TAL_LTZ
QSharedPointer< FiffTag > SPtr
Definition: fiff_tag.h:152
FIFF File I/O routines.
Definition: fiff_stream.h:104
Coordinate transformation descriptor.
Eigen::Matrix< float, 4, 4, Eigen::DontAlign > trans
static bool addInverse(FiffCoordTrans &t)
#define FIFF_COORD_TRANS
Definition: fiff_file.h:475
#define FIFFV_MNE_COORD_RAS
QSharedPointer< FiffStream > SPtr
Definition: fiff_stream.h:107
#define FIFFV_MNE_COORD_CTF_HEAD
#define FIFFV_MNE_COORD_MNI_TAL
FiffTag class declaration, which provides fiff tag I/O and processing methods.
#define FIFFV_MNE_COORD_FS_TAL_GTZ
FiffCoordTransOld class declaration.