DenseKernel(const MatrixXf & f, KernelType ktype, NormalizationType ntype):f_(f), ktype_(ktype), ntype_(ntype) { if (ktype_ == DIAG_KERNEL) parameters_ = VectorXf::Ones( f.rows() ); else if( ktype == FULL_KERNEL ) parameters_ = MatrixXf::Identity( f.rows(), f.rows() ); initLattice( f ); }
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() ); } }
// normalizeMatch respect to "In defense of eight point algorithm" void normalizeMatch(MatrixXf &mat, Matrix3f &T1, Matrix3f &T2) { MatrixXf pts1 = mat.leftCols<3>(); MatrixXf pts2 = mat.block(0, 3, mat.rows(), 3); normalizePts(pts1, T1); normalizePts(pts2, T2); mat.leftCols<3>() = pts1; mat.block(0, 3, mat.rows(), 3) = pts2; }
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); }
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; }
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 SortPoints(MatrixXf &x2d) { // the array is formed by [x1 y1 1, // x2 y2 ] MatrixXf x2d_aux=MatrixXf::Zero(x2d.rows(),3); ////////////////////////////////////////////////////////////// //first sort point in y /////////////////////////////////////////////////////////////// int i,j; for (i =0; i<x2d.rows();i++){ for(j = i+1; j < x2d.rows(); j ++) { if(x2d(j,1) < x2d(i,1)) { float temp_x = x2d(i,0); float temp_y = x2d(i,1); x2d(0,i) = x2d(0,j); x2d(1,i) = x2d(1,j); x2d(0,j) = temp_x; x2d(1,j) = temp_y; } } } ///////////////////////////////////////// //now order in X for (i =0; i<x2d.rows();i++){ for(j = i+1; j < x2d.rows(); j ++) { if(x2d(j,0) < x2d(i,0)) { float temp_x = x2d(i,0); float temp_y = x2d(i,1); x2d(0,i) = x2d(0,j); x2d(1,i) = x2d(1,j); x2d(0,j) = temp_x; x2d(1,j) = temp_y; } } } }
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; }
QByteArray BrainSourceSpaceTreeItem::createVertColor(const MatrixXf& vertices, const QColor& color) const { QByteArray arrayCurvatureColor; arrayCurvatureColor.resize(vertices.rows() * 3 * (int)sizeof(float)); float *rawColorArray = reinterpret_cast<float *>(arrayCurvatureColor.data()); int idxColor = 0; for(int i = 0; i<vertices.rows(); i++) { rawColorArray[idxColor++] = color.redF(); rawColorArray[idxColor++] = color.greenF(); rawColorArray[idxColor++] = color.blueF(); } return arrayCurvatureColor; }
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)); } } } }
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; }
VectorXf param_sensitivity_widget::computeSensitivity( MatrixXf ¶meterMatrix, VectorXf &responseVector) { MatrixXf Ctemp = parameterMatrix.transpose()*parameterMatrix; MatrixXf C; C = Ctemp.inverse(); VectorXf b = C*parameterMatrix.transpose()*responseVector; VectorXf Y_hat = parameterMatrix*b; int p = b.rows(); VectorXf sigma2Vec = responseVector-Y_hat; float sigma2 = sigma2Vec.squaredNorm(); sigma2= sigma2/(parameterMatrix.rows() - p); Ctemp = C*sigma2; MatrixXf denominator = Ctemp.diagonal(); // Do element-wise division VectorXf t = b; for (int i = 0; i < b.rows(); i++) { t(i) = abs(b(i)/sqrt(denominator(i))); } return t; }
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); } } }
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; }
float get_te_cost(int row, int col, int i, const MatrixXf & cost, const ExternNDArrayf & gradient_img) { if ((row + i < 0) || (row + i >= cost.rows())) { return INFINITY; } else { return (col == 0 ? 0 : cost(row+i, col-1)) + gradient_img(row, col); } }
int main(int argc, char* argv[]) { if (argc == 1){ setFooter(); return 0; } clock_t t0=clock(),t1; string fname = argv[1]; Eigen::MatrixXf A; readMatrix(argv[1], A); cout << "Read Matrix "<< argv[1] << " with: " << A.rows() << " datapoints and " << A.cols() << " features"<< endl; MatrixXf C = computeCoresetTree(A, atoi(argv[2]), atoi(argv[2]), atoi(argv[3])); cout << "Constructed coreset with " << C.rows() << " datapoints" << endl; cout << "Time taken in seconds " << (double)(clock() - t0)/CLOCKS_PER_SEC << endl; string npts(argv[2]); string cname = "coreset_" + npts + "_" + fname; writeMatrix(cname, C); /* t1 = clock(); Eigen::MatrixXf B;readMatrix("a9a_Xpos", B); MatrixXf C2 = computeCoresetTree(B, npoints, npoints, svd_method); writeMatrix("cor1_a9a_Xpos", C2); cout << "Time taken in seconds " << (double)(clock() - t1)/CLOCKS_PER_SEC << endl; */ return(0); }
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 }
bool ROSCameraCalibration::load_calibration(std::string const & filename) { ifstream fin(filename.c_str()); if(!fin) { cout << "ERROR: Could not open '" << filename << "' for parsing." << endl; return false; } YAML::Parser parser(fin); YAML::Node doc; while(parser.GetNextDocument(doc)) { doc["image_width"] >> image_width; doc["image_height"] >> image_height; doc["camera_name"] >> camera_name; load_matrix(doc["camera_matrix"], camera_matrix); load_matrix(doc["rectification_matrix"], rectification_matrix); load_matrix(doc["distortion_coefficients"], distortion_coefficients); load_matrix(doc["projection_matrix"], projection_matrix); } fin.close(); // inverse_projection_matrix = pseudo_inverse(projection_matrix); // implemented through SVD SVD<Matrix<float, 4, 3> > svd(projection_matrix.transpose()); MatrixXf diag = svd.singularValues().asDiagonal(); for(int i = 0; i < diag.rows(); i++) { if(diag(i, i) <= 1e-6) diag(i, i) = 0; else diag(i, i) = 1./diag(i, i); } inverse_projection_matrix = svd.matrixU()*diag*svd.matrixV().transpose(); return true; }
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; } }
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; }
extern "C" float find_trailing_edge(float * gradient_imgv, int gradient_rows, int gradient_cols, int startcol, int endrow, int endcol, int n_neighbors, int * outpathv, float * cost_mat) { ExternNDArrayf gradient_img(gradient_imgv, gradient_rows, gradient_cols); ExternNDArrayi outpath(outpathv, endcol - startcol, 2); /* Assume the gradient image is all setup, initialize cost and back */ printf("End point gradient: %0.2f\n", gradient_img(endrow, endcol)); VectorXi neighbor_range(n_neighbors); //printf("Building neighbor range\n"); for (struct {int ind; int neighbor;} N = {0, (-1 * n_neighbors / 2)}; N.neighbor<(n_neighbors / 2) + 1; N.neighbor++, N.ind++) { neighbor_range(N.ind) = N.neighbor; } MatrixXf cost = MatrixXf::Zero(gradient_rows, gradient_cols); //ExternNDArrayf cost(cost_mat, gradient_rows, gradient_cols); MatrixXi back = MatrixXi::Zero(gradient_rows, gradient_cols); //printf("Looping over image\n"); for (int col = startcol; col <= endcol; col++) { for (int row = 0; row < gradient_rows; row++) { // argmin over candidates int best_candidate = 0; // no travel in y-axis float best_cand_cost = INFINITY; for (int i=0; i < neighbor_range.rows(); i++) { float cand_cost = get_te_cost(row, col, neighbor_range(i), cost, gradient_img); if (cand_cost < best_cand_cost) { best_candidate = neighbor_range(i); best_cand_cost = cand_cost; } } back(row, col) = best_candidate; cost(row, col) = best_cand_cost; } } // Now determine the optimal path from the endrow, endcol position // We'll store the result in outpath -- since we know how that the path is constructed // One column at a time, we know how big the path will be ahead of time, which is very helpful int curr_row = endrow; float total_cost = 0; //printf("Reconstructing the optimal path\n"); for (struct {int ind; int col;} P = {0, endcol}; P.col > startcol; P.col--, P.ind++) { //printf("%d\n", curr_row); total_cost += cost(curr_row, P.col); // x, y outpath(P.ind, 0) = P.col; outpath(P.ind, 1) = curr_row; curr_row = MIN(MAX(0, curr_row + back(curr_row, P.col)),cost.rows()-1); } //printf("Cost incurred on optimal path: %0.2f\n", total_cost); return total_cost; }
/////////////////////// ///// 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 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()); }
void SortPointsAngle(MatrixXf &x2d){ // the array is formed by [x1 y1 1, // x2 y2 ] // compute center point Vector3f point=x2d.colwise().sum(); point=point/x2d.rows(); vector<float> angles; int i,j; // compute angles for (i=0; i<x2d.rows();i++){ double x=(point(0)-x2d(i,0)); double y=(point(1)-x2d(i,1)); angles.push_back(atan2(y,x)*180 / (3.1416));//degree in radians } ////////////////////////////////////////////////////// // now order by angle chossing the minor to mayor for (i =0; i<x2d.rows();i++){ for(j = i+1; j < x2d.rows(); j ++) { if(angles[j] < angles[i]) { float temp_x = x2d(i,0); float temp_y = x2d(i,1); x2d(i,0) = x2d(j,0); x2d(i,1) = x2d(j,1); x2d(j,0) = temp_x; x2d(j,1) = temp_y; } } } }
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; }
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; }
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; }
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; } }
virtual VectorXf gradient( const MatrixXf & a, const MatrixXf & b ) const { if (ktype_ == CONST_KERNEL) return VectorXf(); MatrixXf fg = featureGradient( a, b ); if (ktype_ == DIAG_KERNEL) return (f_.array()*fg.array()).rowwise().sum(); else { MatrixXf p = fg*f_.transpose(); p.resize( p.cols()*p.rows(), 1 ); return p; } }