40 #include <Eigen/Dense>
55 #include <QtCore/QtPlugin>
76 , m_bProcessData(false)
78 , m_pRTMSAOutput(NULL)
95 QSharedPointer<IPlugin> RtHpi::clone()
const
97 QSharedPointer<RtHpi> pRtHpiClone(
new RtHpi);
111 connect(m_pRTMSAInput.data(), &PluginInputConnector::notify,
this, &RtHpi::update, Qt::DirectConnection);
118 m_pRTMSAOutput->data()->setMultiArraySize(100);
119 m_pRTMSAOutput->data()->setVisibility(
true);
125 if(!m_pRtHpiBuffer.isNull())
140 void RtHpi::initConnector()
142 qDebug() <<
"void RtHpi::initConnector()";
144 m_pRTMSAOutput->data()->initFromFiffInfo(m_pFiffInfo);
153 if(this->isRunning())
170 m_bIsRunning =
false;
178 m_pRtHpiBuffer->
clear();
180 m_pRTMSAOutput->data()->clear();
197 QString RtHpi::getName()
const
199 return "RtHpi Toolbox";
205 QWidget* RtHpi::setupWidget()
227 m_pFiffInfo = pRTMSA->info();
235 for(qint32 i = 0; i < pRTMSA->getMultiArraySize(); ++i)
237 t_mat = pRTMSA->getMultiSampleArray()[i];
238 m_pRtHpiBuffer->
push(&t_mat);
254 coil.pos = Eigen::MatrixXd::Zero(numCoils,3);
255 coil.mom = Eigen::MatrixXd::Zero(numCoils,3);
257 int numCh = m_pFiffInfo->nchan;
261 while(!m_pFiffInfo) msleep(10);
263 m_bProcessData =
true;
265 QVector<int> refind(0);
267 for(
int j=0;j<numCh;j++) {
268 if (m_pFiffInfo->ch_names[j].indexOf(
"TRG013") >= 0) {
271 if (m_pFiffInfo->ch_names[j].indexOf(
"TRG014") >= 0) {
274 if (m_pFiffInfo->ch_names[j].indexOf(
"TRG015") >= 0) {
277 if (m_pFiffInfo->ch_names[j].indexOf(
"TRG016") >= 0) {
282 QVector<int> innerind(0);
284 for (
int i=0;i<numCh;i++) {
286 if(m_pFiffInfo->chs[i].coil_type == 7002) {
289 if(!(m_pFiffInfo->bads.contains(m_pFiffInfo->ch_names[i]))) {
291 sensors.coilpos(j,0) = m_pFiffInfo->chs[i].loc(0);
292 sensors.coilpos(j,1) = m_pFiffInfo->chs[i].loc(1);
293 sensors.coilpos(j,2) = m_pFiffInfo->chs[i].loc(2);
294 sensors.coilori(j,0) = m_pFiffInfo->chs[i].loc(10);
295 sensors.coilori(j,1) = m_pFiffInfo->chs[i].loc(11);
296 sensors.coilori(j,2) = m_pFiffInfo->chs[i].loc(12);
302 int samF = m_pFiffInfo->sfreq;
304 int numLoc = 3, numBlock, samLoc, phase;
305 samLoc = samF/numLoc;
307 QVector<MatrixXd> buffer;
308 Eigen::MatrixXd amp(innerind.size(),numCoils);
310 while (m_bIsRunning) {
313 MatrixXd t_mat = m_pRtHpiBuffer->
pop();
315 buffer.append(t_mat);
317 if(buffer.size()*t_mat.cols() >= samLoc) {
319 Eigen::MatrixXd alldata(t_mat.rows(),buffer.size()*t_mat.cols());
321 for(
int i=0;i<buffer.size();i++) alldata << buffer[i];
324 Eigen::MatrixXd innerdata(innerind.size(),samLoc);
325 Eigen::MatrixXd refdata(numCoils,samLoc);
327 numBlock = alldata.cols()/samLoc;
330 for(
int i = 0;i<numBlock;i++) {
331 for(
int j = 0;j < innerind.size();j++)
332 innerdata.row(j) << alldata.block(innerind[j],i*samLoc,1,samLoc);
333 for(
int j = 0;j < refind.size();j++)
334 refdata.row(j) << alldata.block(refind[j],i*samLoc,1,samLoc);
337 for(
int j = 0;j < numCoils;j++) {
338 for(
int k = 0;k < innerind.size();k++) {
339 amp(k,j) = innerdata.row(k).cwiseProduct(refdata.row(j)).sum();
340 phase = amp(k,j)/abs(amp(k,j));
341 amp(k,j) = amp(k,j)*phase;
345 coil = dipfit(coil, sensors, amp);
351 for(qint32 i = 0; i < t_mat.cols(); ++i)
352 m_pRTMSAOutput->data()->setValue(t_mat.col(i));
369 coil = fminsearch(coil.pos, maxiter, 2 * maxiter * coil.pos.cols(), display, data, sensors);
371 temp = dipfitError(coil.pos, data, sensors);
373 coil.mom = temp.moment;
384 coilParam RtHpi::fminsearch(Eigen::MatrixXd pos,
int maxiter,
int maxfun,
int display, Eigen::MatrixXd data,
struct sens sensors)
386 double tolx, tolf, rho, chi, psi, sigma, func_evals, usual_delta, zero_term_delta, temp1, temp2;
387 std::string header, how;
388 int n, itercount, prnt;
389 Eigen::MatrixXd onesn, two2np1, one2n, v, y, v1, tempX1, tempX2, xbar, xr, x, xe, xc, xcc, xin;
390 std::vector <double> fv, fv1;
391 std::vector <int> idx;
393 dipError tempdip, fxr, fxe, fxc, fxcc;
406 header =
" Iteration Func-count min f(x) Procedure";
411 rho = 1; chi = 2; psi = 0.5; sigma = 0.5;
412 onesn = Eigen::MatrixXd::Ones(1,n);
413 two2np1 = one2n = Eigen::MatrixXd::Zero(1,n);
415 for(
int i = 0;i < n;i++) {
420 v = v1 = Eigen::MatrixXd::Zero(n, n+1);
421 fv.resize(n+1);idx.resize(n+1);fv1.resize(n+1);
423 for(
int i = 0;i < n; i++) v(i,0) = pos(i);
425 tempdip = dipfitError(pos, data, sensors);
426 fv[0] = tempdip.error;
429 func_evals = 1;itercount = 0;how =
"";
434 zero_term_delta = 0.00025;
435 xin = pos.transpose();
437 for(
int j = 0;j < n;j++) {
440 if(y(j) != 0) y(j) = (1 + usual_delta) * y(j);
441 else y(j) = zero_term_delta;
443 v.col(j+1).array() = y;
445 tempdip = dipfitError(pos, data, sensors);
446 fv[j+1] = tempdip.error;
451 for (
int i = 0; i < n+1; i++) idx[i] = i;
453 sort (idx.begin(), idx.end(), RtHpi::compar);
455 for (
int i = 0;i < n+1;i++) {
456 v1.col(i) = v.col(idx[i]);
462 how =
"initial simplex";
463 itercount = itercount + 1;
466 tempX1 = Eigen::MatrixXd::Zero(1,n);
468 while ((func_evals < maxfun) && (itercount < maxiter)) {
469 for (
int i = 0;i < n;i++) tempX1(i) = abs(fv[0] - fv[i+1]);
470 temp1 = tempX1.maxCoeff();
472 tempX2 = Eigen::MatrixXd::Zero(n,n);
474 for(
int i = 0;i < n;i++) tempX2.col(i) = v.col(i+1) - v.col(0);
476 tempX2 = tempX2.array().abs();
478 temp2 = tempX2.maxCoeff();
480 if((temp1 <= tolf) && (temp2 <= tolx))
break;
482 xbar = v.block(0,0,n,n).rowwise().sum();
485 xr = (1+rho) * xbar - rho * v.block(0,n,v.rows(),1);
488 std::cout <<
"Iteration Count: " << itercount <<
":" << x << std::endl;
490 fxr = dipfitError(x, data, sensors);
492 func_evals = func_evals+1;
494 if (fxr.error < fv[0]) {
496 xe = (1 + rho * chi) * xbar - rho * chi * v.col(v.cols()-1);
498 fxe = dipfitError(x, data, sensors);
499 func_evals = func_evals+1;
501 if(fxe.error < fxr.error) {
502 v.col(v.cols()-1) = xe;
507 v.col(v.cols()-1) = xr;
513 if(fxr.error < fv[n-1]) {
514 v.col(v.cols()-1) = xr;
520 if(fxr.error < fv[n]) {
522 xc = (1 + psi * rho) * xbar - psi * rho * v.col(v.cols()-1);
524 fxc = dipfitError(x, data, sensors);
525 func_evals = func_evals + 1;
527 if(fxc.error <= fxr.error) {
528 v.col(v.cols()-1) = xc;
530 how =
"contract outside";
538 xcc = (1 - psi) * xbar + psi * v.col(v.cols()-1);
540 fxcc = dipfitError(x, data, sensors);
541 func_evals = func_evals+1;
542 if(fxcc.error < fv[n]) {
543 v.col(v.cols()-1) = xcc;
545 how =
"contract inside";
552 if(how.compare(
"shrink") == 0) {
553 for(
int j = 1;j < n+1;j++) {
554 v.col(j).array() = v.col(0).array() + sigma * (v.col(j).array() - v.col(0).array());
555 x = v.col(j).array().transpose();
556 tempdip = dipfitError(x,data, sensors);
557 fv[j] = tempdip.error;
564 for (
int i = 0; i < n+1; i++) idx[i] = i;
565 sort (idx.begin (), idx.end (), RtHpi::compar);
566 for (
int i = 0;i < n+1;i++) {
567 v1.col(i) = v.col(idx[i]);
571 itercount = itercount + 1;
574 x = v.col(0).transpose();
575 std::cout << x << std::endl;
588 dipError RtHpi::dipfitError(Eigen::MatrixXd pos, Eigen::MatrixXd data,
struct sens sensors)
592 Eigen::MatrixXd lf, dif;
595 lf = ft_compute_leadfield(pos, sensors);
598 e.moment = pinv(lf) * data;
600 dif = data - lf * e.moment;
602 e.error = dif.array().square().sum()/data.array().square().sum();
616 Eigen::MatrixXd RtHpi::ft_compute_leadfield(Eigen::MatrixXd pos,
struct sens sensors)
619 Eigen::MatrixXd pnt, ori, lf;
621 pnt = sensors.coilpos;
622 ori = sensors.coilori;
624 lf = magnetic_dipole(pos, pnt, ori);
626 lf = sensors.tra * lf;
636 Eigen::MatrixXd RtHpi::magnetic_dipole(Eigen::MatrixXd pos, Eigen::MatrixXd pnt, Eigen::MatrixXd ori) {
640 Eigen::MatrixXd r, r2, r5, x, y, z, mx, my, mz, Tx, Ty, Tz, lf, temp;
645 pnt.array().col(0) -= pos(0);pnt.array().col(1) -= pos(1);pnt.array().col(2) -= pos(2);
647 r = pnt.array().square().rowwise().sum().sqrt();
649 r2 = r5 = x = y = z = mx = my = mz = Tx = Ty = Tz = lf = Eigen::MatrixXd::Zero(nchan,3);
651 for(
int i = 0;i < nchan;i++) {
652 r2.row(i).array().fill(pow(r(i),2));
653 r5.row(i).array().fill(pow(r(i),5));
656 for(
int i = 0;i < nchan;i++) {
657 x.row(i).array().fill(pnt(i,0));
658 y.row(i).array().fill(pnt(i,1));
659 z.row(i).array().fill(pnt(i,2));
661 mx.col(0).array().fill(1);my.col(1).array().fill(1);mz.col(2).array().fill(1);
663 Tx = 3 * x.cwiseProduct(pnt) - mx.cwiseProduct(r2);
664 Ty = 3 * y.cwiseProduct(pnt) - my.cwiseProduct(r2);
665 Tz = 3 * z.cwiseProduct(pnt) - mz.cwiseProduct(r2);
667 for(
int i = 0;i < nchan;i++) {
668 lf(i,0) = Tx.row(i).dot(ori.row(i));
669 lf(i,1) = Ty.row(i).dot(ori.row(i));
670 lf(i,2) = Tz.row(i).dot(ori.row(i));
673 for(
int i = 0;i < nchan;i++) {
674 for(
int j = 0;j < 3;j++) {
675 lf(i,j) = u0 * lf(i,j)/(4 * M_PI * r5(i,j));
681 bool RtHpi::compar (
int a,
int b){
682 return ((base_arr)[a] < (base_arr)[b]);
685 Eigen::Matrix4d computeTransformation(Eigen::MatrixXd NH, Eigen::MatrixXd BT)
687 Eigen::MatrixXd xdiff, ydiff, zdiff, C, Q;
688 Eigen::Matrix4d trans = Eigen::Matrix4d::Identity(4,4), Rot = Eigen::Matrix4d::Zero(4,4), Trans = Eigen::Matrix4d::Identity(4,4);
689 double meanx,meany,meanz,normf;
691 for(
int i = 0;i < 50;i++) {
692 zdiff = NH.col(2) - BT.col(2);
693 ydiff = NH.col(1) - BT.col(1);
694 xdiff = NH.col(0) - BT.col(0);
700 for (
int j=0;j<NH.rows();j++) {
701 BT(j,0) = BT(j,0) + meanx;
702 BT(j,1) = BT(j,1) + meany;
703 BT(j,2) = BT(j,2) + meanz;
706 C = BT.transpose() * NH;
708 Eigen::JacobiSVD< Eigen::MatrixXd > svd(C ,Eigen::ComputeThinU | Eigen::ComputeThinV);
710 Q = svd.matrixU() * svd.matrixV().transpose();
714 normf = (NH.transpose()-BT.transpose()).norm();
716 for(
int j=0;j<3;j++) {
717 for(
int k=0;k<3;k++) Rot(j,k) = Q(k,j);
721 Trans(0,3) = meanx;Trans(1,3) = meany;Trans(2,3) = meanz;
722 trans = Rot * Trans * trans;
727 Eigen::MatrixXd RtHpi::pinv(Eigen::MatrixXd a)
729 double epsilon = std::numeric_limits<double>::epsilon();
730 Eigen::JacobiSVD< Eigen::MatrixXd > svd(a ,Eigen::ComputeThinU | Eigen::ComputeThinV);
731 double tolerance = epsilon * std::max(a.cols(), a.rows()) * svd.singularValues().array().abs()(0);
732 return svd.matrixV() * (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(),0).matrix().asDiagonal() * svd.matrixU().adjoint();
734 std::vector <double>RtHpi::base_arr;
OutputConnectorList m_outputConnectors
QSharedPointer< CircularMatrixBuffer > SPtr
The circular matrix buffer.
The RtHpi class provides a RtHpi algorithm structure.
Contains the declaration of the RTHPI class.
void push(const Matrix< _Tp, Dynamic, Dynamic > *pMatrix)
Matrix< _Tp, Dynamic, Dynamic > pop()
QSharedPointer< NewMeasurement > SPtr
static QSharedPointer< PluginOutputData< T > > create(IPlugin *parent, const QString &name, const QString &descr)
InputConnectorList m_inputConnectors
The RealTimeMultiSampleArrayNew class is the base class of every RealTimeMultiSampleArrayNew Measurem...