Example #1
0
	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 );
	}
Example #2
0
	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;
}
Example #4
0
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 );
}
Example #5
0
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_(&notrans,&notrans,&M,&N,&K,&fone,
         const_cast<float*>(a.data()),&lda,
         const_cast<float*>(b.data()),&ldb,&fone,
         c.data(),&ldc);
}
Example #6
0
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;
               
               
	}
    
    }
  }
  
  
}
Example #9
0
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;
}
Example #10
0
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;
}
Example #11
0
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));
            }
        }
    }
}
Example #12
0
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 &parameterMatrix, 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;
}
Example #14
0
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);
        }
    }
}
Example #15
0
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;
}
Example #16
0
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);
    }
}
Example #17
0
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);
}
Example #18
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;
}
Example #20
0
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;
}
Example #22
0
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;

}
Example #23
0
///////////////////////
/////  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();
	}
}
Example #24
0
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;
}
Example #27
0
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;
}
Example #29
0
	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;
		}
	}
Example #30
0
	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;
		}
	}