void FiffSimulator::doContinousHPI(MatrixXf& matData)
{
//This only works with babyMEG HPI channels 400 ... 407
if(m_pFiffInfo && m_pHPIWidget && matData.rows() >= 407) {
if(m_pHPIWidget->wasLastFitOk()) {
// Load device to head transformation matrix from Fiff info
QMatrix3x3 rot;
for(int ir = 0; ir < 3; ir++) {
for(int ic = 0; ic < 3; ic++) {
rot(ir,ic) = m_pFiffInfo->dev_head_t.trans(ir,ic);
}
}
QQuaternion quatHPI = QQuaternion::fromRotationMatrix(rot);
// Write rotation quaternion to HPI Ch #1~3
matData.row(401) = MatrixXf::Constant(1,matData.cols(), quatHPI.x());
matData.row(402) = MatrixXf::Constant(1,matData.cols(), quatHPI.y());
matData.row(403) = MatrixXf::Constant(1,matData.cols(), quatHPI.z());
// Write translation vector to HPI Ch #4~6
matData.row(404) = MatrixXf::Constant(1,matData.cols(), m_pFiffInfo->dev_head_t.trans(0,3));
matData.row(405) = MatrixXf::Constant(1,matData.cols(), m_pFiffInfo->dev_head_t.trans(1,3));
matData.row(406) = MatrixXf::Constant(1,matData.cols(), m_pFiffInfo->dev_head_t.trans(2,3));
// Write GOF to HPI Ch #7
// Write goodness of fit (GOF)to HPI Ch #7
float dpfitError = 0.0;
float GOF = 1 - dpfitError;
matData.row(407) = MatrixXf::Constant(1,matData.cols(), GOF);
}
}
}
MatrixXf transformPoints(Matrix3f X, MatrixXf P){
MatrixXf Pfull(3, P.cols());
for(int i=0; i<P.cols(); i++){
Pfull(0, i) = P(0, i);
Pfull(1, i) = P(1, i);
Pfull(2, i) = 1;
}
Pfull = X*Pfull;
MatrixXf Pt(2, P.cols());
for(int i=0; i<P.cols(); i++){
Pt(0, i) = Pfull(0, i);
Pt(1, i) = Pfull(1, i);
}
return Pt;
}
double IntersectionOverUnion::evaluate( MatrixXf & d_mul_Q, const MatrixXf & Q ) const {
assert( gt_.rows() == Q.cols() );
const int N = Q.cols(), M = Q.rows();
d_mul_Q = 0*Q;
VectorXd in(M), un(M);
in.fill(0.f);
un.fill(1e-20);
for( int i=0; i<N; i++ ) {
if( 0 <= gt_[i] && gt_[i] < M ) {
in[ gt_[i] ] += Q(gt_[i],i);
un[ gt_[i] ] += 1;
for( int l=0; l<M; l++ )
if( l!=gt_[i] )
un[ l ] += Q(l,i);
}
}
for( int i=0; i<N; i++ )
if( 0 <= gt_[i] && gt_[i] < M ) {
for( int l=0; l<M; l++ )
if( l==gt_[i] )
d_mul_Q(l,i) = Q(l,i) / (un[l]*M);
else
d_mul_Q(l,i) = - Q(l,i) * in[l] / ( un[l] * un[l] * M);
}
return (in.array()/un.array()).sum()/M;
}
MatrixXf featureGradient( const MatrixXf & a, const MatrixXf & b ) const {
if (ntype_ == NO_NORMALIZATION )
return kernelGradient( a, b );
else if (ntype_ == NORMALIZE_SYMMETRIC ) {
MatrixXf fa = lattice_.compute( a*norm_.asDiagonal(), true );
MatrixXf fb = lattice_.compute( b*norm_.asDiagonal() );
MatrixXf ones = MatrixXf::Ones( a.rows(), a.cols() );
VectorXf norm3 = norm_.array()*norm_.array()*norm_.array();
MatrixXf r = kernelGradient( 0.5*( a.array()*fb.array() + fa.array()*b.array() ).matrix()*norm3.asDiagonal(), ones );
return - r + kernelGradient( a*norm_.asDiagonal(), b*norm_.asDiagonal() );
}
else if (ntype_ == NORMALIZE_AFTER ) {
MatrixXf fb = lattice_.compute( b );
MatrixXf ones = MatrixXf::Ones( a.rows(), a.cols() );
VectorXf norm2 = norm_.array()*norm_.array();
MatrixXf r = kernelGradient( ( a.array()*fb.array() ).matrix()*norm2.asDiagonal(), ones );
return - r + kernelGradient( a*norm_.asDiagonal(), b );
}
else /*if (ntype_ == NORMALIZE_BEFORE )*/ {
MatrixXf fa = lattice_.compute( a, true );
MatrixXf ones = MatrixXf::Ones( a.rows(), a.cols() );
VectorXf norm2 = norm_.array()*norm_.array();
MatrixXf r = kernelGradient( ( fa.array()*b.array() ).matrix()*norm2.asDiagonal(), ones );
return -r+kernelGradient( a, b*norm_.asDiagonal() );
}
}
void sumAndNormalize( MatrixXf & out, const MatrixXf & in, const MatrixXf & Q ) {
out.resize( in.rows(), in.cols() );
for( int i=0; i<in.cols(); i++ ){
VectorXf b = in.col(i);
VectorXf q = Q.col(i);
out.col(i) = b.array().sum()*q - b;
}
}
///////////////////////
///// Inference /////
///////////////////////
void expAndNormalize ( MatrixXf & out, const MatrixXf & in ) {
out.resize( in.rows(), in.cols() );
for( int i=0; i<out.cols(); i++ ){
VectorXf b = in.col(i);
b.array() -= b.maxCoeff();
b = b.array().exp();
out.col(i) = b / b.array().sum();
}
}
void Permutohedral::compute ( MatrixXf & out, const MatrixXf & in, bool reverse ) const
{
if( out.cols() != in.cols() || out.rows() != in.rows() )
out = 0*in;
if( in.rows() <= 2 )
seqCompute( out.data(), in.data(), in.rows(), reverse );
else
sseCompute( out.data(), in.data(), in.rows(), reverse );
}
void blas_gemm(const MatrixXf& a, const MatrixXf& b, MatrixXf& c)
{
int M = c.rows(); int N = c.cols(); int K = a.cols();
int lda = a.rows(); int ldb = b.rows(); int ldc = c.rows();
sgemm_(¬rans,¬rans,&M,&N,&K,&fone,
const_cast<float*>(a.data()),&lda,
const_cast<float*>(b.data()),&ldb,&fone,
c.data(),&ldc);
}
vector<float> applyPCAtoVector(vector<float> &descriptorValues, MatrixXf &eigen_vects)
{
MatrixXf datapoint(1,descriptorValues.size());
for (int i = 0; i < descriptorValues.size(); ++i)
datapoint(0,i) = descriptorValues[i];
MatrixXf reduceddatapnt = pca::transformPointMatrix(datapoint, eigen_vects);
vector<float> retfeatvect(reduceddatapnt.cols());
for (int i = 0; i < reduceddatapnt.cols(); ++i)
retfeatvect[i] = reduceddatapnt(0,i);
return retfeatvect;
}
vector<vector<float> > applyPCAtoVector2D(vector<vector<float> > &descriptorValues, MatrixXf &eigen_vects)
{
MatrixXf datapoints(descriptorValues.size(),descriptorValues[0].size());
for (int i = 0; i < descriptorValues.size(); ++i)
for (int j = 0; j < descriptorValues[0].size(); ++j)
datapoints(i, j) = descriptorValues[i][j];
MatrixXf reduceddatapnts = pca::transformPointMatrix(datapoints, eigen_vects);
vector<vector<float> > retfeatvects(reduceddatapnts.rows(), vector<float>(reduceddatapnts.cols()));
for (int i = 0; i < reduceddatapnts.rows(); ++i)
for (int j = 0; j < reduceddatapnts.cols(); ++j)
retfeatvects[i][j] = reduceddatapnts(i,j);
return retfeatvects;
}
double LogLikelihood::evaluate( MatrixXf & d_mul_Q, const MatrixXf & Q ) const {
assert( gt_.rows() == Q.cols() );
const int N = Q.cols(), M = Q.rows();
double r = 0;
d_mul_Q = 0*Q;
for( int i=0; i<N; i++ )
if( 0 <= gt_[i] && gt_[i] < M ) {
float QQ = std::max( Q(gt_[i],i)+robust_, 1e-20f );
// Make it negative since it's a
r += log(QQ) / N;
d_mul_Q(gt_[i],i) += Q(gt_[i],i) / QQ / N;
}
return r;
}
double Hamming::evaluate( MatrixXf & d_mul_Q, const MatrixXf & Q ) const {
assert( gt_.rows() == Q.cols() );
const int N = Q.cols(), M = Q.rows();
double r = 0;
d_mul_Q = 0*Q;
for( int i=0; i<N; i++ )
if( 0 <= gt_[i] && gt_[i] < M ) {
float QQ = class_weight_[ gt_[i] ] * Q(gt_[i],i);
// Make it negative since it's a
r += QQ;
d_mul_Q(gt_[i],i) += QQ;
}
return r;
}
void Neuromag::run()
{
MatrixXf matValue;
qint32 size = 0;
while(m_bIsRunning) {
if(m_pRawMatrixBuffer_In) {
//pop matrix
matValue = m_pRawMatrixBuffer_In->pop();
//Write raw data to fif file
if(m_bWriteToFile) {
size += matValue.rows()*matValue.cols() * 4;
if(size > MAX_DATA_LEN) {
size = 0;
this->splitRecordingFile();
}
m_mutex.lock();
if(m_pOutfid) {
m_pOutfid->write_raw_buffer(matValue.cast<double>());
}
m_mutex.unlock();
} else {
size = 0;
}
if(m_pRTMSA_Neuromag) {
m_pRTMSA_Neuromag->data()->setValue(this->calibrate(matValue));
}
}
}
}
void RealtimeMF_openni::projectDirections(cv::Mat& I, const MatrixXf& dirs,
double f_d, const Matrix<uint8_t,Dynamic,Dynamic>& colors)
{
double scale = 0.1;
VectorXf p0(3); p0 << 0.35,0.25,1;
double u0 = p0(0)/p0(2)*f_d + 320.;
double v0 = p0(1)/p0(2)*f_d + 240.;
for(uint32_t k=0; k < dirs.cols(); ++k)
{
VectorXf p1 = p0 + dirs.col(k)*scale;
double u1 = p1(0)/p1(2)*f_d + 320.;
double v1 = p1(1)/p1(2)*f_d + 240.;
cv::line(I, cv::Point(u0,v0), cv::Point(u1,v1),
CV_RGB(colors(k,0),colors(k,1),colors(k,2)), 2, CV_AA);
double arrowLen = 10.;
double angle = atan2(v1-v0,u1-u0);
double ru1 = u1 - arrowLen*cos(angle + M_PI*0.25);
double rv1 = v1 - arrowLen*sin(angle + M_PI*0.25);
cv::line(I, cv::Point(u1,v1), cv::Point(ru1,rv1),
CV_RGB(colors(k,0),colors(k,1),colors(k,2)), 2, CV_AA);
ru1 = u1 - arrowLen*cos(angle - M_PI*0.25);
rv1 = v1 - arrowLen*sin(angle - M_PI*0.25);
cv::line(I, cv::Point(u1,v1), cv::Point(ru1,rv1),
CV_RGB(colors(k,0),colors(k,1),colors(k,2)), 2, CV_AA);
}
cv::circle(I, cv::Point(u0,v0), 2, CV_RGB(0,0,0), 2, CV_AA);
}
VectorXf EMclustering::logsumexp(MatrixXf x, int dim)
{
int r = x.rows();
int c = x.cols();
VectorXf y(r);
MatrixXf tmp1(r,c);
VectorXf tmp2(r);
VectorXf s(r);
y = x.rowwise().maxCoeff();//cerr<<"y"<<y<<endl<<endl;
x = x.colwise() - y;
//cerr<<"x"<<x<<endl<<endl;
tmp1 = x.array().exp();
//cerr<<"t"<<tmp1<<endl<<endl;
tmp2 = tmp1.rowwise().sum();
//cerr<<"t"<<tmp2<<endl<<endl;
s = y.array() + tmp2.array().log();
for(int i=0;i<s.size();i++)
{
if(!isfinite(s(i)))
{
s(i) = y(i);
}
}
y.resize(0);
tmp1.resize(0,0);
tmp2.resize(0);
return s;
}
void BabyMEG::createDigTrig(MatrixXf& data)
{
//Look for triggers in all trigger channels
//m_qMapDetectedTrigger = DetectTrigger::detectTriggerFlanksMax(data.at(b), m_lTriggerChannelIndices, m_iCurrentSample-nCol, m_dTriggerThreshold, true);
QMap<int,QList<QPair<int,double> > > qMapDetectedTrigger = DetectTrigger::detectTriggerFlanksGrad(data.cast<double>(), m_lTriggerChannelIndices, 0, 3.0, false, "Rising");
//Combine and write results into data block's digital trigger channel
QMapIterator<int,QList<QPair<int,double> >> i(qMapDetectedTrigger);
int counter = 0;
int idxDigTrig = m_pFiffInfo->ch_names.indexOf("DTRG01");
while (i.hasNext())
{
i.next();
QList<QPair<int,double> > lDetectedTriggers = i.value();
for(int k = 0; k < lDetectedTriggers.size(); ++k)
{
if(lDetectedTriggers.at(k).first < data.cols() && lDetectedTriggers.at(k).first >= 0)
{
data(idxDigTrig,lDetectedTriggers.at(k).first) = data(idxDigTrig,lDetectedTriggers.at(k).first) + pow(2,counter);
}
}
counter++;
}
}
/**
* Normalizes each eigenface in a matrix.
*
* @param eigenfaces A matrix of eigen faces to normalize
*/
void normalizeEigenFaces(MatrixXf &eigenfaces)
{
for(int i = 0; i < eigenfaces.cols(); i++)
{
eigenfaces.col(i).normalize();
}
}
void KF_joseph_update(VectorXf &x, MatrixXf &P,float v,float R, MatrixXf H)
{
VectorXf PHt = P*H.transpose();
MatrixXf S = H*PHt;
S(0,0) += R;
MatrixXf Si = S.inverse();
Si = make_symmetric(Si);
MatrixXf PSD_check = Si.llt().matrixL(); //chol of scalar is sqrt
PSD_check.transpose();
PSD_check.conjugate();
VectorXf W = PHt*Si;
x = x+W*v;
//Joseph-form covariance update
MatrixXf eye(P.rows(), P.cols());
eye.setIdentity();
MatrixXf C = eye - W*H;
P = C*P*C.transpose() + W*R*W.transpose();
float eps = 2.2204*pow(10.0,-16); //numerical safety
P = P+eye*eps;
PSD_check = P.llt().matrixL();
PSD_check.transpose();
PSD_check.conjugate(); //for upper tri
}
void Util::processGramSchmidt(MatrixXf& mat){
for (int i = 0; i < mat.cols(); ++i){
for (int j = 0; j < i; ++j){
float r = mat.col(i).dot(mat.col(j));
mat.col(i) -= r * mat.col(j);
}
float norm = mat.col(i).norm();
if (norm < SVD_EPS){
for (int k = i; k < mat.cols(); ++k){
mat.col(k).setZero();
}
return;
}
mat.col(i) *= (1.f / norm);
}
}
void BabyMEGSQUIDControlDgl::TuneGraphDispProc(MatrixXf tmp)
{
// std::cout << "first ten elements \n" << tmp.block(0,0,1,10) << std::endl;
int cols = tmp.cols();
int chanIndx = ui->m_Qcb_channel->currentIndex();//1;//
// plot the real time data here
settings.minX = 0.0;
settings.maxX = cols;
settings.minY = mmin(tmp,chanIndx);
settings.maxY = mmax(tmp,chanIndx);
settings.xlabel = QString("%1 samples/second").arg(m_pBabyMEG->m_dSfreq) ;
settings.ylabel = QString("Amplitude [rel. unit]");
d_timeplot->setPlotSettings(settings);
// qDebug()<<"minY"<<settings.minY<<"maxY"<<settings.maxY;
QVector <QPointF> F;
for(int i=0; i<cols;i++)
F.append(QPointF(i,tmp(chanIndx,i)));
d_timeplot->setCurveData(0,F);
d_timeplot->show();
}
void Util::sampleGaussianMat(MatrixXf& mat){
for (int i = 0; i < mat.rows(); ++i){
int j = 0;
for ( ; j+1 < mat.cols(); j += 2){
float f1, f2;
sampleTwoGaussian(f1, f2);
mat(i,j ) = f1;
mat(i,j+1) = f2;
}
for (; j < mat.cols(); j ++){
float f1, f2;
sampleTwoGaussian(f1, f2);
mat(i, j) = f1;
}
}
}
void toHomogeneous(MatrixXf &mat) {
MatrixXf temp;
if (mat.cols() == 2) {
temp.resize(mat.rows(), 3);
temp.leftCols<2>() = mat.leftCols<2>();
temp.col(2).setConstant(1);
mat = temp;
} else if (mat.cols() == 4) {
temp.resize(mat.rows(), 6);
temp.leftCols<2>() = mat.leftCols<2>();
temp.col(2).setConstant(1);
temp.block(0, 3, mat.rows(), 2) = temp.block(0, 2, mat.rows(), 2);
temp.col(5).setConstant(1);
mat = temp;
} else
cout << "toHomogeneous with wrong dimension" << endl;
}
void matrixMultiply(const RealDescriptor& x, const MatrixXf& matrix,
RealDescriptor& result) {
Q_ASSERT(x.size() == matrix.cols());
int targetDimension = matrix.rows();
result.resize(targetDimension);
VectorXf::Map(result.data(), targetDimension) = matrix * VectorXf::Map(x.data(), x.size());
}
VectorXs DenseCRF::currentMap( const MatrixXf & Q ) const{
VectorXs r(Q.cols());
// Find the map
for( int i=0; i<N_; i++ ){
int m;
Q.col(i).maxCoeff( &m );
r[i] = m;
}
return r;
}
void noHomogeneous(MatrixXf &mat) {
MatrixXf temp;
if (mat.cols() == 3) {
temp.resize(mat.rows(), 2);
temp.col(0).array() = mat.col(0).array()/mat.col(2).array();
temp.col(1).array() = mat.col(1).array()/mat.col(2).array();
mat = temp;
} else
cout << "toHomogeneous with wrong dimension" << endl;
}
QPainterPath Layouter::mat2Path( const MatrixXf& pntMat )
{
QPainterPath path;
if (pntMat.rows() <= 0 || pntMat.cols() != 2)
return path;
path.moveTo(pntMat(0,0), pntMat(0,1));
for (int i = 1; i < pntMat.rows(); ++i)
path.lineTo(pntMat(i,0), pntMat(i,1));
return path;
}
VectorXf EMclustering::loggausspdf(MatrixXf x, VectorXf mu, MatrixXf sigma)
{
//cerr<<x<<endl<<endl;
//cerr<<mu<<endl<<endl;
//cerr<<sigma<<endl<<endl;
int d = x.rows();
int c = x.cols();
int r_sigma = sigma.rows();
int c_sigma = sigma.cols();
MatrixXf tmpx(x.rows(),x.cols());
tmpx = x.colwise() - mu;
MatrixXf u1(r_sigma,c_sigma);
u1 = sigma.llt().matrixL() ;
MatrixXf u2(u1.cols(),u1.rows());
u2 = u1.adjoint();//cerr<<u2<<endl;
MatrixXf Q(u2.cols(),tmpx.cols());
Q = u1.jacobiSvd(ComputeThinU | ComputeThinV).solve(tmpx);
//cerr<<"q"<<Q<<endl;
VectorXf q(Q.cols());
q = Q.cwiseProduct(Q).colwise().sum();//cerr<<"q"<<q<<endl;
VectorXf tmp1(u2.rows());
tmp1 = u2.diagonal();
tmp1 = tmp1.array().log();
double c1 = tmp1.sum() * 2;
double c2 = d * log(2*PI);//cerr<<c1+c2<<endl;
VectorXf y(q.size());
y = -(c1+c2)/2. - q.array()/2.;
tmpx.resize(0,0);
u1.resize(0,0);
u2.resize(0,0);
Q.resize(0,0);
q.resize(0);
tmp1.resize(0);
return y;
}
virtual VectorXf parameters() const {
if (ktype_ == CONST_KERNEL)
return VectorXf();
else if (ktype_ == DIAG_KERNEL)
return parameters_;
else {
MatrixXf p = parameters_;
p.resize( p.cols()*p.rows(), 1 );
return p;
}
}
bool singleModelRANSAC(const MatrixXf &data, int M, MatrixXf &inlier) {
int maxdegen = 10;
int dataSize = data.rows();
int psize = 4;
MatrixXf x1 = data.block(0, 0, data.rows(), 3);
MatrixXf x2 = data.block(0, 3, data.rows(), 3);
vector<int> sample;
MatrixXf pts1(4, 3);
MatrixXf pts2(4, 3);
int maxInlier = -1;
MatrixXf bestResidue;
for (int m = 0; m < M; m++) {
int degencount = 0;
int isdegen = 1;
while (isdegen==1 && degencount < maxdegen) {
degencount ++;
RandomSampling(psize, dataSize, sample);
for (int i = 0; i < psize; i++) {
pts1.row(i) = x1.row(sample[i]);
pts2.row(i) = x2.row(sample[i]);
}
if (sampleValidTest(pts1, pts2))
isdegen = 0;
}
if (isdegen) {
cout << "Cannot find valid p-subset" << endl;
return false;
}
Matrix3f local_H;
MatrixXf local_A;
fitHomography(pts1, pts2, local_H, local_A);
MatrixXf residue;
computeHomographyResidue(x1, x2, local_H, residue);
int inlierCount = (residue.array() < THRESHOLD).count();
if (inlierCount > maxInlier) {
maxInlier = inlierCount;
bestResidue = residue;
}
}
inlier.resize(maxInlier, data.cols());
int transferCounter = 0;
for (int i = 0; i < dataSize; i++) {
if (bestResidue(i) < THRESHOLD) {
inlier.row(transferCounter) = data.row(i);
transferCounter++;
}
}
if (transferCounter != maxInlier) {
cout << "RANSAC result size does not match!!!!" << endl;
return false;
}
return true;
}
void filterPointAtInfinity(MatrixXf &pts1, MatrixXf &pts2) {
int finiteCount = 0;
for (int i = 0; i < pts1.rows(); i++) {
if (abs(pts1(i, 2)) > FLT_EPSILON && abs(pts2(i, 2) > FLT_EPSILON))
finiteCount++;
}
MatrixXf temp_pts1, temp_pts2;
temp_pts1.resize(finiteCount, pts1.cols());
temp_pts2.resize(finiteCount, pts2.cols());
int idx = 0;
for (int i = 0; i < pts1.rows(); i++) {
if (abs(pts1(i, 2)) > FLT_EPSILON && abs(pts2(i, 2) > FLT_EPSILON)) {
temp_pts1.row(idx) = pts1.row(i);
temp_pts2.row(idx) = pts2.row(i);
idx++;
}
}
pts1 = temp_pts1;
pts2 = temp_pts2;
}
