MNE-CPP  beta 1.0
rtsssalgo_test.cpp
Go to the documentation of this file.
1 //=============================================================================================================
36 #include "rtsssalgo_test.h"
37 #include "rtsssalgo.h"
38 //#include "FormFiles/rtssssetupwidget.h"
39 
40 RtSssAlgoTest::RtSssAlgoTest()
41 {
42 
43 }
44 
45 RtSssAlgoTest::~RtSssAlgoTest()
46 {
47 
48 }
49 
50 QList<MatrixXd> RtSssAlgoTest::buildLinearEqn()
51 {
52  QList<MatrixXd> Eqn, EqnRR;
53  QList<MatrixXd> LinEqn;
54  int LInRR, LOutRR, LIn, LOut;
55 // MatrixXd EqnInRR, EqnOutRR, EqnIn, EqnOut;
56 
57  int MagScale;
58  int MACHINE_TYPE = 2;
59 
60  if (MACHINE_TYPE == 1)
61  {
62  MagScale = 100;
63  std::cout << "loading coil information (VectorView).....";
64  getCoilInfoVectorView4Sim();
65  std::cout << "loading coil information (VectorView)..... ** finished **" << endl;
66  }
67  else if (MACHINE_TYPE == 2)
68  {
69  MagScale = 1;
70  std::cout << "loading coil information (BabyMEG).....";
71  getCoilInfoBabyMEG4Sim();
72  std::cout << "loading coil information (BabyMEG)..... ** finisehd **" << endl;
73  }
74 
75  LInRR = 5;
76  LOutRR = 4;
77  LIn = 8;
78  LOut = 4;
79 
80 // Compute SSS equation
81  EqnRR = getSSSEqn(LInRR, LOutRR);
82  EqnInRR = EqnRR[0];
83  EqnOutRR = EqnRR[1];
84 
85  Eqn = getSSSEqn(LIn, LOut);
86  EqnIn = Eqn[0];
87  EqnOut = Eqn[1];
88 
89 
90 // build linear equation
91  VectorXd CoilScale;
92 // MatrixXd EqnARR(NumCoil, EqnInRR.cols()+EqnOutRR.cols());
93 // MatrixXd EqnA(NumCoil, EqnIn.cols()+EqnOut.cols());
94 // MatrixXd EqnB(NumCoil,1);
95  EqnARR.resize(NumCoil, EqnInRR.cols()+EqnOutRR.cols());
96  EqnA.resize(NumCoil, EqnIn.cols()+EqnOut.cols());
97  EqnB.resize(NumCoil,1);
98 
99  CoilScale.setOnes(NumCoil);
100  for(int i=0; i<NumCoil; i++)
101  if (CoilGrad(i) == 0) CoilScale(i) = MagScale;
102 
103  EqnARR << EqnInRR, EqnOutRR;
104  EqnA << EqnIn, EqnOut;
105 
106  EqnARR = CoilScale.asDiagonal() * EqnARR;
107  EqnA = CoilScale.asDiagonal() * EqnA;
108  EqnB = CoilScale.asDiagonal() * MEGData;
109 
110 // LinEqn.append(EqnInRR);
111 // LinEqn.append(EqnOutRR);
112  LinEqn.append(EqnIn);
113  LinEqn.append(EqnOut);
114  LinEqn.append(EqnARR);
115  LinEqn.append(EqnA);
116  LinEqn.append(EqnB);
117 
118 // std::cout << "EqnInRR *********************************" << endl << EqnInRR << endl << endl;
119 // std::cout << "EqnOutRR ********************************" << endl << EqnOutRR << endl << endl;
120 // std::cout << "EqnIn ***********************************" << endl << EqnIn << endl << endl;
121 // std::cout << "EqnOut **********************************" << endl << EqnOut << endl << endl;
122 // std::cout << "EqnARR **********************************" << endl << EqnARR << endl << endl;
123 // std::cout << "EqnA ************************************" << endl << EqnA << endl << endl;
124 // std::cout << "EqnB ************************************" << endl << EqnB.transpose() << endl;
125 
126  std::cout << "building SSS linear equation .....finished !" << endl;
127 
128  return LinEqn;
129 }
130 
131 // Read FIFF coil configuration and assign them to the Coil class variables
132 // such as CoilName,CoilT,CoilTk, CoilNk, CoilRk, CoilWk, CoilGrad.
133 void RtSssAlgoTest::getCoilInfoVectorView4Sim()
134 {
135  QString str;
136 
137  NumCoil = 306;
138  cout << "Num of Coil = " << NumCoil << endl;
139 
140 // simulation data from test1_sim.mat
141  Origin.resize(3);
142  Origin << 0.0, 0.0, 0.04;
143 
144  std::cout << "loading MEGIn ....";
145  ifstream inMEGIn("/autofs/cluster/fusion/slew/MEG_realtime/mne-matlab-master/matlab/DATA_test_sim/VectorView/MEGIn.txt",ios::in);
146  MEGIn.resize(NumCoil);
147  for (int k=0; k<NumCoil; k++) inMEGIn >> MEGIn(k);
148  inMEGIn.close();
149 // std::cout << MEGIn << endl;
150  std::cout << " finished !" << endl;
151 
152  std::cout << "loading MEGOut ....";
153  ifstream inMEGOut("/autofs/cluster/fusion/slew/MEG_realtime/mne-matlab-master/matlab/DATA_test_sim/VectorView/MEGOut.txt",ios::in);
154  MEGOut.resize(NumCoil);
155  for(int k=0; k<NumCoil; k++) inMEGOut >> MEGOut(k);
156  inMEGOut.close();
157 // std::cout << MEGOut << endl;
158  std::cout << " finished !" << endl;
159 
160  std::cout << "loading MEGNoise ....";
161  ifstream inMEGNoise("/autofs/cluster/fusion/slew/MEG_realtime/mne-matlab-master/matlab/DATA_test_sim/VectorView/MEGNoise.txt",ios::in);
162  MEGNoise.resize(NumCoil);
163  for(int k=0; k<NumCoil; k++) inMEGNoise >> MEGNoise(k);
164  inMEGNoise.close();
165 // std::cout << MEGNoise << endl;
166  std::cout << " finished !" << endl;
167 
168  std::cout << "loading MEGData ....";
169  ifstream inMEGData("/autofs/cluster/fusion/slew/MEG_realtime/mne-matlab-master/matlab/DATA_test_sim/VectorView/MEGData.txt",ios::in);
170  MEGData.resize(NumCoil);
171  for(int k=0; k<NumCoil; k++) inMEGData >> MEGData(k);
172  inMEGData.close();
173 // std::cout << MEGData << endl;
174  std::cout << " finished !" << endl;
175 
176  // name per coil
177  for (int i=0; i<NumCoil; i++)
178  {
179  if (i%3 == 0)
180  CoilName.append("VV_PLANAR_T1");
181  else if (i%3 ==1)
182  CoilName.append("VV_PLANAR_T1");
183  else
184  CoilName.append("VV_MAG_T1");
185  }
186 // std:cout << CoilName[0] << ", " << CoilName[1] << ", " << CoilName[2] << endl;
187 
188  // transformation matrix per coil
189  std::cout << "loading coil transformation matrix ....";
190  ifstream inCT("/autofs/cluster/fusion/slew/MEG_realtime/mne-matlab-master/matlab/DATA_coil_def/VectorView/CoilT.txt",ios::in);
191  MatrixXd tmat(4,4);
192  for(int k=0; k<NumCoil; k++)
193  {
194  for(int i=0; i<4; i++)
195  for(int j=0; j<4; j++)
196  {
197  inCT >> tmat(i,j);
198  }
199  CoilT.append(tmat);
200  }
201  inCT.close();
202 // std::cout << CoilT[305] << endl;
203  std::cout << " finished !" << endl;
204 
205  // coil type - name
206  CoilTk.append("VV_PLANAR_T1");
207  CoilTk.append("VV_MAG_T1");
208 // std::cout << CoilTk[0] << ", " << CoilTk[1] << endl;
209 
210 
211  // coil type - Number of integration points for each coil type
212  CoilNk.resize(2);
213  CoilNk << 8, 9;
214 
215  // coil type - Coordinate of integrating points
216  MatrixXd Cintpts8(3,8), Cintpts9(3,9);
217  Cintpts8 << 0.0059, -0.0059, 0.0059, -0.0059, 0.0108, -0.0108, 0.0108, -0.0108, 0.0067, 0.0067, -0.0067, -0.0067, 0.0067, 0.0067, -0.0067, -0.0067, 0.0003, 0.0003, 0.0003, 0.0003, 0.0003, 0.0003, 0.0003, 0.0003;
218  Cintpts9 << 0.0000, 0.0100, -0.0100, 0.0100, -0.0100, 0.0000, 0.0000, 0.0100, -0.0100, 0.0000, 0.0100, 0.0100, -0.0100, -0.0100, 0.0100, -0.0100, 0.0000, 0.0000, 0.0003, 0.0003, 0.0003, 0.0003, 0.0003, 0.0003, 0.0003, 0.0003, 0.0003;
219  CoilRk.append(Cintpts8);
220  CoilRk.append(Cintpts9);
221 
222  // Coil type - Coil point weight
223  MatrixXd Wintpts8(1,8), Wintpts9(1,9);
224  Wintpts8 << 14.9790, -14.9790, 14.9790, -14.9790, 14.9790, -14.9790, 14.9790, -14.9790;
225  Wintpts9 << 0.1975, 0.0772, 0.0772, 0.0772, 0.0772, 0.1235, 0.1235, 0.1235, 0.1235;
226  CoilWk.append(Wintpts8);
227  CoilWk.append(Wintpts9);
228 
229  // Gradiometer ?
230  CoilGrad.resize(NumCoil);
231  CoilGrad << 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0;
232 // std::cout << CoilGrad << endl;
233 }
234 
235 // Read FIFF coil configuration and assign them to the Coil class variables
236 // such as CoilName,CoilT,CoilTk, CoilNk, CoilRk, CoilWk, CoilGrad.
237 void RtSssAlgoTest::getCoilInfoBabyMEG4Sim()
238 {
239  int NumCoilIn = 270;
240  int NumCoilOut = 105;
241 
242  NumCoil = NumCoilIn + NumCoilOut;
243  cout << "Num of Coil = " << NumCoil << endl;
244 
245 // simulation data from test1_sim.mat
246  Origin.resize(3);
247  Origin << 0.0, 0.0, 0.04;
248 
249  std::cout << "loading MEGIn ....";
250  ifstream inMEGIn("/autofs/cluster/fusion/slew/MEG_realtime/mne-matlab-master/matlab/DATA_test_sim/BabyMEG/MEGIn2.txt",ios::in);
251  MEGIn.resize(NumCoil);
252  for (int k=0; k<NumCoil; k++) inMEGIn >> MEGIn(k);
253  inMEGIn.close();
254 // std::cout << MEGIn << endl;
255  std::cout << " finished !" << endl;
256 
257  std::cout << "loading MEGOut ....";
258  ifstream inMEGOut("/autofs/cluster/fusion/slew/MEG_realtime/mne-matlab-master/matlab/DATA_test_sim/BabyMEG/MEGOut2.txt",ios::in);
259  MEGOut.resize(NumCoil);
260  for(int k=0; k<NumCoil; k++) inMEGOut >> MEGOut(k);
261  inMEGOut.close();
262 // std::cout << MEGOut << endl;
263  std::cout << " finished !" << endl;
264 
265  std::cout << "loading MEGNoise ....";
266  ifstream inMEGNoise("/autofs/cluster/fusion/slew/MEG_realtime/mne-matlab-master/matlab/DATA_test_sim/BabyMEG/MEGNoise.txt",ios::in);
267  MEGNoise.resize(NumCoil);
268  for(int k=0; k<NumCoil; k++) inMEGNoise >> MEGNoise(k);
269  inMEGNoise.close();
270 // std::cout << MEGNoise << endl;
271  std::cout << " finished !" << endl;
272 
273  std::cout << "loading MEGData ....";
274  ifstream inMEGData("/autofs/cluster/fusion/slew/MEG_realtime/mne-matlab-master/matlab/DATA_test_sim/BabyMEG/MEGData2.txt",ios::in);
275  MEGData.resize(NumCoil);
276  for(int k=0; k<NumCoil; k++) inMEGData >> MEGData(k);
277  inMEGData.close();
278 // std::cout << MEGData << endl;
279  std::cout << " finished !" << endl;
280 
281  // name per coil
282  for (int i=0; i<NumCoilIn; i++)
283  CoilName.append("BSQ2_In_Layer");
284  for (int i=0; i<NumCoilOut; i++)
285  CoilName.append("BSQ2_Out_Layer");
286 
287 // std:cout << CoilName[0] << ", " << CoilName[1] << ", " << CoilName[2] << endl;
288 
289  // transformation matrix per coil
290  std::cout << "loading coil transformation matrix ....";
291  ifstream inCT("/autofs/cluster/fusion/slew/MEG_realtime/mne-matlab-master/matlab/DATA_coil_def/BabyMEG/CoilT.txt",ios::in);
292  MatrixXd tmat(4,4);
293  for(int k=0; k<NumCoil; k++)
294  {
295  for(int i=0; i<4; i++)
296  for(int j=0; j<4; j++)
297  {
298  inCT >> tmat(i,j);
299  }
300  CoilT.append(tmat);
301  }
302  inCT.close();
303 // std::cout << CoilT[305] << endl;
304  std::cout << " finished !" << endl;
305 
306 
307  // coil type - name
308  CoilTk.append("BSQ2_In_Layer");
309  CoilTk.append("BSQ2_Out_Layer");
310 // std::cout << CoilTk[0] << ", " << CoilTk[1] << endl;
311 
312 
313  // coil type - Number of integration points for each coil type
314  CoilNk.resize(2);
315  CoilNk << 7, 7;
316 
317  // coil type - Coordinate of integrating points
318  MatrixXd CintptsInlayer(3,7), CintptsOutlayer(3,7);
319  CintptsInlayer << 0.000000, 0.000000, 0.000000, 0.0040825, 0.000000, 0.000000, -0.0040825, 0.000000, 0.000000, 0.0020412, 0.0035356, 0.000000, 0.0020412, -0.0035356, 0.000000, -0.0020412, 0.0035356, 0.000000, -0.0020412, -0.0035356, 0.000000;
320  CintptsOutlayer = CintptsInlayer;
321  CoilRk.append(CintptsInlayer);
322  CoilRk.append(CintptsOutlayer);
323 
324  // Coil type - Coil point weight
325  MatrixXd WintptsInlayer(1,7), WintptsOutlayer(1,7);
326  WintptsInlayer << 0.25, 0.125, 0.125, 0.125, 0.125, 0.125, 0.125;
327  WintptsOutlayer = WintptsInlayer;
328  CoilWk.append(WintptsInlayer);
329  CoilWk.append(WintptsOutlayer);
330 
331  // BabyMEG consists of magnetometers only
332 // CoilGrad.resize(NumCoil);
333  CoilGrad.setZero(NumCoil);
334 // std::cout << CoilGrad << endl;
335 }
336 
337 
338 
339 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
340 //% build linear equation for SSS
341 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
342 //% Origin: origin of device coordinate system [X; Y; Z];
343 //% LIn: expansion order for internal basis functions
344 //% LOut: expansion order for external basis functions
345 //% CoilTk{i,1}: coil type - name
346 //% CoilNk(i,1): coil type - number of samples
347 //% CoilWk{i,1}: coil type - weight of samples
348 //% CoilRk{i,1}: coil type - location of samples
349 //% i: i-th type
350 //% CoilName{i,1}: name per coil
351 //% CoilT{i,1}: transformation matrix per coil
352 //% i: i-th coil
353 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
354 //% EqnIn(i,1): internal basis functions
355 //% EqnOut(i,1): external basis functions
356 //% i: i-th coil
357 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
358 // function [EqnIn,EqnOut] = get_SSS_Eqn(Origin,LIn,LOut,CoilTk,CoilNk,CoilWk,CoilRk,CoilName,CoilT)
359 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
360 QList<MatrixXd> RtSssAlgoTest::getSSSEqn(int LIn, int LOut)
361 {
362  int NumBIn, NumBOut;
363  int coil_index=1;
364  double RScale;
365  VectorXd coil_distance, coil_clocation(4,1), coil_vector(4,1);
366  MatrixXd coil_location;
367  MatrixXd EqnIn, EqnOut;
368  QList<MatrixXd> Eqn;
369 
370 // % initialization
371  NumBIn = (LIn*LIn) + 2*LIn;
372  NumBOut = (LOut*LOut) + 2*LOut;
373 
374 //% calculate scaling factor for distance
375  coil_distance.resize(NumCoil);
376 
377  for(int i = 0; i<NumCoil; i++)
378  {
379  coil_clocation = CoilT[i].block(0,3,3,1);
380  coil_distance(i) = (coil_clocation-Origin).norm();
381  }
382 
383  RScale = exp(coil_distance.array().log().mean());
384 
385 //% build linear equation for internal/external basis functions
386  EqnIn.setZero(NumCoil,NumBIn);
387  EqnOut.setZero(NumCoil,NumBOut);
388 
389  for(int i = 0; i<NumCoil; i++)
390  {
391  if (CoilName[i] == CoilTk[0])
392  coil_index = 0;
393  else if (CoilName[i] == CoilTk[1])
394  coil_index = 1;
395  else
396  {
397  cout<< "There is no matching coil" << endl;
398  exit(1);
399  }
400 
401 // % calculate coil orientation
402  coil_vector = CoilT[i].block(0,2,3,1);
403 
404 // % calculate coil locations (multiple points)
405  int NumCoilPts = CoilNk(coil_index);
406  MatrixXd tmpmat; tmpmat.setOnes(4,NumCoilPts);
407 
408  tmpmat.topRows(3) = CoilRk[coil_index];
409  coil_location = CoilT[i] * tmpmat;
410  tmpmat = coil_location.topRows(3); coil_location.resize(3,NumCoilPts); coil_location = tmpmat;
411  coil_location = coil_location - Origin.replicate(1,NumCoilPts);
412  coil_location = coil_location / RScale;
413 
414 // % build linear equation
415  getSSSBasis(coil_location.row(0).transpose(), coil_location.row(1).transpose(), coil_location.row(2).transpose(), LIn, LOut);
416 // std::cout << "NumCoilPts: " << NumCoilPts << ", NumBIn: " << NumBIn << ", NumBOut: " << NumBOut << endl;
417 // std::cout << "BInX: " << BInX.rows() << " x " << BInX.cols() << endl;
418 
419  MatrixXd b_in, b_out;
420  b_in = coil_vector(0)*BInX + coil_vector(1)*BInY + coil_vector(2)*BInZ;
421  b_out = coil_vector(0)*BOutX + coil_vector(1)*BOutY + coil_vector(2)*BOutZ;
422 // std::cout << "b_in: Coil= " << i+1 << endl << b_in << endl;
423 // std::cout << "b_out: Coil= " << i+1 << endl << b_out << endl;
424 
425  EqnIn.block(i,0,1,NumBIn) = CoilWk[coil_index] * b_in;
426  EqnOut.block(i,0,1,NumBOut) = CoilWk[coil_index] * b_out;
427  }
428 
429  Eqn.append(EqnIn);
430  Eqn.append(EqnOut);
431 // std::cout << "EqnIn: Coil 1 " << endl << Eqn[0].row(0) << endl;
432 // std::cout << "EqnIn: Coil 306 " << endl << Eqn[0].row(305) << endl;
433 // std::cout << "EqnOut: Coil 1 " << endl << Eqn[1].row(0) << endl;
434 // std::cout << "EqnOut: Coil 306 " << endl << Eqn[1].row(305) << endl;
435 
436  return Eqn;
437 }
438 
439 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
440 //% generate SSS basis vectors at given locations
441 //% -- real basis function
442 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
443 //% X/Y/Z(i,1): Cartesian coordinate
444 //% i: i-th point
445 //% LIn: expansion order for internal basis functions
446 //% LOut: expansion order for external basis functions
447 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
448 //% BInX/Y/Z(i,j): internal basis vectors
449 //% BOutX/Y/Z(i,j): external basis vectors
450 //% i: i-th point
451 //% j: j-th basis function
452 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
453 // function [BInX,BInY,BInZ,BOutX,BOutY,BOutZ] = get_SSS_Basis(X,Y,Z,LIn,LOut)
454 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
455 void RtSssAlgoTest::getSSSBasis(VectorXd X, VectorXd Y, VectorXd Z, int LIn, int LOut)
456 {
457  int NumSample, NumBIn, NumBOut;
458  int LMax;
459 
460  QList<MatrixXd> P, dP_dx;
461  MatrixXd cur_p, cur_p1, cur_p2;
462 
463  QList<MatrixXcd> YP, dY_dTHETA;
464  VectorXcd cur_phi;
465 
466  MatrixXd BInR, BInPHI, BInTHETA;
467  int basis_index;
468  VectorXd scale_r;
469  VectorXcd cur_R, cur_PHI, cur_THETA;
470  MatrixXd BOutR, BOutPHI, BOutTHETA;
471 
472 // % check input parameters
473 // % -- R ~= 0 & THETA ~= 0
474  for(int i=0; i<X.rows(); i++)
475  if ( (abs(X(i)) < 1e-30) && (abs(Y(i)) < 1e-30) )
476  {
477  std::cout << "Zero THETA detected!**** ";
478  std::cout << "X, Y: " << X(i) << ", " << Y(i) << endl;
479  exit(1);
480  }
481 
482 // % initialization
483  LMax = qMax(LIn,LOut);
484  NumSample = X.size();
485  NumBIn = (LIn*LIn) + 2*LIn;
486  NumBOut = (LOut*LOut) + 2*LOut;
487 
488 // % Coordinate transdorm: (X,Y,Z) --> (R, PHI, THETA)
489  getCartesianToSpherCoordinate(X,Y,Z);
490 // std::cout << "X, Y, Z: " << endl << X.transpose() << endl << Y.transpose() << endl << Z.transpose() << endl;
491 // std::cout << "R, PHI, THETA: " << endl << R.transpose() << endl << PHI.transpose() << endl << THETA.transpose() << endl;
492 
493 // % calculate P
494  for(int l=1; l<=LMax; l++)
495  {
496  cur_p = legendre(l, THETA.array().cos());
497  P.append(cur_p.transpose());
498  }
499 // std::cout << "Legendre Polynomial 1-----" << endl << P[0].transpose() << endl;
500 // std::cout << "Legendre Polynomial 2-----" << endl << P[1].transpose() << endl;
501 // std::cout << "Legendre Polynomial 3-----" << endl << P[2].transpose() << endl;
502 // std::cout << "Legendre Polynomial 4-----" << endl << P[3].transpose() << endl;
503 // std::cout << "Legendre Polynomial 5-----" << endl << P[4].transpose() << endl;
504 
505 // % calculate dP_dx
506  for(int l=0; l<LMax; l++)
507  {
508  dP_dx.append(MatrixXd::Zero(NumSample,(l+1)+1));
509  for(int m=0; m <= l+1; m++)
510  {
511  cur_p1 = P[l].col(m);
512 // std::cout << "cur_p1. m= " << m << endl << cur_p1.transpose() << endl;
513  if (m == 0)
514  {
515  cur_p2 = -factorial((l+1)-1)/factorial((l+1)+1) * P[l].col(1);
516  }
517  else
518  {
519  cur_p2 = P[l].col(m-1);
520  }
521 // std::cout << "cur_p2. m=." << m << endl << cur_p2.transpose() << endl;
522 
523  // std::cout << "THETA: " << THETA.rows() << " x " << THETA.cols() << endl;
524  // std::cout << "cur_p1: " << cur_p1.rows() << " x " << cur_p1.cols() << endl;
525  // std::cout << "P[0]: " << P[0].col(1) << endl;
526 // std::cout << "dp_dx[" << l << "]: " << endl << dP_dx[l] << endl;
527  dP_dx[l].col(m) = (m*THETA.array().cos() * cur_p1.array() + (((l+1)+m)*((l+1)-m+1)*THETA.array().sin()) * cur_p2.array()) / THETA.array().sin().pow(2);
528  }
529 
530 // std::cout << "dP_dx[" << l << "]" <<endl << dP_dx[l] << endl;
531  }
532 
533 // % calculate Y & dY_dTHETA
534  for(int l=0; l<LMax; l++)
535  {
536  YP.append(MatrixXcd::Zero(NumSample,(l+1)+1));
537  dY_dTHETA.append(MatrixXcd::Zero(NumSample,(l+1)+1));
538 
539  for(int m=0; m <=l+1; m++)
540  {
541 // % cur_phi = sqrt((2*l+1)*factorial(l-m)/factorial(l+m)/2/pi) * exp(sqrt(-1)*m*PHI);
542 // % Y{l,1}(:,m+1) = cur_phi .* P{l,1}(:,m+1);
543 // % dY_dTHETA{l,1}(:,m+1) = -cur_phi .* sin(THETA) .* dP_dx{l,1}(:,m+1);
544  cur_phi = sqrt((2*(l+1)+1)*factorial((l+1)-m)/factorial((l+1)+m)/2/M_PI) * (sqrt(cplxd(-1,0)) * cplxd(m,0) * PHI.cast<cplxd >()).array().exp();
545 // std::cout << "cur_phi. m=." << m << endl << cur_phi.transpose() << endl;
546 
547  YP[l].col(m) = cur_phi.array() * P[l].col(m).array().cast<cplxd >();
548  dY_dTHETA[l].col(m) = -cur_phi.array() * THETA.array().sin().cast<cplxd >() * dP_dx[l].col(m).array().cast<cplxd >();
549  }
550 // std::cout << "YP[" << l << "]" <<endl << YP[l] << endl;
551 // std::cout << "dY_dTHETA[" << l << "]" <<endl << dY_dTHETA[l] << endl;
552  }
553 
554 // % calculate BIn (spherical coordinate)
555  BInR.setZero(NumSample,NumBIn);
556  BInPHI.setZero(NumSample,NumBIn);
557  BInTHETA.setZero(NumSample,NumBIn);
558  basis_index = 0;
559  for(int l=0; l<LIn; l++)
560  {
561  scale_r= R.array().pow((l+1)+2);
562 
563  BInR.col(basis_index) = -((l+1)+1) * YP[l].col(0).array().real() / scale_r.array();
564  BInPHI.col(basis_index) = VectorXd::Zero(NumSample);
565  BInTHETA.col(basis_index) = dY_dTHETA[l].col(0).array().real() / scale_r.array();
566  basis_index = basis_index + 1;
567 
568 // std::cout << "BInR" << endl << BInR << endl;
569 
570  for(int m=0; m<=l; m++)
571  {
572  cur_R = -sqrt(2) * ((l+1)+1) * YP[l].col(m+1).array() / scale_r.array().cast<cplxd >();
573  cur_PHI = sqrt(cplxd(-2,0)) * cplxd(m+1,0) * YP[l].col(m+1).array() / THETA.array().sin().cast<cplxd >() / scale_r.array().cast<cplxd >();
574  cur_THETA = sqrt(2) * dY_dTHETA[l].col(m+1).array() / scale_r.array().cast<cplxd >();
575 // std::cout << "cur_R, m=" << m << endl << cur_R.transpose() << endl;
576 
577  BInR.col(basis_index) = cur_R.real();
578  BInPHI.col(basis_index) = cur_PHI.real();
579  BInTHETA.col(basis_index) = cur_THETA.real();
580  basis_index = basis_index + 1;
581 
582  BInR.col(basis_index) = cur_R.imag();
583  BInPHI.col(basis_index) = cur_PHI.imag();
584  BInTHETA.col(basis_index) = cur_THETA.imag();
585  basis_index = basis_index + 1;
586  }
587  }
588 // std::cout << "BInR" << endl << BInR << endl;
589 // std::cout << "BInPHI" << endl << BInPHI << endl;
590 // std::cout << "BInTHETA" << endl << BInTHETA << endl;
591 
592 // % calculate BOut (spherical coordinate)
593  BOutR.setZero(NumSample,NumBOut);
594  BOutPHI.setZero(NumSample,NumBOut);
595  BOutTHETA.setZero(NumSample,NumBOut);
596  basis_index = 0;
597  for(int l=0; l<LOut; l++)
598  {
599  scale_r= R.array().pow((l+1)-1);
600 
601  BOutR.col(basis_index) = (l+1) * YP[l].col(0).array().real() * scale_r.array();
602  BOutPHI.col(basis_index) = VectorXd::Zero(NumSample);
603  BOutTHETA.col(basis_index) = dY_dTHETA[l].col(0).array().real() * scale_r.array();
604  basis_index = basis_index + 1;
605 
606  for(int m=0; m<=l; m++)
607  {
608  cur_R = sqrt(2) * (l+1) * YP[l].col(m+1).array() * scale_r.array().cast<cplxd >();
609  cur_PHI = sqrt(cplxd(-2,0)) * cplxd(m+1,0) * YP[l].col(m+1).array() / THETA.array().sin().cast<cplxd >() * scale_r.array().cast<cplxd >();
610  cur_THETA = sqrt(2) * dY_dTHETA[l].col(m+1).array() * scale_r.array().cast<cplxd >();
611  BOutR.col(basis_index) = cur_R.real();
612  BOutPHI.col(basis_index) = cur_PHI.real();
613  BOutTHETA.col(basis_index) = cur_THETA.real();
614  basis_index = basis_index + 1;
615  BOutR.col(basis_index) = cur_R.imag();
616  BOutPHI.col(basis_index) = cur_PHI.imag();
617  BOutTHETA.col(basis_index) = cur_THETA.imag();
618  basis_index = basis_index + 1;
619  }
620  }
621 // std::cout << "BOutR" << endl << BOutR << endl;
622 // std::cout << "BOutPHI" << endl << BOutPHI << endl;
623 // std::cout << "BOutTHETA" << endl << BOutTHETA << endl;
624 
625 // % convert from spherical coordinate to Cartesian coordinate
626 // [R_X,R_Y,R_Z,PHI_X,PHI_Y,PHI_Z,THETA_X,THETA_Y,THETA_Z] = get_Sph_2_Cart_Vector(R,PHI,THETA);
627  getSphereToCartesianVector();
628  BInX = BInR.array() * (R_X.replicate(1,NumBIn)).array() + BInPHI.array() * (PHI_X.replicate(1,NumBIn)).array() + BInTHETA.array() * (THETA_X.replicate(1,NumBIn)).array();
629  BInY = BInR.array() * (R_Y.replicate(1,NumBIn)).array() + BInPHI.array() * (PHI_Y.replicate(1,NumBIn)).array() + BInTHETA.array() * (THETA_Y.replicate(1,NumBIn)).array();
630  BInZ = BInR.array() * (R_Z.replicate(1,NumBIn)).array() + BInPHI.array() * (PHI_Z.replicate(1,NumBIn)).array() + BInTHETA.array() * (THETA_Z.replicate(1,NumBIn)).array();
631  BOutX = BOutR.array() * (R_X.replicate(1,NumBOut)).array() + BOutPHI.array() * (PHI_X.replicate(1,NumBOut)).array() + BOutTHETA.array() * (THETA_X.replicate(1,NumBOut)).array();
632  BOutY = BOutR.array() * (R_Y.replicate(1,NumBOut)).array() + BOutPHI.array() * (PHI_Y.replicate(1,NumBOut)).array() + BOutTHETA.array() * (THETA_Y.replicate(1,NumBOut)).array();
633  BOutZ = BOutR.array() * (R_Z.replicate(1,NumBOut)).array() + BOutPHI.array() * (PHI_Z.replicate(1,NumBOut)).array() + BOutTHETA.array() * (THETA_Z.replicate(1,NumBOut)).array();
634 
635 // std::cout << "BInX" << endl << BInX << endl;
636 // std::cout << "BInY" << endl << BInY << endl;
637 // std::cout << "BInZ" << endl << BInZ << endl;
638 // std::cout << "BOutX" << endl << BOutX << endl;
639 // std::cout << "BOutY" << endl << BOutY << endl;
640 // std::cout << "BOutZ" << endl << BOutZ << endl;
641 
642 }
643 
644 
645 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
646 //%
647 //% RtSssAlgoTest::getCartesianToSpherCeoordinate(double X, double Y, double Z
648 //%
649 //% convert point coordinate from Cartesian to spherical
650 //%
651 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
652 //% X/Y/Z(i,1): Cartesian coordinate
653 //% i: i-th point
654 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
655 //% R/PHI/THETA(i,1): spherical coordinate
656 //% i: i-th point
657 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
658 // function [R,PHI,THETA] = get_Cart_2_Sph_Coordinate(X,Y,Z)
659 // hypotxy = hypot(X,Y);
660 // R = hypot(hypotxy,Z);
661 // PHI = atan2(Y,X);
662 // THETA = atan2(hypotxy,Z);
663 // return;
664 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
665 void RtSssAlgoTest::getCartesianToSpherCoordinate(VectorXd X, VectorXd Y, VectorXd Z)
666 {
667  VectorXd hypotxy;
668 // std::cout << "X: " << X.rows() << " x " << X.cols() << endl;
669 // std::cout << "Y: " << Y.rows() << " x " << Y.cols() << endl;
670 
671  hypotxy = hypot(X,Y);
672  R = hypot(hypotxy,Z);
673  PHI = atan2vec(Y,X);
674  THETA = atan2vec(hypotxy,Z);
675 }
676 
677 
678 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
679 //%
680 //% RtSssAlgoTest::getSphereToCartesianVector(double R, double PHI, double THETA)
681 //%
682 //% convert unit vector from spherical to Cartesian
683 //%
684 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
685 //% R/PHI/THETA(i,1): spherical coordinate
686 //% i: i-th point
687 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
688 //% R_X/R_Y/R_Z(i,1): unit vector R in Cartesian coordinate
689 //% PHI_X/PHI_Y/PHI_Z(i,1): unit vector PHI in Cartesian coordinate
690 //% THETA_X/THETA_Y/THETA_Z(i,1): unit vector THETA in Cartesian coordinate
691 //% i: i-th point
692 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
693 // function [R_X,R_Y,R_Z,PHI_X,PHI_Y,PHI_Z,THETA_X,THETA_Y,THETA_Z] = get_Sph_2_Cart_Vector(R,PHI,THETA)
694 // NumExp = size(R,1);
695 // R_X = sin(THETA) .* cos(PHI);
696 // R_Y = sin(THETA) .* sin(PHI);
697 // R_Z = cos(THETA);
698 // PHI_X = -sin(PHI);
699 // PHI_Y = cos(PHI);
700 // PHI_Z = zeros(NumExp,1);
701 // THETA_X = cos(THETA) .* cos(PHI);
702 // THETA_Y = cos(THETA) .* sin(PHI);
703 // THETA_Z = -sin(THETA);
704 // return;
705 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
706 void RtSssAlgoTest::getSphereToCartesianVector()
707 {
708  int NumExp = R.size();
709 
710  R_X = THETA.array().sin() * PHI.array().cos();
711  R_Y = THETA.array().sin() * PHI.array().sin();
712  R_Z = THETA.array().cos();
713 
714  PHI_X = -PHI.array().sin();
715  PHI_Y = PHI.array().cos();
716  PHI_Z = VectorXd::Zero(NumExp,1);
717 
718 
719  THETA_X = THETA.array().cos() * PHI.array().cos();
720  THETA_Y = THETA.array().cos() * PHI.array().sin();
721  THETA_Z = -THETA.array().sin();
722 }
723 
724 
725 
726 
727 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
728 //% SSS by robust regression (subspace iteration & low-rank update)
729 //% -- RR_K1 is determined by bi-square function (w = 0.75 at RR_K1)
730 //% -- RR_K2 is determined by bi-square function (95% statistical efficiency)
731 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
732 //% EqnIn(i,j): internal basis functions
733 //% EqnOut(i,j): external basis functions
734 //% i: i-th coil
735 //% j: j-th basis function
736 //% EqnARR(i,j): SSS equation (LHS) after scaling -- subspace
737 //% i: i-th coil
738 //% j: j-th basis function
739 //% EqnA(i,j): SSS equation (LHS) after scaling -- full
740 //% i: i-th coil
741 //% j: j-th basis function
742 //% EqnB(i,j): SSS equation (RHS) after scaling
743 //% i: i-th coil
744 //% j: j-th sample in time domain
745 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
746 //% SSSIn(i,j): internal MEG signal recovered by SSS
747 //% SSSOut(i,j): external MEG signal recovered by SSS
748 //% Weight(i,j): optimal weight for MEG coil
749 //% ErrRel(1,j): relative residual of over-determined linear equation
750 //% i: i-th coil
751 //% j: j-th sample in time domain
752 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
753 // function [SSSIn,SSSOut,Weight,ErrRel] = get_SSS_RR(EqnIn,EqnOut,EqnARR,EqnA,EqnB)
754 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
755 
756 QList<MatrixXd> RtSssAlgoTest::getSSSRR(MatrixXd EqnIn, MatrixXd EqnOut, MatrixXd EqnARR, MatrixXd EqnA, MatrixXd EqnB)
757 {
758  int NumBIn, NumBOut, NumCoil, NumExp;
759  MatrixXd EqnRRInv, EqnInv, SSSIn, SSSOut, Weight;
760  VectorXd ErrRel;
761  double RR_K1, RR_K2, RR_K3;
762  double eqn_scale0, eqn_scale;
763  MatrixXd sol_X, sol_X_old, eqn_Y, eqn_D, temp_M, temp_N, sol_in, sol_out;
764  MatrixXd diagMat;
765  VectorXd eqn_err, weight_index;
766  QList<MatrixXd> RRsss;
767 
768 // % error tolerance for robust regression
769  double ErrTolRel = 1e-3;
770 
771 // % weight threshold for robust regression
772  double WeightThres = 1 - 1e-6;
773 
774 // % initialization
775  NumBIn = EqnIn.cols();
776  NumBOut = EqnOut.cols();
777  NumCoil = EqnB.rows();
778  NumExp = EqnB.cols();
779 
780  //ooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo
781  // QFile t_fileRaw("/autofs/cluster/fusion/slew/GitHub/mne-cpp/bin/MNE-sample-data/MEG/sample/sample_audvis_raw.fif");
782  // FiffRawData raw(t_fileRaw);
783 
784  // QStringList include;
785  // include << "STI 014";
786  // bool want_meg = true;
787  // bool want_eeg = false;
788  // bool want_stim = false;
789 
790  // RowVectorXi picks = raw.info.pick_types(want_meg, want_eeg, want_stim, include, raw.info.bads);
791 
792  // float from = 42.956f;
793  // float to = 320.670f;
794  // bool in_samples = false;
795  // bool readSuccessful = false;
796  // MatrixXd data;
797  // MatrixXd times;
798  // if (in_samples)
799  // readSuccessful = raw.read_raw_segment(data, times, (qint32)from, (qint32)to, picks);
800  // else
801  // readSuccessful = raw.read_raw_segment_times(data, times, from, to, picks);
802 
803  // if (!readSuccessful)
804  // {
805  // printf("Could not read raw segment.\n");
806  // exit(1);
807  // }
808  //
812  //
813  // int start_sample = 0;
814  // int end_sample = 20;
815 
816  // EqnB = data.block(0, start_sample, NumCoil, end_sample-start_sample+1);
817  // NumCoil = EqnB.rows();
818  // NumExp = EqnB.cols();
819  // printf("%d samples, %d channels \n",(qint32)EqnB.cols(), (qint32)EqnB.rows());
820 
821  //cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
822 
823  EqnRRInv = (EqnARR.transpose() * EqnARR).inverse();
824  EqnInv = (EqnA.transpose() * EqnA).inverse();
825 
826  SSSIn.setZero(NumCoil,NumExp);
827  SSSOut.setZero(NumCoil,NumExp);
828  Weight.setZero(NumCoil,NumExp);
829  ErrRel.setZero(NumExp);
830  RR_K3 = 3;
831  RR_K2 = 4.685;
832  RR_K1 = qSqrt(1-qSqrt(3)/2) * RR_K2;
833 
834  for(int i=0; i<NumExp; i++)
835  {
836 // % solve OLS solution
837  sol_X = EqnRRInv * (EqnARR.transpose() * EqnB.col(i));
838 // std::cout << "sol_X ************************" << endl << sol_X.transpose() << endl;
839 
840 // % scale linear equation
841  eqn_err = EqnARR * sol_X - EqnB.col(i);
842  eqn_scale0 = stdev(eqn_err);
843  eqn_err = eqn_err.cwiseAbs() / eqn_scale0;
844 
845 // % solve iteratively re-weighted least squares (Bi-Square) -- subspace
846  sol_X_old.setConstant(sol_X.rows(), sol_X.cols(), 1e30);
847 // while (((sol_X-sol_X_old).norm() / sol_X.norm()) > ErrTolRel)
848  int cnt=0;
849  while (((sol_X.array()-sol_X_old.array()).matrix().norm() / sol_X.norm()) > ErrTolRel)
850 // for (int h=0; h<2; h++)
851  {
852  cnt++;
853  sol_X_old = sol_X;
854 // Weight(:,i) = (eqn_err <= RR_K1) + (eqn_err > RR_K1 & eqn_err <= RR_K2) .* (1-(eqn_err-RR_K1).^2/(RR_K2-RR_K1)^2).^2;
855 // Weight.col(i) = eigen_LTE(eqn_err, RR_K1) + eigen_AND(eigen_GT(eqn_err, RR_K1),eigen_LTE(eqn_err, RR_K2));
856  Weight.col(i) = eigen_LTE(eqn_err,RR_K1).array() + eigen_AND(eigen_GT(eqn_err,RR_K1),eigen_LTE(eqn_err,RR_K2)).array() * (1 - ((eqn_err.array()-RR_K1).pow(2)) / pow(RR_K2-RR_K1,2) ).pow(2);
857 // std::cout << "Weight **************************" << endl << Weight.transpose() << endl;
858 
859 // % weight_index = find(Weight(:,i) < WeightThres);
860  weight_index = eigen_LT_index(Weight.col(i), WeightThres);
861 // weight_index = eigen_LT_index_test(Weight.col(i), h);
862 // std:cout << "weight_index ********************* " << endl << weight_index << endl;
863 
864 // % eqn_Y = EqnARR(weight_index,:); eqn_D = Weight(weight_index,i) - 1;
865  eqn_Y.resize(weight_index.size(), EqnARR.cols());
866  eqn_D.resize(weight_index.size(), 1);
867  for(int k=0; k<weight_index.size(); k++)
868  {
869  eqn_Y.row(k) = EqnARR.row(weight_index(k));
870 // std::cout << "eqn_Y: " << eqn_Y.rows() << " x " << eqn_Y.cols() << endl;
871 // std::cout << "eqn_D: " << eqn_D.rows() << " x " << eqn_D.cols() << endl;
872  eqn_D(k) = Weight(weight_index(k),i) - 1;
873  }
874 // std::cout << "eqn_D: " << endl << eqn_D.transpose() << endl << endl;
875 
876  temp_M = EqnARR.transpose() * (Weight.col(i).array() * EqnB.col(i).array()).matrix();
877 // std::cout << "temp_M: " << endl << temp_M.transpose() << endl << endl;
878 
879 // std::cout << "EqnRRInv: " << EqnRRInv.rows() << " x " << EqnRRInv.cols() << endl;
880 // std::cout << "eqn_Y: " << eqn_Y.rows() << " x " << eqn_Y.cols() << endl;
881 // std::cout << "EqnRRInv: " << endl << EqnRRInv.transpose() << endl << endl;
882 // std::cout << "eqn_Y: " << endl << eqn_Y.transpose() << endl << endl;
883 
884  temp_N = EqnRRInv * eqn_Y.transpose();
885 // std::cout << "temp_N: " << endl << temp_N.transpose() << endl << endl;
886 
887 
888 // diagMat = eqn_D.asDiagonal();
889 // std::cout << "diagMat ************************" << endl << diagMat.transpose() << endl;
890 // diagMat = 1 / diagMat.array();
891 // std::cout << "diagMat ************************" << endl << diagMat.transpose() << endl;
892 
893  diagMat = (1 / eqn_D.array()).matrix().asDiagonal();
894 // std::cout << "diagMat ************************" << endl << diagMat.transpose() << endl;
895 
896  sol_X = EqnRRInv * temp_M - temp_N * (diagMat + eqn_Y * temp_N).inverse() * (temp_N.transpose() * temp_M);
897  eqn_err = (EqnARR * sol_X - EqnB.col(i)).cwiseAbs();
898  eqn_scale = qMin(eqn_scale0, RR_K3 * qSqrt((Weight.col(i).array() * eqn_err.array() * eqn_err.array()).mean()));
899  eqn_err = eqn_err / eqn_scale;
900 // std::cout << "temp_M: " << endl << temp_M.transpose() << endl << endl;
901 
902 // std::cout << "(sol_X.array()-sol_X_old.array()).matrix().norm(): " << (sol_X.array()-sol_X_old.array()).matrix().norm() << endl << endl;
903 // std::cout << "sol_X.norm(): " << sol_X.norm() << endl << endl;
904 // std::cout << "sol_X ************************" << endl << sol_X.transpose() << endl << endl;
905  }
906 
907 // % solve weighted SSS - full
908 // % eqn_Y = EqnA(weight_index,:); temp_M = EqnA' * (Weight(:,i).*EqnB(:,i)); temp_N = EqnInv * eqn_Y';
909  eqn_Y.resize(weight_index.size(), NumBIn+NumBOut);
910  for(int k=0; k<weight_index.size(); k++) eqn_Y.row(k) = EqnA.row(weight_index(k));
911  temp_M = EqnA.transpose() * (Weight.col(i).array() * EqnB.col(i).array()).matrix();
912  temp_N = EqnInv * eqn_Y.transpose();
913 
914 // % sol_X = EqnInv * temp_M - temp_N * ((diag(1./eqn_D) + eqn_Y * temp_N); // \ (temp_N'*temp_M));
915 // diagMat = eqn_D.asDiagonal();
916 // diagMat = 1 / diagMat.array();
917  diagMat = (1 / eqn_D.array()).matrix().asDiagonal();
918  sol_X = EqnInv * temp_M - temp_N * (diagMat + eqn_Y * temp_N).inverse() * (temp_N.transpose() * temp_M);
919 
920  ErrRel(i) = (EqnA * sol_X - EqnB.col(i)).norm() / EqnB.col(i).norm();
921  sol_in = sol_X.block(0,0,NumBIn,1);
922  sol_out = sol_X.block(NumBIn,0,NumBOut,1); // sol_X(NumBIn+1:NumBIn+NumBOut,1);
923 
924 // % recover internal/external MEG siganl
925  SSSIn.col(i) = EqnIn * sol_in;
926  SSSOut.col(i) = EqnOut * sol_out;
927 
928  }
929 
930  RRsss.append(SSSIn);
931  RRsss.append(SSSOut);
932  RRsss.append(Weight);
933  RRsss.append(ErrRel);
934 
935  return RRsss;
936 }
937 
938 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
939 //% SSS by OLS
940 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
941 //% EqnIn(i,j): internal basis functions
942 //% EqnOut(i,j): external basis functions
943 //% i: i-th coil
944 //% j: j-th basis function
945 //% EqnA(i,j): SSS equation (LHS) after scaling
946 //% i: i-th coil
947 //% j: j-th basis function
948 //% EqnB(i,j): SSS equation (RHS) after scaling
949 //% i: i-th coil
950 //% j: j-th sample in time domain
951 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
952 //% SSSIn(i,j): internal MEG signal recovered by SSS
953 //% SSSOut(i,j): external MEG signal recovered by SSS
954 //% ErrRel(1,j): relative residual of over-determined linear equation
955 //% i: i-th coil
956 //% j: j-th sample in time domain
957 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
958 //function [SSSIn,SSSOut,ErrRel] = get_SSS_OLS(EqnIn,EqnOut,EqnA,EqnB)
959 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
960 
961 QList<MatrixXd> RtSssAlgoTest::getSSSOLS(MatrixXd EqnIn, MatrixXd EqnOut, MatrixXd EqnA, MatrixXd EqnB)
962 {
963  int NumBIn, NumBOut, NumCoil, NumExp;
964  MatrixXd EqnRRInv, EqnInv, SSSIn, SSSOut;
965  MatrixXd sol_X, sol_in, sol_out;
966  VectorXd ErrRel;
967  QList<MatrixXd> OLSsss;
968 
969 // % initialization
970  NumBIn = EqnIn.cols();
971  NumBOut = EqnOut.cols();
972  NumCoil = EqnB.rows();
973  NumExp = EqnB.cols();
974 
975 /*
976  //ooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo
977  QFile t_fileRaw("/autofs/cluster/fusion/slew/GitHub/mne-cpp/bin/MNE-sample-data/MEG/sample/sample_audvis_raw.fif");
978  FiffRawData raw(t_fileRaw);
979 
980  QStringList include;
981  include << "STI 014";
982  bool want_meg = true;
983  bool want_eeg = false;
984  bool want_stim = false;
985 
986  RowVectorXi picks = raw.info.pick_types(want_meg, want_eeg, want_stim, include, raw.info.bads);
987 
988  float from = 42.956f;
989  float to = 320.670f;
990  bool in_samples = false;
991  bool readSuccessful = false;
992  MatrixXd data;
993  MatrixXd times;
994  if (in_samples)
995  readSuccessful = raw.read_raw_segment(data, times, (qint32)from, (qint32)to, picks);
996  else
997  readSuccessful = raw.read_raw_segment_times(data, times, from, to, picks);
998 
999  if (!readSuccessful)
1000  {
1001  printf("Could not read raw segment.\n");
1002  exit(1);
1003  }
1004 
1005  // printf("Read %d samples.\n",(qint32)data.cols());
1006  // printf("Read %d channels.\n",(qint32)data.rows());
1007  // std::cout << data.block(0,0,10,10) << std::endl;
1008 
1009 
1010  int start_sample = 0;
1011  int end_sample = 20;
1012 
1013  EqnB = data.block(0, start_sample, NumCoil, end_sample-start_sample+1);
1014  NumCoil = EqnB.rows();
1015  NumExp = EqnB.cols();
1016  printf("%d samples, %d channels \n",(qint32)EqnB.cols(), (qint32)EqnB.rows());
1017  */
1018  //cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
1019 
1020  EqnRRInv = (EqnA.transpose() * EqnA).inverse();
1021  SSSIn.setZero(NumCoil,NumExp);
1022  SSSOut.setZero(NumCoil,NumExp);
1023  ErrRel.setZero(NumExp);
1024 
1025  for (int i=0; i<NumExp; i++)
1026  {
1027 // % solve OLS solution
1028  sol_X = EqnRRInv * (EqnA.transpose() * EqnB.col(i));
1029  ErrRel(i) = (EqnA * sol_X - EqnB.col(i)).norm() / EqnB.col(i).norm();
1030  sol_in = sol_X.block(0,0,NumBIn,1);
1031  sol_out = sol_X.block(NumBIn,0, NumBOut,1); // probably NumBIn+0 is correct. Please debug !!!!
1032 
1033 // % recover internal/external MEG siganl
1034  SSSIn.col(i) = EqnIn * sol_in;
1035  SSSOut.col(i) = EqnOut * sol_out;
1036  }
1037  OLSsss.append(SSSIn);
1038  OLSsss.append(SSSOut);
1039  OLSsss.append(ErrRel);
1040 
1041  return OLSsss;
1042 }
1043 
1044 QList<MatrixXd> RtSssAlgoTest::getLinEqn()
1045 {
1046  QList<MatrixXd> LinEqn;
1047 
1048  LinEqn.append(EqnIn);
1049  LinEqn.append(EqnOut);
1050  LinEqn.append(EqnARR);
1051  LinEqn.append(EqnA);
1052  LinEqn.append(EqnB);
1053 
1054  return LinEqn;
1055 }
1056 
1058 //MatrixXd legendre(int l, VectorXd x)
1059 //{
1060 // MatrixXd outP;
1062 
1063 // outP.setZero(l+1, x.size());
1065 
1066 // for(int m=0; m<=l; m++)
1067 // {
1068 // for(int k=0; k<x.size(); k++)
1069 // outP(m,k) = plgndr(l,m,x(k));
1070 // }
1071 
1072 // return outP;
1073 //}
1074 
1075 
1076 //float plgndr(int l, int m, float x)
1077 //{
1079 // float fact,pll,pmm,pmmp1,somx2;
1080 // int i,ll;
1081 
1082 // if (m < 0 || m > l || fabs(x) > 1.0)
1083 // std::cout << "Bad arguments in routine plgndr";
1085 // pmm=1.0;
1086 // if (m > 0)
1087 // {
1088 // somx2=sqrt((1.0-x)*(1.0+x));
1089 // fact=1.0;
1090 // for (i=1;i<=m;i++)
1091 // {
1092 // pmm *= -fact*somx2;
1093 // fact += 2.0;
1094 // }
1095 // }
1096 // if (l == m)
1097 // return pmm;
1098 // else {
1099 // pmmp1=x*(2*m+1)*pmm;
1100 // if (l == (m+1))
1101 // return pmmp1;
1102 // else
1103 // {
1104 // for (ll=m+2;ll<=l;ll++)
1105 // {
1106 // pll=(x*(2*ll-1)*pmmp1-(ll+m-1)*pmm)/(ll-m);
1107 // pmm=pmmp1;
1108 // pmmp1=pll;
1109 // }
1110 // return pll;
1111 // }
1112 // }
1113 //}
1114 
1117 // Robust computation of the square root of the sum of squares.
1118 // C = hypot(A,B) returns SQRT(ABS(A).^2+ABS(B).^2) carefully computed to
1119 // avoid underflow and overflow.*/
1120 //VectorXd hypot(VectorXd X, VectorXd Y)
1121 //{
1122 // VectorXd rst;
1123 // rst = (X.array().abs().pow(2) + Y.array().abs().pow(2)).sqrt();
1124 // return rst;
1125 //}
1126 
1130 //VectorXd atan2vec(VectorXd a, VectorXd b)
1131 //{
1132 // int n = a.size();
1133 // VectorXd at2(n);
1134 
1135 // for(int i=0; i<n; i++)
1136 // {
1137 // at2(i) = qAtan2(a(i),b(i));
1138 // }
1139 
1140 // return at2;
1141 //}
1142 
1143 //VectorXd find(MatrixXd M, int I)
1144 //{
1145 // VectorXd outIDX;
1146 // int k=0;
1147 // for(int i=0; i<M.cols(); i++)
1148 // for(int j=0; j<M.rows(); j++)
1149 // if (M(i,j) == I)
1150 // {
1151 // outIDX(k) = i*M.rows() + j;
1152 // k++;
1153 // }
1154 // return outIDX;
1155 //}
1156 
1157 
1158 
1159 //double factorial(int n)
1160 //{
1161 // double fac=1;
1162 
1163 // for(int i=2; i<=n; i++) fac *= i;
1164 
1165 // return fac;
1166 //}
1167 
1168 
1171 //double stdev(VectorXd V)
1172 //{
1173 // int size = V.size();
1174 // double ave, sum=0;
1175 
1176 // ave = V.mean();
1177 // for(int i=0; i<size; i++)
1178 // {
1179 // sum += pow((V(i)-ave),2);
1180 // }
1181 // return sqrt(sum/size);
1182 //}
1183 
1187 //VectorXd eigen_LTE(VectorXd V, double tol)
1188 //{
1189 // int row = V.rows();
1190 // VectorXd outTrue = VectorXd::Zero(row);
1191 
1192 // for(int i=0; i<row; i++)
1193 // if(V(i) <= tol) outTrue(i) = 1;
1195 // return outTrue;
1196 //}
1197 
1201 //VectorXd eigen_GT(VectorXd V, double tol)
1202 //{
1203 // int row = V.rows();
1204 // VectorXd outTrue = VectorXd::Zero(row);
1205 
1206 // for(int i=0; i<row; i++)
1207 // if(V(i) > tol) outTrue(i) = 1;
1208 // return outTrue;
1209 //}
1210 
1214 //VectorXd eigen_AND(VectorXd V1, VectorXd V2)
1215 //{
1216 // int row = V1.rows();
1217 // VectorXd outTrue = VectorXd::Zero(row);
1218 
1219 // for(int i=0; i<row; i++)
1220 // if(V1(i) && V2(i)) outTrue(i) = 1;
1222 // return outTrue;
1223 //}
1224 
1227 //VectorXd eigen_LT_index(VectorXd V, double tol)
1228 //{
1229 // int cnt = 0;
1230 // int size = V.size();
1231 // VectorXd outIdx; outIdx.setZero(size);
1232 
1233 // for(int i=0; i<size; i++)
1234 // if(V(i) < tol)
1235 // {
1236 // outIdx(cnt) = i;
1237 // cnt++;
1238 // }
1239 // return outIdx.head(cnt);
1240 //}
1241 
1242 //VectorXd eigen_LT_index_test(VectorXd V, int cnt)
1243 //{
1244 // VectorXd outIdx = VectorXd::Zero(cnt+1);
1245 
1246 // for (int i=0; i<cnt+1; i++)
1247 // outIdx(i) = i+1;
1248 
1250 // return outIdx;
1251 //}
Contains the declaration of the RtSssAlgo class.