EMModel.java

Go to the documentation of this file.

package edu.wpi.rail.jinteractiveworld.model;

import edu.wpi.rail.jinteractiveworld.data.*;
import edu.wpi.rail.jinteractiveworld.ros.msgs.interactiveworldmsgs.*;
import edu.wpi.rail.jrosbridge.messages.geometry.Point;
import org.ejml.simple.SimpleMatrix;
import weka.clusterers.EM;
import weka.core.*;
import weka.core.Instances;
import weka.filters.Filter;
import weka.filters.unsupervised.attribute.Remove;
import java.util.ArrayList;
import java.util.List;

public class EMModel implements Model {

        public enum RankingFunction {
                CUSTOM, SCATTER_SEPARABILITY
        }

        private EM em;
        private Placement best;
        private double decisionValue, sigmaX, sigmaY, sigmaZ, sigmaTheta;
        private DataSet data;
        private RankingFunction rankingType;

        public EMModel(DataSet data, RankingFunction rankingType) {
                this.data = data;
                this.rankingType = rankingType;
                this.decisionValue = Double.POSITIVE_INFINITY;
                this.train();
        }

        @Override
        public void add(DataPoint point) {
                this.data.add(point);
                // retrain
                this.train();
        }

        public int size() {
                return this.data.size();
        }

        @Override
        public List<DataPoint> getData() {
                // copy into an array list
                ArrayList<DataPoint> list = new ArrayList<DataPoint>();
                for (int i = 0; i < this.data.size(); i++) {
                        list.add(this.data.get(i));
                }
                return list;
        }

        @Override
        public String getReferenceFrame() {
                return this.data.getReferenceFrame();
        }

        @Override
        public Item getItem() {
                return this.data.getItem();
        }

        @Override
        public Room getRoom() {
                return this.data.getRoom();
        }

        @Override
        public Surface getSurface() {
                return this.data.getSurface();
        }

        @Override
        public void train() {
                try {
                        this.em = new EM();
                        this.best = null;
                        this.decisionValue = Double.POSITIVE_INFINITY;
                        this.sigmaX = 0;
                        this.sigmaY = 0;
                        this.sigmaZ = 0;
                        this.sigmaTheta = 0;

                        // get the instances
                        Instances instances = this.data.toInstances();

                        // remove z for now
                        Remove rm = new Remove();
                        rm.setAttributeIndicesArray(new int[] { DataSet.Z_ATTRIBUTE.index() });
                        rm.setInputFormat(instances);
                        Instances newInstances = Filter.useFilter(instances, rm);

                        // run EM
                        this.em.buildClusterer(newInstances);
                        // get the results
                        double clusterData[][][] = this.em.getClusterModelsNumericAtts();
                        double[] priors = this.em.clusterPriors();


                        if (this.rankingType.equals(RankingFunction.CUSTOM)) {

                                // cluster each point
                                @SuppressWarnings("unchecked")
                                ArrayList<Instance>[] clusters = (ArrayList<Instance>[]) new ArrayList[clusterData.length];
                                for (int i = 0; i < clusters.length; i++) {
                                        clusters[i] = new ArrayList<Instance>();
                                }
                                for (int i = 0; i < newInstances.numInstances(); i++) {
                                        Instance inst = newInstances.instance(i);
                                        int clust = this.em.clusterInstance(inst);
                                        clusters[clust].add(inst);
                                }

                                // rank the clusters
                                for (int m = 0; m < clusters.length; m++) {
                                        double distance = 0;
                                        ArrayList<Instance> curInstances = clusters[m];
                                        for (int i = 0; i < curInstances.size(); i++) {
                                                Instance instI = curInstances.get(i);
                                                for (int j = 0; j < curInstances.size(); j++) {
                                                        if (i != j) {
                                                                Instance instJ = curInstances.get(j);
                                                                // get each attribute
                                                                double sum = 0;
                                                                for (int k = 0; k < instI.numAttributes(); k++) {
                                                                        sum += Math.pow(
                                                                                        instI.value(k) - instJ.value(k), 2.0);
                                                                }
                                                                // get the distance
                                                                distance += Math.sqrt(sum);
                                                        }
                                                }
                                        }
                                        // average
                                        double ranking = distance
                                                        / (curInstances.size() * (curInstances.size() - 1)) * 1.0
                                                        / (((double) curInstances.size()) / ((double) this.size()));

                                        // check for a new best
                                        if (ranking < this.decisionValue) {
                                                this.decisionValue = ranking;
                                                double x = clusterData[m][0][0];
                                                this.sigmaX = clusterData[m][0][1];
                                                double y = clusterData[m][1][0];
                                                this.sigmaY = clusterData[m][1][1];
                                                double z = 0.0;
                                                this.sigmaZ = 0.0;
                                                double theta = clusterData[m][2][0];
                                                this.sigmaTheta = clusterData[m][2][1];
                                                this.best = new Placement(this.getItem(), this.getRoom(), this.getSurface(), this.getReferenceFrame(), new Point(x, y, z), theta);
                                        }
                                }
                        } else if (this.rankingType.equals(RankingFunction.SCATTER_SEPARABILITY)) {
                                int n = newInstances.numAttributes();
                                // priors
//                              SimpleMatrix sw = new SimpleMatrix(n, n);
//                              SimpleMatrix mo = new SimpleMatrix(n, 1);
//                              SimpleMatrix sb = new SimpleMatrix(n, n);

                                // go through each cluster for sw and mo
                                for (int i = 0; i < clusterData.length; i++) {
                                        // covariance matrix
                                        double[][] covArray = new double[n][n];
                                        // mu for the cluster
//                                      double [][] muArray = new double[n][1];
                                        for (int j = 0; j < n; j++) {
                                                covArray[j][j] = clusterData[i][j][1];
//                                              muArray[j][0] = clusterData[i][j][0];
                                        }
                                        SimpleMatrix cov = new SimpleMatrix(covArray);
                                        SimpleMatrix covScale = cov.scale(priors[i]);
                                        double trace = covScale.trace();
                                        if (trace < this.decisionValue) {
                                                this.decisionValue = trace;
                                                double x = clusterData[i][0][0];
                                                this.sigmaX = clusterData[i][0][1];
                                                double y = clusterData[i][1][0];
                                                this.sigmaY = clusterData[i][1][1];
                                                double z = 0.0;
                                                this.sigmaZ = 0.0;
                                                double theta = clusterData[i][2][0];
                                                this.sigmaTheta = clusterData[i][2][1];
                                                this.best = new Placement(this.getItem(), this.getRoom(), this.getSurface(), this.getReferenceFrame(), new Point(x, y, z), theta);
                                        }
//                                      SimpleMatrix mu = new SimpleMatrix(muArray);
//                                      SimpleMatrix muScale = mu.scale(priors[i]);
//                                      mo = mo.plus(muScale);
                                }

//                              // go through each cluster for sb
//                              for (int i = 0; i < clusterData.length; i++) {
//                                      // mu for the cluster
//                                      double [][] muArray = new double[n][1];
//                                      for (int j = 0; j < n; j++) {
//                                              muArray[j][0] = clusterData[i][j][0];
//                                      }
//                                      SimpleMatrix mu = new SimpleMatrix(muArray);
//                                      SimpleMatrix meanDiff = mu.minus(mo);
//                                      SimpleMatrix meanDiffTrans = meanDiff.transpose();
//                                      SimpleMatrix product = meanDiff.mult(meanDiffTrans);
//                                      SimpleMatrix scale = product.scale(priors[i]);
//                                      sb = sb.plus(scale);
//                              }

                                // trace as the value
//                              this.decisionValue = sw.invert().mult(sb).trace();
                                //this.decisionValue = sw.trace();
                                System.out.println(this.decisionValue); // + " -- " + sw.invert().trace());
                        }

                } catch (Exception e) {
                        System.err.println("[ERROR]: Could not train model: "
                                        + e.getMessage());
                        e.printStackTrace();
                }
        }

        @Override
        public Placement getPlacementLocation() {
                return this.best;
        }

        @Override
        public double getDecisionValue() {
                return this.decisionValue;
        }

        @Override
        public double getSigmaX() {
                return this.sigmaX;
        }

        @Override
        public double getSigmaY() {
                return this.sigmaY;
        }

        @Override
        public double getSigmaZ() {
                return this.sigmaZ;
        }

        @Override
        public double getSigmaTheta() {
                return this.sigmaTheta;
        }
}