Java Code Examples for org.ejml.simple.SimpleMatrix#minus()

public static SimpleMatrix laplacian(final GraphReadMethods graph) {
    final SimpleMatrix adjacency = adjacency(graph, false);
    final SimpleMatrix degree = degree(graph);
    return degree.minus(adjacency);
}
 

public static double euclidianDistance(SimpleMatrix columnVector1, SimpleMatrix columnVector2) {
	double distance = 0;
	SimpleMatrix distVector = columnVector2.minus(columnVector1);
	distance = Math.sqrt(MatrixOps.elemPow(distVector, 2).elementSum());
	return distance;
}
 

/**
 * This method searches a local maximum by gradient-quadratic search. First a direct leap to the maximum by 
 * quadratic optimization is tried. Then gradient search is used to refine the result in case of an overshoot.
 * Uses means, covariances and component weights given as parameters.
 * 
 * This algorithm was motivated by this paper: 
 * Miguel A. Carreira-Perpinan (2000): "Mode-finding for mixtures of
 * Gaussian distributions", IEEE Trans. on Pattern Analysis and
 * Machine Intelligence 22(11): 1318-1323.
 * 
 * @param start Defines the starting point for the search.
 * @return The serach result containing the point and the probability value at that point.
 */
public static SearchResult gradQuadrSearch(SimpleMatrix start, ArrayList<SimpleMatrix> means, ArrayList<SimpleMatrix> covs, ArrayList<Double> weights, SampleModel model){

	SimpleMatrix gradient = new SimpleMatrix(2,1);
	SimpleMatrix hessian = new SimpleMatrix(2,2);
	double n = means.get(0).numRows();
	double a = Math.pow(Math.sqrt(2 * Math.PI), n);
	
	SimpleMatrix x = new SimpleMatrix(2,1);
	x.set(0,0,start.get(start.numRows()-2,0));
	x.set(1,0,start.get(start.numRows()-1,0));
	ArrayList<Double> mahalanobisDistances;
	double step = START_STEP_SIZE;
	double probability = 0;
	SimpleMatrix gradStep = null;
	int count =0;
	do {
		mahalanobisDistances = mahalanobis(x, means, covs);
		double prob = 0;
		// this loop calculates gradient and hessian as well as probability at x
		for (int i = 0; i < means.size(); i++) {
			// check whether the component actually contributes to to the density at given point by mahalanobis distance
			if(mahalanobisDistances.get(i) < MAX_MAHALANOBIS_DIST) {
				SimpleMatrix m = means.get(i);
				SimpleMatrix dm = m.minus(x);
				SimpleMatrix c = covs.get(i);
				SimpleMatrix invC = c.invert();
				double w = weights.get(i);
				//probability p(x,m) under component m
				double p = ((1 / (a * Math.sqrt(c.determinant()))) * Math.exp((-0.5d) * mahalanobisDistances.get(i))) * w;
				prob += p; 
				// gradient at x
				gradient = gradient.plus( invC.mult(dm).scale(p) );
				// hessian at x
				hessian = hessian.plus( invC.mult( dm.mult(dm.transpose()).minus(c) ).mult(invC).scale(p) );
			}


		}
		// save x
		SimpleMatrix xOld = new SimpleMatrix(x);
		double tst = evaluate(xOld, means, covs, weights);
		// check if hessian is negative definite
		SimpleEVD hessianEVD = hessian.eig();
		int maxEVIndex = hessianEVD.getIndexMax();
		// try a direct leap by quadratic optimization
		if(hessianEVD.getEigenvalue(maxEVIndex).getReal() < 0){
			gradStep = hessian.invert().mult(gradient);
			x = xOld.minus(gradStep);
		}
		double prob1 = 	evaluate(x, means, covs, weights);
		// if quadratic optimization did not work try gradient ascent
		if( prob1 <= prob | hessianEVD.getEigenvalue(maxEVIndex).getReal() >= 0) {
			gradStep = gradient.scale(step);
			x = xOld.plus(gradStep);
			// if still not ok decrease step size iteratively
			while(evaluate(x, means, covs, weights) < prob){
				step = step/2;
				gradStep = gradient.scale(step);
				x = xOld.plus(gradStep);
			}
		}
		probability =	model.evaluate(x, means, covs, weights);
		count++;
		// continue until the last step is sufficiently small or
		// a predefined amount of steps was performed
	}while(gradStep.elementMaxAbs() > STOP_STEP_SIZE && count<10);

	// return results
	return new SearchResult(x, probability);
}
 

private double getIntSquaredHessian(SimpleMatrix[] means, Double[] weights, SimpleMatrix[] covariance, SimpleMatrix F, SimpleMatrix g) {
	long time = System.currentTimeMillis();
	long d = means[0].numRows();
	long N = means.length;
	// normalizer
	double constNorm = Math.pow((1d / (2d * Math.PI)), (d / 2d));

	// test if F is identity for speedup
	SimpleMatrix Id = SimpleMatrix.identity(F.numCols());
	double deltaF = F.minus(Id).elementSum();

	double w1, w2, m, I = 0, eta, f_t, c;
	SimpleMatrix s1, s2, mu1, mu2, dm, ds, B, b, C;
	for (int i1 = 0; i1 < N; i1++) {
		s1 = covariance[i1].plus(g);
		mu1 = means[i1];
		w1 = weights[i1];
		for (int i2 = i1; i2 < N; i2++) {
			s2 = covariance[i2];
			mu2 = means[i2];
			w2 = weights[i2];
			SimpleMatrix A = s1.plus(s2).invert();
			dm = mu1.minus(mu2);

			// if F is not identity
			if (deltaF > 1e-3) {
				ds = dm.transpose().mult(A);
				b = ds.transpose().mult(ds);
				B = A.minus(b.scale(2));
				C = A.minus(b);
				f_t = constNorm * Math.sqrt(A.determinant()) * Math.exp(-0.5 * ds.mult(dm).trace());
				c = 2 * F.mult(A).mult(F).mult(B).trace() + Math.pow(F.mult(C).trace(), 2);
			} else {
				m = dm.transpose().mult(A).mult(dm).get(0);
				f_t = constNorm * Math.sqrt(A.determinant()) * Math.exp(-0.5 * m);

				DenseMatrix64F A_sqr = new DenseMatrix64F(A.numRows(), A.numCols());
				CommonOps.elementMult(A.getMatrix(), A.transpose().getMatrix(), A_sqr);
				double sum = CommonOps.elementSum(A_sqr);
				c = 2d * sum * (1d - 2d * m) + Math.pow((1d - m), 2d) * Math.pow(A.trace(), 2);
			}

			// determine the weight of the current term
			if (i1 == i2)
				eta = 1;
			else
				eta = 2;
			I = I + f_t * c * w2 * w1 * eta;
		}
	}
	/*time = System.currentTimeMillis()-time;
	if((time) > 100)
		System.out.println("Time for IntSqrdHessian: "+ ((double)time/1000)+"s"+"  loopcount: "+N);*/
	return I;
}
 

/**
 * This method derives the conditional distribution of the actual sample model kde with distribution p(x).
 * It takes a condition parameter that is a vector c of dimension m. Using this vector
 * it finds the conditional distribution p(x*|c) where c=(x_0,...,x_m), x*=(x_m+1,...,x_n).
 * For detailed description see:
 * @param condition A vector that defines c in p(x*|c)
 * @return The conditional distribution of this sample model under the given condition
 */
public ConditionalDistribution getConditionalDistribution(SimpleMatrix condition){
	int lenCond = condition.numRows();
	
	ArrayList<SimpleMatrix> means = this.getSubMeans();
	ArrayList<SimpleMatrix> conditionalMeans = new ArrayList<SimpleMatrix>();

	ArrayList<SimpleMatrix> covs = this.getSubSmoothedCovariances();
	ArrayList<SimpleMatrix> conditionalCovs = new ArrayList<SimpleMatrix>();
	
	ArrayList<Double> weights = this.getSubWeights();
	ArrayList<Double> conditionalWeights = new ArrayList<Double>();

	ConditionalDistribution result = null;
	
	double n = condition.numRows();
	double a = Math.pow(Math.sqrt(2 * Math.PI), n);

	
	for(int i=0; i<means.size(); i++) {
		SimpleMatrix c = covs.get(i);
		SimpleMatrix invC = c.invert();
		SimpleMatrix m = means.get(i);
		int lenM1 = m.numRows()-lenCond;
		SimpleMatrix m1 = new SimpleMatrix(lenM1,1);
		SimpleMatrix m2 = new SimpleMatrix(lenCond,1);
		
		// extract all elements from inverse covariance that correspond only to m1
		// that means extract the block in the right bottom corner with height=width=lenM1
		SimpleMatrix newC1 = new SimpleMatrix(lenM1,lenM1);
		for(int j=0; j<lenM1; j++) {
			for(int k=0; k<lenM1; k++) {
				newC1.set(j, k, invC.get(j+lenCond,k+lenCond) );
			}
		}
		// extract all elements from inverse covariance that correspond to m1 and m2
		// from the the block in the left bottom corner with height=width=lenM1
		SimpleMatrix newC2 = new SimpleMatrix(lenM1,lenCond);
		for(int j=0; j<lenM1; j++) {
			for(int k=0; k<lenCond; k++) {
				newC2.set(j, k, invC.get(j+lenCond,k) );
			}
		}		
		
		//extract first rows from mean to m2
		for(int j=0; j<lenCond; j++) {
			m2.set(j,0,m.get(j,0));
		}
		//extract last rows from mean to m1
		for(int j=0; j<lenM1; j++) {
			m1.set(j,0,m.get(j+lenCond,0));
		}
		SimpleMatrix invNewC1 = newC1.invert();
		// calculate new mean and new covariance of conditional distribution
		SimpleMatrix condMean = m1.minus( invNewC1.mult(newC2).mult( condition.minus(m2) ) );
		SimpleMatrix condCovariance = invNewC1;
		conditionalMeans.add(condMean);
		conditionalCovs.add(condCovariance);
		
		// calculate new weights
		
		// extract all elements from inverse covariance that correspond only to m2
		// that means extract the block in the left top corner with height=width=lenCond
		SimpleMatrix newC22 = new SimpleMatrix(lenCond,lenCond);
		for(int j=0; j<lenCond; j++) {
			for(int k=0; k<lenCond; k++) {
				newC22.set(j, k, c.get(j,k) );
			}
		}
		double mahalanobisDistance = condition.minus(m2).transpose().mult(newC22.invert()).mult(condition.minus(m2)).trace();
		double newWeight = ((1 / (a * Math.sqrt(newC22.determinant()))) * Math.exp((-0.5d) * mahalanobisDistance))* weights.get(i);
		conditionalWeights.add(newWeight);
	}
	// normalize weights
	double weightSum = 0;
	for(int i=0; i<conditionalWeights.size(); i++) {
		weightSum += conditionalWeights.get(i);
	}
	for(int i=0; i<conditionalWeights.size(); i++) {
		double weight = conditionalWeights.get(i);
		weight = weight /weightSum;
		conditionalWeights.set(i,weight);
	}
	result = new ConditionalDistribution(conditionalMeans, conditionalCovs, conditionalWeights);
	return result;
}
 

/**
 * Find Maximum by gradient-quadratic search.
 * First a conditional distribution is derived from the kde.
 * @param start
 * @return
 */
public SearchResult gradQuadrSearch(SimpleMatrix start){
	
	
	SimpleMatrix condVector = new SimpleMatrix(4,1);
	for(int i=0; i<condVector.numRows(); i++){
		condVector.set(i,0,start.get(i,0));
	}
	ConditionalDistribution conditionalDist = getConditionalDistribution(condVector);
	
	ArrayList<SimpleMatrix> means = conditionalDist.conditionalMeans;
	ArrayList<SimpleMatrix> covs = conditionalDist.conditionalCovs;
	ArrayList<Double> weights = conditionalDist.conditionalWeights;

	SimpleMatrix gradient = new SimpleMatrix(2,1);
	SimpleMatrix hessian = new SimpleMatrix(2,2);
	double n = means.get(0).numRows();
	double a = Math.pow(Math.sqrt(2 * Math.PI), n);
	
	SimpleMatrix x = new SimpleMatrix(2,1);
	x.set(0,0,start.get(start.numRows()-2,0));
	x.set(1,0,start.get(start.numRows()-1,0));
	ArrayList<Double> mahalanobisDistances;
	double step = 1;
	double probability = 0;
	SimpleMatrix gradStep = null;
	do {
		mahalanobisDistances = mahalanobis(x, means, covs);
		//calculate gradient and hessian:
		double prob = 0;
		for (int i = 0; i < means.size(); i++) {
			// check wether the component actually contributes to to the density at given point 
			if(mahalanobisDistances.get(i) < MAX_MAHALANOBIS_DIST) {
				SimpleMatrix m = means.get(i);

				SimpleMatrix dm = m.minus(x);
				SimpleMatrix c = covs.get(i);

				
				SimpleMatrix invC = c.invert();
				double w = weights.get(i);
				//probability p(x,m)
				double p = ((1 / (a * Math.sqrt(c.determinant()))) * Math.exp((-0.5d) * mahalanobisDistances.get(i))) * w;
				prob += p; 
				gradient = gradient.plus( invC.mult(dm).scale(p) );
				hessian = hessian.plus( invC.mult( dm.mult(dm.transpose()).minus(c) ).mult(invC).scale(p) );
			}


		}
		// save x
		SimpleMatrix xOld = new SimpleMatrix(x);
		SimpleEVD<?> hessianEVD = hessian.eig();
		int maxEVIndex = hessianEVD.getIndexMax();
		if(hessianEVD.getEigenvalue(maxEVIndex).getReal() < 0){
			gradStep = hessian.invert().mult(gradient);
			x = xOld.minus(gradStep);
		}
		double prob1 = 	evaluate(x, means, covs, weights);
		if( prob1 <= prob | hessianEVD.getEigenvalue(maxEVIndex).getReal() >= 0) {
			gradStep = gradient.scale(step);
			x = xOld.plus(gradStep);
			while(evaluate(x, means, covs, weights) < prob){
				step = step/2;
				gradStep = gradient.scale(step);
				x = xOld.plus(gradStep);
			}
		}
		probability =	evaluate(x, means, covs, weights); 
	}while(gradStep.elementMaxAbs() > 1E-10);
	
	return new SearchResult(x, probability);
}
 

/**
 * Runs LSTDQ on this object's current {@link SARSData} dataset.
 * @return the new weight matrix as a {@link SimpleMatrix} object.
 */
public SimpleMatrix LSTDQ(){
	
	//set our policy
	Policy p = new GreedyQPolicy(this);
	
	//first we want to get all the features for all of our states in our data set; this is important if our feature database generates new features on the fly
	List<SSFeatures> features = new ArrayList<LSPI.SSFeatures>(this.dataset.size());
	int nf = 0;
	for(SARS sars : this.dataset.dataset){
		SSFeatures transitionFeatures = new SSFeatures(this.saFeatures.features(sars.s, sars.a), this.saFeatures.features(sars.sp, p.action(sars.sp)));
		features.add(transitionFeatures);
		nf = Math.max(nf, transitionFeatures.sActionFeatures.length);
	}

	SimpleMatrix B = SimpleMatrix.identity(nf).scale(this.identityScalar);
	SimpleMatrix b = new SimpleMatrix(nf, 1);
	
	
	
	for(int i = 0; i < features.size(); i++){

		SimpleMatrix phi = this.phiConstructor(features.get(i).sActionFeatures, nf);
		SimpleMatrix phiPrime = this.phiConstructor(features.get(i).sPrimeActionFeatures, nf);
		double r = this.dataset.get(i).r;
		

		SimpleMatrix numerator = B.mult(phi).mult(phi.minus(phiPrime.scale(gamma)).transpose()).mult(B);
		SimpleMatrix denomenatorM = phi.minus(phiPrime.scale(this.gamma)).transpose().mult(B).mult(phi);
		double denomenator = denomenatorM.get(0) + 1;
		
		B = B.minus(numerator.scale(1./denomenator));
		b = b.plus(phi.scale(r));
		
		//DPrint.cl(0, "updated matrix for row " + i + "/" + features.size());
		
	}
	
	
	SimpleMatrix w = B.mult(b);
	
	this.vfa = this.vfa.copy();
	for(int i = 0; i < nf; i++){
		this.vfa.setParameter(i, w.get(i, 0));
	}
	
	return w;
	
	
}