diff --git a/GVF/GVF.cpp b/GVF/GVF.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..36ef7d53af50be299a2295bd55d14d3585ea0ea1
--- /dev/null
+++ b/GVF/GVF.cpp
@@ -0,0 +1,1375 @@
+/**
+ * Gesture Variation Follower class allows for early gesture recognition and variation tracking
+ *
+ * @details Original algorithm designed and implemented in 2011 at Ircam Centre Pompidou
+ * by Baptiste Caramiaux and Nicola Montecchio. The library has been created and is maintained by Baptiste Caramiaux
+ *
+ * Copyright (C) 2015 Baptiste Caramiaux, Nicola Montecchio
+ * STMS lab Ircam-CRNS-UPMC, University of Padova, Goldsmiths College University of London
+ *
+ * The library is under the GNU Lesser General Public License (LGPL v3)
+ */
+
+#include "GVF.h"
+#include <string.h>
+#include <stdio.h>
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <memory>
+#include <algorithm>
+#include <numeric>
+
+//debug max
+//#include "ext.h"
+
+
+using namespace std;
+
+//--------------------------------------------------------------
+GVF::GVF()
+{
+ config.inputDimensions = 2;
+ config.translate = true;
+ config.segmentation = false;
+
+ parameters.numberParticles = 1000;
+ parameters.tolerance = 0.2f;
+ parameters.resamplingThreshold = 250;
+ parameters.distribution = 0.0f;
+ parameters.alignmentVariance = sqrt(0.000001f);
+ parameters.dynamicsVariance = vector<float>(1,sqrt(0.001f));
+ parameters.scalingsVariance = vector<float>(1,sqrt(0.00001f));
+ parameters.rotationsVariance = vector<float>(1,sqrt(0.0f));
+ parameters.predictionSteps = 1;
+ parameters.dimWeights = vector<float>(1,sqrt(1.0f));
+ parameters.alignmentSpreadingCenter = 0.0;
+ parameters.alignmentSpreadingRange = 0.2;
+ parameters.dynamicsSpreadingCenter = 1.0;
+ parameters.dynamicsSpreadingRange = 0.3;
+ parameters.scalingsSpreadingCenter = 1.0;
+ parameters.scalingsSpreadingRange = 0.3;
+ parameters.rotationsSpreadingCenter = 0.0;
+ parameters.rotationsSpreadingRange = 0.5;
+
+ tolerancesetmanually = false;
+ learningGesture = -1;
+
+ normgen = std::mt19937(rd());
+ rndnorm = new std::normal_distribution<float>(0.0,1.0);
+ unifgen = std::default_random_engine(rd());
+ rndunif = new std::uniform_real_distribution<float>(0.0,1.0);
+
+}
+
+////--------------------------------------------------------------
+//GVF::GVF(GVFConfig _config){
+// setup(_config);
+//}
+//
+////--------------------------------------------------------------
+//GVF::GVF(GVFConfig _config, GVFParameters _parameters){
+// setup(_config, _parameters);
+//}
+//
+////--------------------------------------------------------------
+//void GVF::setup(){
+//
+// // use defualt parameters
+// GVFConfig defaultConfig;
+//
+// defaultConfig.inputDimensions = 2;
+// defaultConfig.translate = true;
+// defaultConfig.segmentation = false;
+//
+// setup(defaultConfig);
+//}
+//
+////--------------------------------------------------------------
+//void GVF::setup(GVFConfig _config){
+//
+// clear(); // just in case
+//
+// learningGesture = -1;
+//
+// // Set configuration:
+// config = _config;
+//
+// // default parameters
+// GVFParameters defaultParameters;
+// defaultParameters.numberParticles = 1000;
+// defaultParameters.tolerance = 0.2f;
+// defaultParameters.resamplingThreshold = 250;
+// defaultParameters.distribution = 0.0f;
+// defaultParameters.alignmentVariance = sqrt(0.000001f);
+// defaultParameters.dynamicsVariance = vector<float>(1,sqrt(0.001f));
+// defaultParameters.scalingsVariance = vector<float>(1,sqrt(0.00001f));
+// defaultParameters.rotationsVariance = vector<float>(1,sqrt(0.0f));
+// defaultParameters.predictionSteps = 1;
+// defaultParameters.dimWeights = vector<float>(1,sqrt(1.0f));
+//
+// // default spreading
+// defaultParameters.alignmentSpreadingCenter = 0.0;
+// defaultParameters.alignmentSpreadingRange = 0.2;
+//
+// defaultParameters.dynamicsSpreadingCenter = 1.0;
+// defaultParameters.dynamicsSpreadingRange = 0.3;
+//
+// defaultParameters.scalingsSpreadingCenter = 1.0;
+// defaultParameters.scalingsSpreadingRange = 0.3;
+//
+// defaultParameters.rotationsSpreadingCenter = 0.0;
+// defaultParameters.rotationsSpreadingRange = 0.0;
+//
+// tolerancesetmanually = false;
+//
+// setup(_config, defaultParameters);
+//
+//}
+//
+////--------------------------------------------------------------
+//void GVF::setup(GVFConfig _config, GVFParameters _parameters)
+//{
+// clear(); // just in case
+// // Set configuration and parameters
+// config = _config;
+// parameters = _parameters;
+// // Init random generators
+// normgen = std::mt19937(rd());
+// rndnorm = new std::normal_distribution<float>(0.0,1.0);
+// unifgen = std::default_random_engine(rd());
+// rndunif = new std::uniform_real_distribution<float>(0.0,1.0);
+//}
+
+//--------------------------------------------------------------
+GVF::~GVF()
+{
+ if (rndnorm != NULL)
+ delete (rndnorm);
+ clear(); // not really necessary but it's polite ;)
+}
+
+//--------------------------------------------------------------
+void GVF::clear()
+{
+ state = STATE_CLEAR;
+ gestureTemplates.clear();
+ mostProbableIndex = -1;
+}
+
+//--------------------------------------------------------------
+void GVF::startGesture()
+{
+ if (state==STATE_FOLLOWING)
+ {
+ restart();
+ }
+ else if (state==STATE_LEARNING)
+ {
+ if (theGesture.getNumberOfTemplates()>0)
+ {
+ if (theGesture.getTemplateLength()>0)
+ addGestureTemplate(theGesture);
+ }
+ theGesture.clear();
+ }
+}
+
+//--------------------------------------------------------------
+void GVF::addObservation(vector<float> data)
+{
+ theGesture.addObservation(data);
+}
+
+//--------------------------------------------------------------
+void GVF::addGestureTemplate(GVFGesture & gestureTemplate)
+{
+
+ // if (getState() != GVF::STATE_LEARNING)
+ // setState(GVF::STATE_LEARNING);
+
+ int inputDimension = gestureTemplate.getNumberDimensions();
+ config.inputDimensions = inputDimension;
+
+ gestureTemplates.push_back(gestureTemplate);
+ activeGestures.push_back(gestureTemplates.size());
+
+ if(minRange.size() == 0){
+ minRange.resize(inputDimension);
+ maxRange.resize(inputDimension);
+ }
+
+ for(int j = 0; j < inputDimension; j++){
+ minRange[j] = INFINITY;
+ maxRange[j] = -INFINITY;
+ }
+
+ // compute min/max from the data
+ for(int i = 0; i < gestureTemplates.size(); i++){
+ GVFGesture& tGestureTemplate = gestureTemplates[i];
+ vector<float>& tMinRange = tGestureTemplate.getMinRange();
+ vector<float>& tMaxRange = tGestureTemplate.getMaxRange();
+ for(int j = 0; j < inputDimension; j++){
+ if(tMinRange[j] < minRange[j]) minRange[j] = tMinRange[j];
+ if(tMaxRange[j] > maxRange[j]) maxRange[j] = tMaxRange[j];
+ }
+ }
+
+ for(int i = 0; i < gestureTemplates.size(); i++){
+ GVFGesture& tGestureTemplate = gestureTemplates[i];
+ tGestureTemplate.setMinRange(minRange);
+ tGestureTemplate.setMaxRange(maxRange);
+ }
+ train();
+
+}
+
+//--------------------------------------------------------------
+void GVF::replaceGestureTemplate(GVFGesture & gestureTemplate, int index)
+{
+ if(gestureTemplate.getNumberDimensions()!=config.inputDimensions)
+ return;
+ if(minRange.size() == 0)
+ {
+ minRange.resize(config.inputDimensions);
+ maxRange.resize(config.inputDimensions);
+ }
+ for(int j = 0; j < config.inputDimensions; j++)
+ {
+ minRange[j] = INFINITY;
+ maxRange[j] = -INFINITY;
+ }
+ if (index<=gestureTemplates.size())
+ gestureTemplates[index-1]=gestureTemplate;
+ for(int i = 0; i < gestureTemplates.size(); i++)
+ {
+ GVFGesture& tGestureTemplate = gestureTemplates[i];
+ vector<float>& tMinRange = tGestureTemplate.getMinRange();
+ vector<float>& tMaxRange = tGestureTemplate.getMaxRange();
+ for(int j = 0; j < config.inputDimensions; j++){
+ if(tMinRange[j] < minRange[j]) minRange[j] = tMinRange[j];
+ if(tMaxRange[j] > maxRange[j]) maxRange[j] = tMaxRange[j];
+ }
+ }
+ for(int i = 0; i < gestureTemplates.size(); i++)
+ {
+ GVFGesture& tGestureTemplate = gestureTemplates[i];
+ tGestureTemplate.setMinRange(minRange);
+ tGestureTemplate.setMaxRange(maxRange);
+ }
+}
+
+////--------------------------------------------------------------
+//vector<float>& GVF::getGestureTemplateSample(int gestureIndex, float cursor)
+//{
+// int frameindex = min((int)(gestureTemplates[gestureIndex].getTemplateLength() - 1),
+// (int)(floor(cursor * gestureTemplates[gestureIndex].getTemplateLength() ) ) );
+// return gestureTemplates[gestureIndex].getTemplate()[frameindex];
+//}
+
+//--------------------------------------------------------------
+GVFGesture & GVF::getGestureTemplate(int index){
+ assert(index < gestureTemplates.size());
+ return gestureTemplates[index];
+}
+
+//--------------------------------------------------------------
+vector<GVFGesture> & GVF::getAllGestureTemplates(){
+ return gestureTemplates;
+}
+
+//--------------------------------------------------------------
+int GVF::getNumberOfGestureTemplates(){
+ return (int)gestureTemplates.size();
+}
+
+//--------------------------------------------------------------
+void GVF::removeGestureTemplate(int index){
+ assert(index < gestureTemplates.size());
+ gestureTemplates.erase(gestureTemplates.begin() + index);
+}
+
+//--------------------------------------------------------------
+void GVF::removeAllGestureTemplates(){
+ gestureTemplates.clear();
+}
+
+//----------------------------------------------
+void GVF::train(){
+
+ if (gestureTemplates.size() > 0)
+ {
+
+ // get the number of dimension in templates
+ config.inputDimensions = gestureTemplates[0].getTemplateDimension();
+
+ dynamicsDim = 2; // hard coded: just speed now
+ scalingsDim = config.inputDimensions;
+
+ // manage orientation
+ if (config.inputDimensions==2) rotationsDim=1;
+ else if (config.inputDimensions==3) rotationsDim=3;
+ else rotationsDim=0;
+
+ // Init state space
+ initVec(classes, parameters.numberParticles); // Vector of gesture class
+ initVec(alignment, parameters.numberParticles); // Vector of phase values (alignment)
+ initMat(dynamics, parameters.numberParticles, dynamicsDim); // Matric of dynamics
+ initMat(scalings, parameters.numberParticles, scalingsDim); // Matrix of scaling
+ if (rotationsDim!=0) initMat(rotations, parameters.numberParticles, rotationsDim); // Matrix of rotations
+ initMat(offsets, parameters.numberParticles, config.inputDimensions);
+ initVec(weights, parameters.numberParticles); // Weights
+
+ initMat(particles, parameters.numberParticles, 3);
+ // std::cout << particles.size() << " " << parameters.numberParticles << std::endl;
+
+ // bayesian elements
+ initVec(prior, parameters.numberParticles);
+ initVec(posterior, parameters.numberParticles);
+ initVec(likelihood, parameters.numberParticles);
+
+
+ initPrior(); // prior on init state values
+ initNoiseParameters(); // init noise parameters (transition and likelihood)
+
+
+ // weighted dimensions in case: default is not weighted
+ if (parameters.dimWeights.size()!=config.inputDimensions){
+ parameters.dimWeights = vector<float> (config.inputDimensions);
+ for(int k = 0; k < config.inputDimensions; k++) parameters.dimWeights[k] = 1.0 / config.inputDimensions;
+ }
+
+ // NORMALIZATION
+// if (config.normalization) { // update the global normaliation factor
+// globalNormalizationFactor = -1.0;
+// // loop on previous gestures already learned
+// // take the max of all the gesture learned ...
+// for (int k=0; k<getNumberOfGestureTemplates() ; k++){
+// for(int j = 0; j < config.inputDimensions; j++){
+// float rangetmp = fabs(getGestureTemplate(k).getMaxRange()[j]-getGestureTemplate(k).getMinRange()[j]);
+// if (rangetmp > globalNormalizationFactor)
+// globalNormalizationFactor=rangetmp;
+// }
+// }
+// }
+// // only for logs
+// if (config.logOn) {
+// vecRef = vector<vector<float> > (parameters.numberParticles);
+// vecObs = vector<float> (config.inputDimensions);
+// stateNoiseDist = vector<float> (parameters.numberParticles);
+// }
+ }
+}
+
+//--------------------------------------------------------------
+//void GVF::initPrior()
+//{
+//
+// // PATICLE FILTERING
+// for (int k = 0; k < parameters.numberParticles; k++)
+// {
+// initPrior(k);
+//
+// classes[k] = activeGestures[k % activeGestures.size()] - 1;
+// }
+//
+//}
+
+//--------------------------------------------------------------
+void GVF::initPrior() //int pf_n)
+{
+ for (int pf_n = 0; pf_n < parameters.numberParticles; pf_n++)
+ {
+ // alignment
+ alignment[pf_n] = ((*rndunif)(unifgen) - 0.5) * parameters.alignmentSpreadingRange + parameters.alignmentSpreadingCenter; // spread phase
+
+
+ // dynamics
+ dynamics[pf_n][0] = ((*rndunif)(unifgen) - 0.5) * parameters.dynamicsSpreadingRange + parameters.dynamicsSpreadingCenter; // spread speed
+ if (dynamics[pf_n].size()>1)
+ {
+ dynamics[pf_n][1] = ((*rndunif)(unifgen) - 0.5) * parameters.dynamicsSpreadingRange; // spread accel
+ }
+
+ // scalings
+ for(int l = 0; l < scalings[pf_n].size(); l++) {
+ scalings[pf_n][l] = ((*rndunif)(unifgen) - 0.5) * parameters.scalingsSpreadingRange + parameters.scalingsSpreadingCenter; // spread scalings
+ }
+
+ // rotations
+ if (rotationsDim!=0)
+ for(int l = 0; l < rotations[pf_n].size(); l++)
+ rotations[pf_n][l] = ((*rndunif)(unifgen) - 0.5) * parameters.rotationsSpreadingRange + parameters.rotationsSpreadingCenter; // spread rotations
+
+ if (config.translate) for(int l = 0; l < offsets[pf_n].size(); l++) offsets[pf_n][l] = 0.0;
+
+
+ prior[pf_n] = 1.0 / (float) parameters.numberParticles;
+
+ // set the posterior to the prior at the initialization
+ posterior[pf_n] = prior[pf_n];
+
+ classes[pf_n] = activeGestures[pf_n % activeGestures.size()] - 1;
+ }
+
+}
+
+//--------------------------------------------------------------
+void GVF::initNoiseParameters() {
+
+ // NOISE (ADDITIVE GAUSSIAN NOISE)
+ // ---------------------------
+
+ if (parameters.dynamicsVariance.size() != dynamicsDim)
+ {
+ float variance = parameters.dynamicsVariance[0];
+ parameters.dynamicsVariance.resize(dynamicsDim);
+ for (int k=0; k<dynamicsDim; k++)
+ parameters.dynamicsVariance[k] = variance;
+ }
+
+ if (parameters.scalingsVariance.size() != scalingsDim)
+ {
+ float variance = parameters.scalingsVariance[0];
+ parameters.scalingsVariance.resize(scalingsDim);
+ for (int k=0; k<scalingsDim; k++)
+ parameters.scalingsVariance[k] = variance;
+ }
+
+ if (rotationsDim!=0)
+ {
+ if (parameters.rotationsVariance.size() != rotationsDim)
+ {
+ float variance = parameters.rotationsVariance[0];
+ parameters.rotationsVariance.resize(rotationsDim);
+ for (int k=0; k<rotationsDim; k++)
+ parameters.rotationsVariance[k] = variance;
+ }
+ }
+
+ // ADAPTATION OF THE TOLERANCE IF DEFAULT PARAMTERS
+ // ---------------------------
+ if (!tolerancesetmanually){
+ float obsMeanRange = 0.0f;
+ for (int gt=0; gt<gestureTemplates.size(); gt++) {
+ for (int d=0; d<config.inputDimensions; d++)
+ obsMeanRange += (gestureTemplates[gt].getMaxRange()[d] - gestureTemplates[gt].getMinRange()[d])
+ /config.inputDimensions;
+ }
+ obsMeanRange /= gestureTemplates.size();
+ parameters.tolerance = obsMeanRange / 4.0f; // dividing by an heuristic factor [to be learned?]
+ }
+}
+
+//--------------------------------------------------------------
+void GVF::setState(GVFState _state, vector<int> indexes)
+{
+ switch (_state)
+ {
+ case STATE_CLEAR:
+ clear();
+ theGesture.clear();
+ break;
+
+ case STATE_LEARNING:
+ if ((state==STATE_LEARNING) && (theGesture.getNumberOfTemplates()>0))
+ {
+ if (learningGesture==-1)
+ addGestureTemplate(theGesture);
+ else
+ {
+ replaceGestureTemplate(theGesture, learningGesture);
+ learningGesture=-1;
+ }
+ if (indexes.size()!=0)
+ learningGesture=indexes[0];
+ }
+ state = _state;
+ theGesture.clear();
+ break;
+
+ case STATE_FOLLOWING:
+ if ((state==STATE_LEARNING) && (theGesture.getNumberOfTemplates()>0))
+ {
+ if (learningGesture==-1)
+ addGestureTemplate(theGesture);
+ else
+ {
+ replaceGestureTemplate(theGesture, learningGesture);
+ learningGesture=-1;
+ }
+ }
+ if (gestureTemplates.size() > 0)
+ {
+ train();
+ state = _state;
+ }
+ else
+ state = STATE_CLEAR;
+ theGesture.clear();
+ break;
+
+ default:
+ theGesture.clear();
+ break;
+ }
+}
+
+//--------------------------------------------------------------
+GVF::GVFState GVF::getState()
+{
+ return state;
+}
+
+////--------------------------------------------------------------
+//int GVF::getDynamicsDimension(){
+// return dynamicsDim;
+//}
+
+//--------------------------------------------------------------
+vector<int> GVF::getGestureClasses()
+{
+ return classes;
+}
+
+////--------------------------------------------------------------
+//vector<float> GVF::getAlignment(){
+// return alignment;
+//}
+//
+////--------------------------------------------------------------
+//vector<float> GVF::getEstimatedAlignment(){
+// return estimatedAlignment;
+//}
+//
+////--------------------------------------------------------------
+//vector< vector<float> > GVF::getDynamics(){
+// return dynamics;
+//}
+//
+////--------------------------------------------------------------
+//vector< vector<float> > GVF::getEstimatedDynamics(){
+// return estimatedDynamics;
+//}
+//
+////--------------------------------------------------------------
+//vector< vector<float> > GVF::getScalings(){
+// return scalings;
+//}
+//
+////--------------------------------------------------------------
+//vector< vector<float> > GVF::getEstimatedScalings(){
+// return estimatedScalings;
+//}
+//
+////--------------------------------------------------------------
+//vector< vector<float> > GVF::getRotations(){
+// return rotations;
+//}
+//
+////--------------------------------------------------------------
+//vector< vector<float> > GVF::getEstimatedRotations(){
+// return estimatedRotations;
+//}
+
+////--------------------------------------------------------------
+//vector<float> GVF::getEstimatedProbabilities(){
+// return estimatedProbabilities;
+//}
+//
+////--------------------------------------------------------------
+//vector<float> GVF::getEstimatedLikelihoods(){
+// return estimatedLikelihoods;
+//}
+//
+////--------------------------------------------------------------
+//vector<float> GVF::getWeights(){
+// return weights;
+//}
+//
+////--------------------------------------------------------------
+//vector<float> GVF::getPrior(){
+// return prior;
+//}
+
+////--------------------------------------------------------------
+//vector<vector<float> > GVF::getVecRef() {
+// return vecRef;
+//}
+//
+////--------------------------------------------------------------
+//vector<float> GVF::getVecObs() {
+// return vecObs;
+//}
+//
+////--------------------------------------------------------------
+//vector<float> GVF::getStateNoiseDist(){
+// return stateNoiseDist;
+//}
+
+////--------------------------------------------------------------
+//int GVF::getScalingsDim(){
+// return scalingsDim;
+//}
+//
+////--------------------------------------------------------------
+//int GVF::getRotationsDim(){
+// return rotationsDim;
+//}
+
+//--------------------------------------------------------------
+void GVF::restart()
+{
+ theGesture.clear();
+ initPrior();
+}
+
+#pragma mark - PARTICLE FILTERING
+
+//--------------------------------------------------------------
+void GVF::updatePrior(int n) {
+
+ // Update alignment / dynamics / scalings
+ float L = gestureTemplates[classes[n]].getTemplateLength();
+ alignment[n] += (*rndnorm)(normgen) * parameters.alignmentVariance + dynamics[n][0]/L; // + dynamics[n][1]/(L*L);
+
+ if (dynamics[n].size()>1){
+ dynamics[n][0] += (*rndnorm)(normgen) * parameters.dynamicsVariance[0] + dynamics[n][1]/L;
+ dynamics[n][1] += (*rndnorm)(normgen) * parameters.dynamicsVariance[1];
+ }
+ else {
+ dynamics[n][0] += (*rndnorm)(normgen) * parameters.dynamicsVariance[0];
+ }
+
+ // for(int l= 0; l < dynamics[n].size(); l++) dynamics[n][l] += (*rndnorm)(normgen) * parameters.dynamicsVariance[l];
+ for(int l= 0; l < scalings[n].size(); l++) scalings[n][l] += (*rndnorm)(normgen) * parameters.scalingsVariance[l];
+ if (rotationsDim!=0) for(int l= 0; l < rotations[n].size(); l++) rotations[n][l] += (*rndnorm)(normgen) * parameters.rotationsVariance[l];
+
+ // update prior (bayesian incremental inference)
+ prior[n] = posterior[n];
+}
+
+//--------------------------------------------------------------
+void GVF::updateLikelihood(vector<float> obs, int n)
+{
+
+// if (config.normalization) for (int kk=0; kk<vobs.size(); kk++) vobs[kk] = vobs[kk] / globalNormalizationFactor;
+
+ if(alignment[n] < 0.0)
+ {
+ alignment[n] = fabs(alignment[n]); // re-spread at the beginning
+// if (config.segmentation)
+// classes[n] = n % getNumberOfGestureTemplates();
+ }
+ else if(alignment[n] > 1.0)
+ {
+ if (config.segmentation)
+ {
+// alignment[n] = fabs(1.0-alignment[n]); // re-spread at the beginning
+ alignment[n] = fabs((*rndunif)(unifgen) * 0.5); //
+ classes[n] = n % getNumberOfGestureTemplates();
+ offsets[n] = obs;
+ // dynamics
+ dynamics[n][0] = ((*rndunif)(unifgen) - 0.5) * parameters.dynamicsSpreadingRange + parameters.dynamicsSpreadingCenter; // spread speed
+ if (dynamics[n].size()>1)
+ dynamics[n][1] = ((*rndunif)(unifgen) - 0.5) * parameters.dynamicsSpreadingRange;
+ // scalings
+ for(int l = 0; l < scalings[n].size(); l++)
+ scalings[n][l] = ((*rndunif)(unifgen) - 0.5) * parameters.scalingsSpreadingRange + parameters.scalingsSpreadingCenter; // spread scalings
+ // rotations
+ if (rotationsDim!=0)
+ for(int l = 0; l < rotations[n].size(); l++)
+ rotations[n][l] = ((*rndunif)(unifgen) - 0.5) * parameters.rotationsSpreadingRange + parameters.rotationsSpreadingCenter; // spread rotations
+ // prior
+ prior[n] = 1/(float)parameters.numberParticles;
+ }
+ else{
+ alignment[n] = fabs(2.0-alignment[n]); // re-spread at the end
+ }
+ }
+
+ vector<float> vobs(config.inputDimensions);
+ setVec(vobs, obs);
+
+ if (config.translate)
+ for (int j=0; j < config.inputDimensions; j++)
+ vobs[j] = vobs[j] - offsets[n][j];
+
+
+ // take vref from template at the given alignment
+ int gestureIndex = classes[n];
+ float cursor = alignment[n];
+ int frameindex = min((int)(gestureTemplates[gestureIndex].getTemplateLength() - 1),
+ (int)(floor(cursor * gestureTemplates[gestureIndex].getTemplateLength() ) ) );
+// return gestureTemplates[gestureIndex].getTemplate()[frameindex];
+ vector<float> vref = gestureTemplates[gestureIndex].getTemplate()[frameindex];; //getGestureTemplateSample(classes[n], alignment[n]);
+
+ // Apply scaling coefficients
+ for (int k=0;k < config.inputDimensions; k++)
+ {
+// if (config.normalization) vref[k] = vref[k] / globalNormalizationFactor;
+ vref[k] *= scalings[n][k];
+ }
+
+ // Apply rotation coefficients
+ if (config.inputDimensions==2) {
+ float tmp0=vref[0]; float tmp1=vref[1];
+ vref[0] = cos(rotations[n][0])*tmp0 - sin(rotations[n][0])*tmp1;
+ vref[1] = sin(rotations[n][0])*tmp0 + cos(rotations[n][0])*tmp1;
+ }
+ else if (config.inputDimensions==3) {
+ // Rotate template sample according to the estimated angles of rotations (3d)
+ vector<vector< float> > RotMatrix = getRotationMatrix3d(rotations[n][0],rotations[n][1],rotations[n][2]);
+ vref = multiplyMat(RotMatrix, vref);
+ }
+
+ // weighted euclidean distance
+ float dist = distance_weightedEuclidean(vref,vobs,parameters.dimWeights);
+
+ if(parameters.distribution == 0.0f){ // Gaussian distribution
+ likelihood[n] = exp(- dist * 1 / (parameters.tolerance * parameters.tolerance));
+ }
+ else { // Student's distribution
+ likelihood[n] = pow(dist/parameters.distribution + 1, -parameters.distribution/2 - 1); // dimension is 2 .. pay attention if editing]
+ }
+// // if log on keep track on vref and vobs
+// if (config.logOn){
+// vecRef.push_back(vref);
+// vecObs = vobs;
+// }
+}
+
+//--------------------------------------------------------------
+void GVF::updatePosterior(int n) {
+ posterior[n] = prior[n] * likelihood[n];
+}
+
+//--------------------------------------------------------------
+GVFOutcomes & GVF::update(vector<float> & observation)
+{
+
+ if (state != GVF::STATE_FOLLOWING) setState(GVF::STATE_FOLLOWING);
+
+ theGesture.addObservation(observation);
+ vector<float> obs = theGesture.getLastObservation();
+
+ // std::cout << obs[0] << " " << obs[0] << " "
+ // << gestureTemplates[0].getTemplate()[20][0] << " " << gestureTemplates[0].getTemplate()[20][1] << " "
+ // << gestureTemplates[1].getTemplate()[20][0] << " " << gestureTemplates[1].getTemplate()[20][1] << std::endl;
+
+
+ // for each particle: perform updates of state space / likelihood / prior (weights)
+ float sumw = 0.0;
+ for(int n = 0; n< parameters.numberParticles; n++)
+ {
+
+ for (int m=0; m<parameters.predictionSteps; m++)
+ {
+ updatePrior(n);
+ updateLikelihood(obs, n);
+ updatePosterior(n);
+ }
+
+ sumw += posterior[n]; // sum posterior to normalise the distrib afterwards
+
+ particles[n][0] = alignment[n];
+ particles[n][1] = scalings[n][0];
+ particles[n][2] = classes[n];
+ }
+
+ // normalize the weights and compute the resampling criterion
+ float dotProdw = 0.0;
+ for (int k = 0; k < parameters.numberParticles; k++){
+ posterior[k] /= sumw;
+ dotProdw += posterior[k] * posterior[k];
+ }
+ // avoid degeneracy (no particles active, i.e. weight = 0) by resampling
+ if( (1./dotProdw) < parameters.resamplingThreshold)
+ resampleAccordingToWeights(obs);
+
+ // estimate outcomes
+ estimates();
+
+ return outcomes;
+
+}
+
+//--------------------------------------------------------------
+void GVF::resampleAccordingToWeights(vector<float> obs)
+{
+ // covennient
+ int numOfPart = parameters.numberParticles;
+
+ // cumulative dist
+ vector<float> c(numOfPart);
+
+ // tmp matrices
+ vector<int> oldClasses;
+ vector<float> oldAlignment;
+ vector< vector<float> > oldDynamics;
+ vector< vector<float> > oldScalings;
+ vector< vector<float> > oldRotations;
+
+ setVec(oldClasses, classes);
+ setVec(oldAlignment, alignment);
+ setMat(oldDynamics, dynamics);
+ setMat(oldScalings, scalings);
+ if (rotationsDim!=0) setMat(oldRotations, rotations);
+
+
+ c[0] = 0;
+ for(int i = 1; i < numOfPart; i++) c[i] = c[i-1] + posterior[i];
+
+
+ float u0 = (*rndunif)(unifgen)/numOfPart;
+
+ int i = 0;
+ for (int j = 0; j < numOfPart; j++)
+ {
+ float uj = u0 + (j + 0.) / numOfPart;
+
+ while (uj > c[i] && i < numOfPart - 1){
+ i++;
+ }
+
+ classes[j] = oldClasses[i];
+ alignment[j] = oldAlignment[i];
+
+ for (int l=0;l<dynamicsDim;l++) dynamics[j][l] = oldDynamics[i][l];
+ for (int l=0;l<scalingsDim;l++) scalings[j][l] = oldScalings[i][l];
+ if (rotationsDim!=0) for (int l=0;l<rotationsDim;l++) rotations[j][l] = oldRotations[i][l];
+
+ // update posterior (partilces' weights)
+ posterior[j] = 1.0/(float)numOfPart;
+ }
+
+}
+
+
+//--------------------------------------------------------------
+void GVF::estimates(){
+
+
+ int numOfPart = parameters.numberParticles;
+ vector<float> probabilityNormalisation(getNumberOfGestureTemplates());
+ setVec(probabilityNormalisation, 0.0f, getNumberOfGestureTemplates()); // rows are gestures
+ setVec(estimatedAlignment, 0.0f, getNumberOfGestureTemplates()); // rows are gestures
+ setMat(estimatedDynamics, 0.0f, getNumberOfGestureTemplates(), dynamicsDim); // rows are gestures, cols are features + probabilities
+ setMat(estimatedScalings, 0.0f, getNumberOfGestureTemplates(), scalingsDim); // rows are gestures, cols are features + probabilities
+ if (rotationsDim!=0) setMat(estimatedRotations, 0.0f, getNumberOfGestureTemplates(), rotationsDim); // ..
+ setVec(estimatedProbabilities, 0.0f, getNumberOfGestureTemplates()); // rows are gestures
+ setVec(estimatedLikelihoods, 0.0f, getNumberOfGestureTemplates()); // rows are gestures
+
+ // float sumposterior = 0.;
+
+ for(int n = 0; n < numOfPart; n++)
+ {
+ probabilityNormalisation[classes[n]] += posterior[n];
+ }
+
+
+ // compute the estimated features and likelihoods
+ for(int n = 0; n < numOfPart; n++)
+ {
+
+ // sumposterior += posterior[n];
+ estimatedAlignment[classes[n]] += alignment[n] * posterior[n];
+
+ for(int m = 0; m < dynamicsDim; m++)
+ estimatedDynamics[classes[n]][m] += dynamics[n][m] * (posterior[n]/probabilityNormalisation[classes[n]]);
+
+ for(int m = 0; m < scalingsDim; m++)
+ estimatedScalings[classes[n]][m] += scalings[n][m] * (posterior[n]/probabilityNormalisation[classes[n]]);
+
+ if (rotationsDim!=0)
+ for(int m = 0; m < rotationsDim; m++)
+ estimatedRotations[classes[n]][m] += rotations[n][m] * (posterior[n]/probabilityNormalisation[classes[n]]);
+
+ if (!isnan(posterior[n]))
+ estimatedProbabilities[classes[n]] += posterior[n];
+ estimatedLikelihoods[classes[n]] += likelihood[n];
+ }
+
+ // calculate most probable index during scaling...
+ float maxProbability = 0.0f;
+ mostProbableIndex = -1;
+
+ for(int gi = 0; gi < getNumberOfGestureTemplates(); gi++)
+ {
+ if(estimatedProbabilities[gi] > maxProbability){
+ maxProbability = estimatedProbabilities[gi];
+ mostProbableIndex = gi;
+ }
+ }
+ // std::cout << estimatedProbabilities[0] << " " << estimatedProbabilities[1] << std::endl;
+
+ // outcomes.estimations.clear();
+ outcomes.likelihoods.clear();
+ outcomes.alignments.clear();
+ outcomes.scalings.clear();
+ outcomes.dynamics.clear();
+ outcomes.rotations.clear();
+
+ // most probable gesture index
+ outcomes.likeliestGesture = mostProbableIndex;
+
+ // Fill estimation for each gesture
+ for (int gi = 0; gi < gestureTemplates.size(); ++gi) {
+
+ // GVFEstimation estimation;
+ outcomes.likelihoods.push_back(estimatedProbabilities[gi]);
+ outcomes.alignments.push_back(estimatedAlignment[gi]);
+ // estimation.probability = estimatedProbabilities[gi];
+ // estimation.alignment = estimatedAlignment[gi];
+
+
+ vector<float> gDynamics(dynamicsDim, 0.0);
+ for (int j = 0; j < dynamicsDim; ++j) gDynamics[j] = estimatedDynamics[gi][j];
+ outcomes.dynamics.push_back(gDynamics);
+
+ vector<float> gScalings(scalingsDim, 0.0);
+ for (int j = 0; j < scalingsDim; ++j) gScalings[j] = estimatedScalings[gi][j];
+ outcomes.scalings.push_back(gScalings);
+
+ vector<float> gRotations;
+ if (rotationsDim!=0)
+ {
+ gRotations.resize(rotationsDim);
+ for (int j = 0; j < rotationsDim; ++j) gRotations[j] = estimatedRotations[gi][j];
+ outcomes.rotations.push_back(gRotations);
+ }
+
+ // estimation.likelihood = estimatedLikelihoods[gi];
+
+ // push estimation for gesture gi in outcomes
+ // outcomes.estimations.push_back(estimation);
+ }
+
+
+ // assert(outcomes.estimations.size() == gestureTemplates.size());
+
+}
+
+////--------------------------------------------------------------
+//int GVF::getMostProbableGestureIndex()
+//{
+// return mostProbableIndex;
+//}
+
+////--------------------------------------------------------------
+//GVFOutcomes GVF::getOutcomes()
+//{
+// return outcomes;
+//}
+
+////--------------------------------------------------------------
+//GVFEstimation GVF::getTemplateRecogInfo(int templateNumber)
+//{
+// if (getOutcomes().estimations.size() <= templateNumber) {
+// GVFEstimation estimation;
+// return estimation; // blank
+// }
+// else
+// return getOutcomes().estimations[templateNumber];
+//}
+//
+////--------------------------------------------------------------
+//GVFEstimation GVF::getRecogInfoOfMostProbable() // FIXME: Rename!
+//{
+// int indexMostProbable = getMostProbableGestureIndex();
+//
+// if ((getState() == GVF::STATE_FOLLOWING) && (getMostProbableGestureIndex() != -1)) {
+// return getTemplateRecogInfo(indexMostProbable);
+// }
+// else {
+// GVFEstimation estimation;
+// return estimation; // blank
+// }
+//}
+
+
+////--------------------------------------------------------------
+//vector<float> & GVF::getGestureProbabilities()
+//{
+// gestureProbabilities.resize(getNumberOfGestureTemplates());
+// setVec(gestureProbabilities, 0.0f);
+// for(int n = 0; n < parameters.numberParticles; n++)
+// gestureProbabilities[classes[n]] += posterior[n];
+//
+// return gestureProbabilities;
+//}
+
+//--------------------------------------------------------------
+const vector<vector<float> > & GVF::getParticlesPositions(){
+ return particles;
+}
+
+////--------------------------------------------------------------
+//void GVF::setParameters(GVFParameters _parameters){
+//
+// // if the number of particles has changed, we have to re-allocate matrices
+// if (_parameters.numberParticles != parameters.numberParticles)
+// {
+// parameters = _parameters;
+//
+// // minimum number of particles allowed
+// if (parameters.numberParticles < 4) parameters.numberParticles = 4;
+//
+// // re-learn
+// train();
+//
+// // adapt the resampling threshold in case if RT < NS
+// if (parameters.numberParticles <= parameters.resamplingThreshold)
+// parameters.resamplingThreshold = parameters.numberParticles / 4;
+//
+// }
+// else
+// parameters = _parameters;
+//
+//
+//}
+//
+//GVFParameters GVF::getParameters(){
+// return parameters;
+//}
+
+//--------------------------------------------------------------
+// Update the number of particles
+void GVF::setNumberOfParticles(int numberOfParticles){
+
+ parameters.numberParticles = numberOfParticles;
+
+ if (parameters.numberParticles < 4) // minimum number of particles allowed
+ parameters.numberParticles = 4;
+
+ train();
+
+ if (parameters.numberParticles <= parameters.resamplingThreshold) {
+ parameters.resamplingThreshold = parameters.numberParticles / 4;
+ }
+
+}
+
+//--------------------------------------------------------------
+int GVF::getNumberOfParticles(){
+ return parameters.numberParticles; // Return the number of particles
+}
+
+//--------------------------------------------------------------
+void GVF::setActiveGestures(vector<int> activeGestureIds)
+{
+ int argmax = *std::max_element(activeGestureIds.begin(), activeGestureIds.end());
+ if (activeGestureIds[argmax] <= gestureTemplates.size())
+ {
+ activeGestures = activeGestureIds;
+ }
+ else
+ {
+ activeGestures.resize(gestureTemplates.size());
+ std::iota(activeGestures.begin(), activeGestures.end(), 1);
+ }
+}
+
+//--------------------------------------------------------------
+void GVF::setPredictionSteps(int predictionSteps)
+{
+ if (predictionSteps<1)
+ parameters.predictionSteps = 1;
+ else
+ parameters.predictionSteps = predictionSteps;
+}
+
+//--------------------------------------------------------------
+int GVF::getPredictionSteps()
+{
+ return parameters.predictionSteps; // Return the number of particles
+}
+
+//--------------------------------------------------------------
+// Update the resampling threshold used to avoid degeneracy problem
+void GVF::setResamplingThreshold(int _resamplingThreshold){
+ if (_resamplingThreshold >= parameters.numberParticles)
+ _resamplingThreshold = floor(parameters.numberParticles/2.0f);
+ parameters.resamplingThreshold = _resamplingThreshold;
+}
+
+//--------------------------------------------------------------
+// Return the resampling threshold used to avoid degeneracy problem
+int GVF::getResamplingThreshold(){
+ return parameters.resamplingThreshold;
+}
+
+//--------------------------------------------------------------
+// Update the standard deviation of the observation distribution
+// this value acts as a tolerance for the algorithm
+// low value: less tolerant so more precise but can diverge
+// high value: more tolerant so less precise but converge more easily
+void GVF::setTolerance(float _tolerance){
+ if (_tolerance <= 0.0) _tolerance = 0.1;
+ parameters.tolerance = _tolerance;
+ tolerancesetmanually = true;
+}
+
+//--------------------------------------------------------------
+float GVF::getTolerance(){
+ return parameters.tolerance;
+}
+
+////--------------------------------------------------------------
+void GVF::setDistribution(float _distribution){
+ //nu = _distribution;
+ parameters.distribution = _distribution;
+}
+//
+////--------------------------------------------------------------
+//float GVF::getDistribution(){
+// return parameters.distribution;
+//}
+
+//void GVF::setDimWeights(vector<float> dimWeights){
+// if (dimWeights.size()!=parameters.dimWeights.size())
+// parameters.dimWeights.resize(dimWeights.size());
+// parameters.dimWeights = dimWeights;
+//}
+//
+//vector<float> GVF::getDimWeights(){
+// return parameters.dimWeights;
+//}
+
+
+//// VARIANCE COEFFICIENTS: PHASE
+////--------------------------------------------------------------
+//void GVF::setAlignmentVariance(float alignmentVariance){
+// parameters.alignmentVariance = sqrt(alignmentVariance);
+//}
+////--------------------------------------------------------------
+//float GVF::getAlignmentVariance(){
+// return parameters.alignmentVariance;
+//}
+
+
+// VARIANCE COEFFICIENTS: DYNAMICS
+//--------------------------------------------------------------
+//void GVF::setDynamicsVariance(float dynVariance)
+//{
+// for (int k=0; k< parameters.dynamicsVariance.size(); k++)
+// parameters.dynamicsVariance[k] = dynVariance;
+//}
+//--------------------------------------------------------------
+void GVF::setDynamicsVariance(float dynVariance, int dim)
+{
+ if (dim == -1)
+ {
+ for (int k=0; k< parameters.dynamicsVariance.size(); k++)
+ parameters.dynamicsVariance[k] = dynVariance;
+ }
+ else
+ {
+ if (dim<parameters.dynamicsVariance.size())
+ parameters.dynamicsVariance[dim-1] = dynVariance;
+ }
+}
+
+//--------------------------------------------------------------
+void GVF::setDynamicsVariance(vector<float> dynVariance)
+{
+ parameters.dynamicsVariance = dynVariance;
+}
+//--------------------------------------------------------------
+vector<float> GVF::getDynamicsVariance()
+{
+ return parameters.dynamicsVariance;
+}
+
+//--------------------------------------------------------------
+void GVF::setScalingsVariance(float scaleVariance, int dim)
+{
+ if (dim == -1)
+ {
+ for (int k=0; k< parameters.scalingsVariance.size(); k++)
+ parameters.scalingsVariance[k] = scaleVariance;
+ }
+ else
+ {
+ if (dim<parameters.scalingsVariance.size())
+ parameters.scalingsVariance[dim-1] = scaleVariance;
+ }
+}
+
+//--------------------------------------------------------------
+void GVF::setScalingsVariance(vector<float> scaleVariance)
+{
+ parameters.scaleVariance = scaleVariance;
+}
+
+//--------------------------------------------------------------
+vector<float> GVF::getScalingsVariance()
+{
+ return parameters.scalingsVariance;
+}
+
+//--------------------------------------------------------------
+void GVF::setRotationsVariance(float rotationVariance, int dim)
+{
+ if (dim == -1)
+ {
+ for (int k=0; k< parameters.rotationsVariance.size(); k++)
+ parameters.rotationsVariance[k] = rotationVariance;
+ }
+ else
+ {
+ if (dim<parameters.rotationsVariance.size())
+ parameters.scalingsVariance[dim-1] = rotationVariance;
+ }
+}
+
+//--------------------------------------------------------------
+void GVF::setRotationsVariance(vector<float> rotationVariance)
+{
+ parameters.scaleVariance = rotationVariance;
+}
+
+//--------------------------------------------------------------
+vector<float> GVF::getRotationsVariance()
+{
+ return parameters.rotationsVariance;
+}
+
+//--------------------------------------------------------------
+void GVF::setSpreadDynamics(float center, float range, int dim)
+{
+ parameters.dynamicsSpreadingCenter = center;
+ parameters.dynamicsSpreadingRange = range;
+}
+
+//--------------------------------------------------------------
+void GVF::setSpreadScalings(float center, float range, int dim)
+{
+ parameters.scalingsSpreadingCenter = center;
+ parameters.scalingsSpreadingRange = range;
+}
+
+//--------------------------------------------------------------
+void GVF::setSpreadRotations(float center, float range, int dim)
+{
+ parameters.rotationsSpreadingCenter = center;
+ parameters.rotationsSpreadingRange = range;
+}
+
+//--------------------------------------------------------------
+void GVF::translate(bool translateFlag)
+{
+ config.translate = translateFlag;
+}
+
+//--------------------------------------------------------------
+void GVF::segmentation(bool segmentationFlag)
+{
+ config.segmentation = segmentationFlag;
+}
+
+
+// UTILITIES
+
+//--------------------------------------------------------------
+// Save function. This function is used by applications to save the
+// vocabulary in a text file given by filename (filename is also the complete path + filename)
+void GVF::saveTemplates(string filename){
+
+ std::string directory = filename;
+
+ std::ofstream file_write(directory.c_str());
+
+ for(int i=0; i < gestureTemplates.size(); i++) // Number of gesture templates
+ {
+ file_write << "template " << i << " " << config.inputDimensions << endl;
+ vector<vector<float> > templateTmp = gestureTemplates[i].getTemplate();
+ for(int j = 0; j < templateTmp.size(); j++)
+ {
+ for(int k = 0; k < config.inputDimensions; k++)
+ file_write << templateTmp[j][k] << " ";
+ file_write << endl;
+ }
+ }
+ file_write.close();
+
+}
+
+
+
+
+//--------------------------------------------------------------
+// Load function. This function is used by applications to load a vocabulary
+// given by filename (filename is also the complete path + filename)
+void GVF::loadTemplates(string filename){
+ // clear();
+ //
+
+ GVFGesture loadedGesture;
+ loadedGesture.clear();
+
+ ifstream infile;
+ stringstream doung;
+
+ infile.open (filename.c_str(), ifstream::in);
+ //
+ string line;
+ vector<string> list;
+ int cl = -1;
+ while(!infile.eof())
+ {
+ cl++;
+ infile >> line;
+
+ list.push_back(line);
+ }
+
+ int k = 0;
+ int template_id = -1;
+ int template_dim = 0;
+
+
+ while (k < (list.size() - 1)){ // TODO to be changed if dim>2
+
+
+ if (!strcmp(list[k].c_str(),"template"))
+ {
+ template_id = atoi(list[k+1].c_str());
+ template_dim = atoi(list[k+2].c_str());
+ k = k + 3;
+
+ if (loadedGesture.getNumberOfTemplates() > 0){
+ addGestureTemplate(loadedGesture);
+ loadedGesture.clear();
+ }
+ }
+
+ if (template_dim <= 0){
+ //post("bug dim = -1");
+ }
+ else{
+
+ vector<float> vect(template_dim);
+
+ for (int kk = 0; kk < template_dim; kk++)
+ vect[kk] = (float) atof(list[k + kk].c_str());
+
+ loadedGesture.addObservation(vect);
+ }
+ k += template_dim;
+
+ }
+
+ if (loadedGesture.getTemplateLength() > 0){
+ addGestureTemplate(loadedGesture);
+ loadedGesture.clear();
+ }
+
+ infile.close();
+}
+
+
+
+
+
+
diff --git a/GVF/GVF.h b/GVF/GVF.h
new file mode 100755
index 0000000000000000000000000000000000000000..647fe7c6ea372e9ec8fc265a946fb3a44c91abcc
--- /dev/null
+++ b/GVF/GVF.h
@@ -0,0 +1,487 @@
+/**
+ * Gesture Variation Follower class allows for early gesture recognition and variation tracking
+ *
+ * @details Original algorithm designed and implemented in 2011 at Ircam Centre Pompidou
+ * by Baptiste Caramiaux and Nicola Montecchio. The library has been created and is maintained by Baptiste Caramiaux
+ *
+ * Copyright (C) 2015 Baptiste Caramiaux, Nicola Montecchio
+ * STMS lab Ircam-CRNS-UPMC, University of Padova, Goldsmiths College University of London
+ *
+ * The library is under the GNU Lesser General Public License (LGPL v3)
+ */
+
+
+#ifndef _H_GVF
+#define _H_GVF
+
+#include "GVFUtils.h"
+#include "GVFGesture.h"
+#include <random>
+#include <iostream>
+#include <iomanip>
+#include <string>
+#include <map>
+#include <random>
+#include <cmath>
+
+
+using namespace std;
+
+class GVF
+{
+
+public:
+
+ /**
+ * GVF possible states
+ */
+ enum GVFState
+ {
+ STATE_CLEAR = 0, /**< STATE_CLEAR: clear the GVF and be in standby */
+ STATE_LEARNING, /**< STATE_LEARNING: recording mode, input gestures are added to the templates */
+ STATE_FOLLOWING, /**< STATE_FOLLOWING: tracking mode, input gestures are classifed and their variations tracked (need the GVF to be trained) */
+ STATE_BYPASS /**< STATE_BYPASS: by pass GVF but does not erase templates or training */
+ };
+
+
+#pragma mark - Constructors
+
+ /**
+ * GVF default constructor
+ * @details use default configuration and parameters, can be changed using accessors
+ */
+ GVF();
+
+ /**
+ * GVF default destructor
+ */
+ ~GVF();
+
+#pragma mark - Gesture templates
+
+ /**
+ * Start a gesture either to be recorded or followed
+ */
+ void startGesture();
+
+ /**
+ * Add an observation to a gesture template
+ * @details
+ * @param data vector of features
+ */
+ void addObservation(vector<float> data);
+
+ /**
+ * Add gesture template to the vocabulary
+ *
+ * @details a gesture template is a GVFGesture object and can be added directly to the vocabulqry or
+ * recorded gesture templates by using this method
+ * @param gestureTemplate the gesture template to be recorded
+ */
+ void addGestureTemplate(GVFGesture & gestureTemplate);
+
+ /**
+ * Replace a specific gesture template by another
+ *
+ * @param gestureTemplate the gesture template to be used
+ * @param index the gesture index (as integer) to be replaced
+ */
+ void replaceGestureTemplate(GVFGesture & gestureTemplate, int index);
+
+ /**
+ * Remove a specific template
+ *
+ * @param index the gesture index (as integer) to be removed
+ */
+ void removeGestureTemplate(int index);
+
+ /**
+ * Remove every recorded gesture template
+ */
+ void removeAllGestureTemplates();
+
+ /**
+ * Get a specific gesture template a gesture template by another
+ *
+ * @param index the index of the template to be returned
+ * @return the template
+ */
+ GVFGesture & getGestureTemplate(int index);
+
+ /**
+ * Get every recorded gesture template
+ *
+ * @return the vecotr of gesture templates
+ */
+ vector<GVFGesture> & getAllGestureTemplates();
+
+ /**
+ * Get number of gesture templates in the vocabulary
+ * @return the number of templates
+ */
+ int getNumberOfGestureTemplates();
+
+ /**
+ * Get gesture classes
+ */
+ vector<int> getGestureClasses();
+
+
+#pragma mark - Recognition and tracking
+
+ /**
+ * Set the state of GVF
+ * @param _state the state to be given to GVF, it is a GVFState
+ * @param indexes an optional argument providing a list of gesture index.
+ * In learning mode the index of the gesture being recorded can be given as an argument
+ * since the type is vector<int>, it should be something like '{3}'. In following mode, the list of indexes
+ * is the list of active gestures to be considered in the recognition/tracking.
+ */
+ void setState(GVFState _state, vector<int> indexes = vector<int>());
+
+ /**
+ * Return the current state of GVF
+ * @return GVFState the current state
+ */
+ GVFState getState();
+
+ /**
+ * Compute the estimated gesture and its potential variations
+ *
+ * @details infers the probability that the current observation belongs to
+ * one of the recorded gesture template and track the variations of this gesture
+ * according to each template
+ *
+ * @param observation vector of the observation data at current time
+ * @return the estimated probabilities and variaitons relative to each template
+ */
+ GVFOutcomes & update(vector<float> & observation);
+
+ /**
+ * Define a subset of gesture templates on which to perform the recognition
+ * and variation tracking
+ *
+ * @details By default every recorded gesture template is considered
+ * @param set of gesture template index to consider
+ */
+ void setActiveGestures(vector<int> activeGestureIds);
+
+ /**
+ * Restart GVF
+ * @details re-sample particles at the origin (i.e. initial prior)
+ */
+ void restart();
+
+ /**
+ * Clear GVF
+ * @details delete templates
+ */
+ void clear();
+
+ /**
+ * Translate data according to the first point
+ * @details substract each gesture feature by the first point of the gesture
+ * @param boolean to activate or deactivate translation
+ */
+ void translate(bool translateFlag);
+
+ /**
+ * Segment gestures within a continuous gesture stream
+ * @details if segmentation is true, the method will segment a continuous gesture into a sequence
+ * of gestures. In other words no need to call the method startGesture(), it is done automatically
+ * @param segmentationFlag boolean to activate or deactivate segmentation
+ */
+ void segmentation(bool segmentationFlag);
+
+#pragma mark - [ Accessors ]
+#pragma mark > Parameters
+ /**
+ * Set tolerance between observation and estimation
+ * @details tolerance depends on the range of the data
+ * typially tolerance = (data range)/3.0;
+ * @param tolerance value
+ */
+ void setTolerance(float tolerance);
+
+ /**
+ * Get the obervation tolerance value
+ * @details see setTolerance(float tolerance)
+ * @return the current toleranc value
+ */
+ float getTolerance();
+
+ void setDistribution(float _distribution);
+
+ /**
+ * Set number of particles used in estimation
+ * @details default valye is 1000, note that the computational
+ * cost directly depends on the number of particles
+ * @param new number of particles
+ */
+ void setNumberOfParticles(int numberOfParticles);
+
+ /**
+ * Get the current number of particles
+ * @return the current number of particles
+ */
+ int getNumberOfParticles();
+
+ /**
+ * Number of prediciton steps
+ * @details it is possible to leave GVF to perform few steps of prediction
+ * ahead which can be useful to estimate more fastly the variations. Default value is 1
+ * which means no prediction ahead
+ * @param the number of prediction steps
+ */
+ void setPredictionSteps(int predictionSteps);
+
+ /**
+ * Get the current number of prediction steps
+ * @return current number of prediciton steps
+ */
+ int getPredictionSteps();
+
+ /**
+ * Set resampling threshold
+ * @details resampling threshold is the minimum number of active particles
+ * before resampling all the particles by the estimated posterior distribution.
+ * in other words, it re-targets particles around the best current estimates
+ * @param the minimum number of particles (default is (number of particles)/2)
+ */
+ void setResamplingThreshold(int resamplingThreshold);
+
+ /**
+ * Get the current resampling threshold
+ * @return resampling threshold
+ */
+ int getResamplingThreshold();
+
+#pragma mark > Dynamics
+ /**
+ * Change variance of adaptation in dynamics
+ * @details if dynamics adaptation variance is high the method will adapt faster to
+ * fast changes in dynamics. Dynamics is 2-dimensional: the first dimension is the speed
+ * The second dimension is the acceleration.
+ *
+ * Typically the variance is the average amount the speed or acceleration can change from
+ * one sample to another. As an example, if the relative estimated speed can change from 1.1 to 1.2
+ * from one sample to another, the variance should allow a change of 0.1 in speed. So the variance
+ * should be set to 0.1*0.1 = 0.01
+ *
+ * @param dynVariance dynamics variance value
+ * @param dim optional dimension of the dynamics for which the change of variance is applied (default value is 1)
+ */
+ void setDynamicsVariance(float dynVariance, int dim = -1);
+
+ /**
+ * Change variance of adaptation in dynamics
+ * @details See setDynamicsVariance(float dynVariance, int dim) for more details
+ * @param dynVariance vector of dynamics variances, each vector index is the variance to be applied to
+ * each dynamics dimension (consequently the vector should be 2-dimensional).
+ */
+ void setDynamicsVariance(vector<float> dynVariance);
+
+ /**
+ * Get dynamics variances
+ * @return the vector of variances (the returned vector is 2-dimensional)
+ */
+ vector<float> getDynamicsVariance();
+
+#pragma mark > Scalings
+ /**
+ * Change variance of adaptation in scalings
+ * @details if scalings adaptation variance is high the method will adapt faster to
+ * fast changes in relative sizes. There is one scaling variance for each dimension
+ * of the input gesture. If the gesture is 2-dimensional, the scalings variances will
+ * also be 2-dimensional.
+ *
+ * Typically the variance is the average amount the size can change from
+ * one sample to another. As an example, if the relative estimated size changes from 1.1 to 1.15
+ * from one sample to another, the variance should allow a change of 0.05 in size. So the variance
+ * should be set to 0.05*0.05 = 0.0025
+ *
+ * @param scalings variance value
+ * @param dimension of the scalings for which the change of variance is applied
+ */
+ void setScalingsVariance(float scaleVariance, int dim = -1);
+
+ /**
+ * Change variance of adaptation in dynamics
+ * @details See setScalingsVariance(float scaleVariance, int dim) for more details
+ * @param vector of scalings variances, each vector index is the variance to be applied to
+ * each scaling dimension.
+ * @param vector of variances (should be the size of the template gestures dimension)
+ */
+ void setScalingsVariance(vector<float> scaleVariance);
+
+ /**
+ * Get scalings variances
+ * @return the vector of variances
+ */
+ vector<float> getScalingsVariance();
+
+#pragma mark > Rotations
+ /**
+ * Change variance of adaptation in orientation
+ * @details if rotation adaptation variance is high the method will adapt faster to
+ * fast changes in relative orientation. If the gesture is 2-dimensional, there is
+ * one variance value since the rotation can be defined by only one angle of rotation. If
+ * the gesture is 3-dimensional, there are 3 variance values since the rotation in 3-d is
+ * defined by 3 rotation angles. For any other dimension, the rotation is not defined.
+ *
+ * The variance is the average amount the orientation can change from one sample to another.
+ * As an example, if the relative orientation in rad changes from 0.1 to 0.2 from one observation
+ * to another, the variance should allow a change of 0.1 in rotation angle. So the variance
+ * should be set to 0.1*0.1 = 0.01
+ *
+ * @param rotationsVariance rotation variance value
+ * @param dim optional dimension of the rotation for which the change of variance is applied
+ */
+ void setRotationsVariance(float rotationsVariance, int dim = -1);
+
+ /**
+ * Change variance of adaptation in orientation
+ * @details See setRotationsVariance(float rotationsVariance, int dim) for more details
+ * @param vector of rotation variances, each vector index is the variance to be applied to
+ * each rotation angle (1 or 3)
+ * @param vector of variances (should be 1 if the the template gestures are 2-dim or 3 if
+ * they are 3-dim)
+ */
+ void setRotationsVariance(vector<float> rotationsVariance);
+
+ /**
+ * Get rotation variances
+ * @return the vector of variances
+ */
+ vector<float> getRotationsVariance();
+
+
+#pragma mark > Others
+
+ /**
+ * Get particle values
+ * @return vector of list of estimated particles
+ */
+ const vector<vector<float> > & getParticlesPositions();
+
+ /**
+ * Set the interval on which the dynamics values should be spread at the beginning (before adaptation)
+ * @details this interval can be used to concentrate the potential dynamics value on a narrow interval,
+ * typically around 1 (the default value), for instance between -0.05 and 0.05, or to allow at the very
+ * beginning, high changes in dynamics by spreading, for instance between 0.0 and 2.0
+ * @param min lower value of the inital values for dynamics
+ * @param max higher value of the inital values for dynamics
+ * @param dim the dimension on which the change of initial interval should be applied (optional)
+ */
+ void setSpreadDynamics(float min, float max, int dim = -1);
+
+ /**
+ * Set the interval on which the scalings values should be spread at the beginning (before adaptation)
+ * @details this interval can be used to concentrate the potential scalings value on a narrow interval,
+ * typically around 1.0 (the default value), for instance between 0.95 and 1.05, or to allow at the very
+ * beginning high changes in dynamics by spreading, for instance, between 0.0 and 2.0
+ * @param min lower value of the inital values for scalings
+ * @param max higher value of the inital values for scalings
+ * @param dim the dimension on which the change of initial interval should be applied (optional)
+ */
+ void setSpreadScalings(float min, float max, int dim = -1);
+
+ /**
+ * Set the interval on which the angle of rotation values should be spread at the beginning (before adaptation)
+ * @details this interval can be used to concentrate the potential angle values on a narrow interval,
+ * typically around 0.0 (the default value), for instance between -0.05 and 0.05, or to allow at the very
+ * beginning, high changes in orientation by spreading, for instance, between -0.5 and 0.5
+ * @param min lower value of the inital values for angle of rotation
+ * @param max higher value of the inital values for angle of rotation
+ * @param dim the dimension on which the change of initial interval should be applied (optional)
+ */
+ void setSpreadRotations(float min, float max, int dim = -1);
+
+#pragma mark - Import/Export templates
+ /**
+ * Export template data in a filename
+ * @param filename file name as a string
+ */
+ void saveTemplates(string filename);
+
+ /**
+ * Import template data in a filename
+ * @details needs to respect a given format provided by saveTemplates()
+ * @param file name as a string
+ */
+ void loadTemplates(string filename);
+
+protected:
+
+ GVFConfig config; // Structure storing the configuration of GVF (in GVFUtils.h)
+ GVFParameters parameters; // Structure storing the parameters of GVF (in GVFUtils.h)
+ GVFOutcomes outcomes; // Structure storing the outputs of GVF (in GVFUtils.h)
+ GVFState state; // State (defined above)
+ GVFGesture theGesture; // GVFGesture object to handle incoming data in learning and following modes
+
+ vector<GVFGesture> gestureTemplates; // vector storing the gesture templates recorded when using the methods addObservation(vector<float> data) or addGestureTemplate(GVFGesture & gestureTemplate)
+
+ vector<float> dimWeights; // TOOD: to be put in parameters?
+ vector<float> maxRange;
+ vector<float> minRange;
+ int dynamicsDim; // dynamics state dimension
+ int scalingsDim; // scalings state dimension
+ int rotationsDim; // rotation state dimension
+ float globalNormalizationFactor; // flagged if normalization
+ int mostProbableIndex; // cached most probable index
+ int learningGesture;
+
+ vector<int> classes; // gesture index for each particle [ns x 1]
+ vector<float > alignment; // alignment index (between 0 and 1) [ns x 1]
+ vector<vector<float> > dynamics; // dynamics estimation [ns x 2]
+ vector<vector<float> > scalings; // scalings estimation [ns x D]
+ vector<vector<float> > rotations; // rotations estimation [ns x A]
+ vector<float> weights; // weight of each particle [ns x 1]
+ vector<float> prior; // prior of each particle [ns x 1]
+ vector<float> posterior; // poserior of each particle [ns x 1]
+ vector<float> likelihood; // likelihood of each particle [ns x 1]
+
+ // estimations
+ vector<float> estimatedGesture; // ..
+ vector<float> estimatedAlignment; // ..
+ vector<vector<float> > estimatedDynamics; // ..
+ vector<vector<float> > estimatedScalings; // ..
+ vector<vector<float> > estimatedRotations; // ..
+ vector<float> estimatedProbabilities; // ..
+ vector<float> estimatedLikelihoods; // ..
+ vector<float> absoluteLikelihoods; // ..
+
+ bool tolerancesetmanually;
+
+
+ vector<vector<float> > offsets; // translation offset
+
+ vector<int> activeGestures;
+
+ vector<float> gestureProbabilities;
+ vector< vector<float> > particles;
+
+private:
+
+ // random number generator
+ std::random_device rd;
+ std::mt19937 normgen;
+ std::normal_distribution<float> *rndnorm;
+ std::default_random_engine unifgen;
+ std::uniform_real_distribution<float> *rndunif;
+
+#pragma mark - Private methods for model mechanics
+ void initPrior();
+ void initNoiseParameters();
+ void updateLikelihood(vector<float> obs, int n);
+ void updatePrior(int n);
+ void updatePosterior(int n);
+ void resampleAccordingToWeights(vector<float> obs);
+ void estimates(); // update estimated outcome
+ void train();
+
+
+};
+
+
+#endif
\ No newline at end of file
diff --git a/GVF/GVFGesture.h b/GVF/GVFGesture.h
new file mode 100644
index 0000000000000000000000000000000000000000..d556fb0a9ed0f9ff84f7f09e48a157fd6357ab3b
--- /dev/null
+++ b/GVF/GVFGesture.h
@@ -0,0 +1,240 @@
+//
+// GVFGesture.h
+// gvf
+//
+// Created by Baptiste Caramiaux on 22/01/16.
+//
+//
+
+#ifndef GVFGesture_h
+#define GVFGesture_h
+
+#ifndef MAX
+#define MAX(a,b) (((a) > (b)) ? (a) : (b))
+#endif
+
+#ifndef MIN
+#define MIN(a,b) (((a) < (b)) ? (a) : (b))
+#endif
+
+
+class GVFGesture
+{
+public:
+
+ GVFGesture()
+ {
+ inputDimensions = 2;
+ setAutoAdjustRanges(true);
+ templatesRaw = vector<vector<vector<float > > >();
+ templatesNormal = vector<vector<vector<float > > >();
+ clear();
+ }
+
+ GVFGesture(int inputDimension){
+ inputDimensions = inputDimension;
+ setAutoAdjustRanges(true);
+ templatesRaw = vector<vector<vector<float > > >();
+ templatesNormal = vector<vector<vector<float > > >();
+ clear();
+ }
+
+ ~GVFGesture(){
+ clear();
+ }
+
+ void setNumberDimensions(int dimensions){
+ assert(dimensions > 0);
+ inputDimensions = dimensions;
+ }
+
+ void setAutoAdjustRanges(bool b){
+ // if(b) bIsRangeMinSet = bIsRangeMaxSet = false;
+ bAutoAdjustNormalRange = b;
+ }
+
+ void setMax(float x, float y){
+ assert(inputDimensions == 2);
+ vector<float> r(2);
+ r[0] = x; r[1] = y;
+ setMaxRange(r);
+ }
+
+ void setMin(float x, float y){
+ assert(inputDimensions == 2);
+ vector<float> r(2);
+ r[0] = x; r[1] = y;
+ setMinRange(r);
+ }
+
+ void setMax(float x, float y, float z){
+ assert(inputDimensions == 3);
+ vector<float> r(3);
+ r[0] = x; r[1] = y; r[2] = z;
+ setMaxRange(r);
+ }
+
+ void setMin(float x, float y, float z){
+ assert(inputDimensions == 3);
+ vector<float> r(3);
+ r[0] = x; r[1] = y; r[2] = z;
+ setMinRange(r);
+ }
+
+ void setMaxRange(vector<float> observationRangeMax){
+ this->observationRangeMax = observationRangeMax;
+ // bIsRangeMaxSet = true;
+ normalise();
+ }
+
+ void setMinRange(vector<float> observationRangeMin){
+ this->observationRangeMin = observationRangeMin;
+ // bIsRangeMinSet = true;
+ normalise();
+ }
+
+ vector<float>& getMaxRange(){
+ return observationRangeMax;
+ }
+
+ vector<float>& getMinRange(){
+ return observationRangeMin;
+ }
+
+ void autoAdjustMinMax(vector<float> & observation){
+ if(observationRangeMax.size() < inputDimensions){
+ observationRangeMax.assign(inputDimensions, -INFINITY);
+ observationRangeMin.assign(inputDimensions, INFINITY);
+ }
+ for(int i = 0; i < inputDimensions; i++){
+ observationRangeMax[i] = MAX(observationRangeMax[i], observation[i]);
+ observationRangeMin[i] = MIN(observationRangeMin[i], observation[i]);
+ }
+ }
+
+ void addObservation(vector<float> observation, int templateIndex = 0){
+ if (observation.size() != inputDimensions)
+ inputDimensions = observation.size();
+
+ // check we have a valid templateIndex and correct number of input dimensions
+ assert(templateIndex <= templatesRaw.size());
+ assert(observation.size() == inputDimensions);
+
+ // if the template index is same as the number of temlates make a new template
+ if(templateIndex == templatesRaw.size()){ // make a new template
+
+ // reserve space in raw and normal template storage
+ templatesRaw.resize(templatesRaw.size() + 1);
+ templatesNormal.resize(templatesNormal.size() + 1);
+
+ }
+
+ if(templatesRaw[templateIndex].size() == 0)
+ {
+ templateInitialObservation = observation;
+ templateInitialNormal = observation;
+ }
+
+ for(int j = 0; j < observation.size(); j++)
+ observation[j] = observation[j] - templateInitialObservation[j];
+
+ // store the raw observation
+ templatesRaw[templateIndex].push_back(observation);
+
+ autoAdjustMinMax(observation);
+
+ normalise();
+ }
+
+
+
+ void normalise()
+ {
+ templatesNormal.resize(templatesRaw.size());
+ for(int t = 0; t < templatesRaw.size(); t++)
+ {
+ templatesNormal[t].resize(templatesRaw[t].size());
+ for(int o = 0; o < templatesRaw[t].size(); o++)
+ {
+ templatesNormal[t][o].resize(inputDimensions);
+ for(int d = 0; d < inputDimensions; d++)
+ {
+ templatesNormal[t][o][d] = templatesRaw[t][o][d] / (observationRangeMax[d] - observationRangeMin[d]);
+ templateInitialNormal[d] = templateInitialObservation[d] / (observationRangeMax[d] - observationRangeMin[d]);
+ }
+ }
+ }
+ }
+
+ void setTemplate(vector< vector<float> > & observations, int templateIndex = 0){
+ for(int i = 0; i < observations.size(); i++){
+ addObservation(observations[i], templateIndex);
+ }
+ }
+
+ vector< vector<float> > & getTemplate(int templateIndex = 0){
+ assert(templateIndex < templatesRaw.size());
+ return templatesRaw[templateIndex];
+ }
+
+ int getNumberOfTemplates(){
+ return templatesRaw.size();
+ }
+
+ int getNumberDimensions(){
+ return inputDimensions;
+ }
+
+ int getTemplateLength(int templateIndex = 0){
+ return templatesRaw[templateIndex].size();
+ }
+
+ int getTemplateDimension(int templateIndex = 0){
+ return templatesRaw[templateIndex][0].size();
+ }
+
+ vector<float>& getLastObservation(int templateIndex = 0){
+ return templatesRaw[templateIndex][templatesRaw[templateIndex].size() - 1];
+ }
+
+ vector< vector< vector<float> > >& getTemplates(){
+ return templatesRaw;
+ }
+
+ vector<float>& getInitialObservation(){
+ return templateInitialObservation;
+ }
+
+ void deleteTemplate(int templateIndex = 0)
+ {
+ assert(templateIndex < templatesRaw.size());
+ templatesRaw[templateIndex].clear();
+ templatesNormal[templateIndex].clear();
+ }
+
+ void clear()
+ {
+ templatesRaw.clear();
+ templatesNormal.clear();
+ observationRangeMax.assign(inputDimensions, -INFINITY);
+ observationRangeMin.assign(inputDimensions, INFINITY);
+ }
+
+private:
+
+ int inputDimensions;
+ bool bAutoAdjustNormalRange;
+
+ vector<float> observationRangeMax;
+ vector<float> observationRangeMin;
+
+ vector<float> templateInitialObservation;
+ vector<float> templateInitialNormal;
+
+ vector< vector< vector<float> > > templatesRaw;
+ vector< vector< vector<float> > > templatesNormal;
+
+ vector<vector<float> > gestureDataFromFile;
+};
+
+#endif /* GVFGesture_h */
diff --git a/GVF/GVFUtils.h b/GVF/GVFUtils.h
new file mode 100755
index 0000000000000000000000000000000000000000..125676c29c01593c17833a180171f22199f17ee1
--- /dev/null
+++ b/GVF/GVFUtils.h
@@ -0,0 +1,309 @@
+//
+// GVFTypesAndUtils.h
+//
+//
+//
+
+#ifndef __H_GVFTYPES
+#define __H_GVFTYPES
+
+#include <map>
+#include <vector>
+#include <iostream>
+#include <random>
+#include <iostream>
+#include <math.h>
+#include <assert.h>
+
+using namespace std;
+
+/**
+ * Configuration structure
+ */
+typedef struct
+{
+ int inputDimensions; /**< input dimesnion */
+ bool translate; /**< translate flag */
+ bool segmentation; /**< segmentation flag */
+} GVFConfig;
+
+/**
+ * Parameters structure
+ */
+typedef struct
+{
+ float tolerance; /**< input dimesnion */
+ float distribution;
+ int numberParticles;
+ int resamplingThreshold;
+ float alignmentVariance;
+ float speedVariance;
+ vector<float> scaleVariance;
+ vector<float> dynamicsVariance;
+ vector<float> scalingsVariance;
+ vector<float> rotationsVariance;
+ // spreadings
+ float alignmentSpreadingCenter;
+ float alignmentSpreadingRange;
+ float dynamicsSpreadingCenter;
+ float dynamicsSpreadingRange;
+ float scalingsSpreadingCenter;
+ float scalingsSpreadingRange;
+ float rotationsSpreadingCenter;
+ float rotationsSpreadingRange;
+
+ int predictionSteps;
+ vector<float> dimWeights;
+} GVFParameters;
+
+// Outcomes structure
+typedef struct
+{
+ int likeliestGesture;
+ vector<float> likelihoods;
+ vector<float> alignments;
+ vector<vector<float> > dynamics;
+ vector<vector<float> > scalings;
+ vector<vector<float> > rotations;
+} GVFOutcomes;
+
+
+//--------------------------------------------------------------
+// init matrix by allocating memory
+template <typename T>
+inline void initMat(vector< vector<T> > & M, int rows, int cols){
+ M.resize(rows);
+ for (int n=0; n<rows; n++){
+ M[n].resize(cols);
+ }
+}
+
+//--------------------------------------------------------------
+// init matrix and copy values from another matrix
+template <typename T>
+inline void setMat(vector< vector<T> > & C, vector< vector<float> > & M){
+ int rows = M.size();
+ int cols = M[0].size();
+ //C.resize(rows);
+ C = vector<vector<T> >(rows);
+ for (int n=0; n<rows; n++){
+ //C[n].resize(cols);
+ C[n] = vector<T>(cols);
+ for (int m=0;m<cols;m++){
+ C[n][m] = M[n][m];
+ }
+ }
+}
+
+//--------------------------------------------------------------
+// init matrix by allocating memory and fill with T value
+template <typename T>
+inline void setMat(vector< vector<T> > & M, T value, int rows, int cols){
+ M.resize(rows);
+ for (int n=0; n<rows; n++){
+ M[n].resize(cols);
+ for (int m=0; m<cols; m++){
+ M[n][m] = value;
+ }
+ }
+}
+
+//--------------------------------------------------------------
+// set matrix filled with T value
+template <typename T>
+inline void setMat(vector< vector<T> > & M, T value){
+ for (int n=0; n<M.size(); n++){
+ for (int m=0; m<M[n].size(); m++){
+ M[n][m] = value;
+ }
+ }
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline void printMat(vector< vector<T> > & M){
+ for (int k=0; k<M.size(); k++){
+ cout << k << ": ";
+ for (int l=0; l<M[0].size(); l++){
+ cout << M[k][l] << " ";
+ }
+ cout << endl;
+ }
+ cout << endl;
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline void printVec(vector<T> & V){
+ for (int k=0; k<V.size(); k++){
+ cout << k << ": " << V[k] << (k == V.size() - 1 ? "" : " ,");
+ }
+ cout << endl;
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline void initVec(vector<T> & V, int rows){
+ V.resize(rows);
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline void setVec(vector<T> & C, vector<int> &V){
+ int rows = V.size();
+ C = vector<T>(rows);
+ //C.resize(rows);
+ for (int n=0; n<rows; n++){
+ C[n] = V[n];
+ }
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline void setVec(vector<T> & C, vector<float> & V){
+ int rows = V.size();
+ C.resize(rows);
+ for (int n=0; n<rows; n++){
+ C[n] = V[n];
+ }
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline void setVec(vector<T> & V, T value){
+ for (int n=0; n<V.size(); n++){
+ V[n] = value;
+ }
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline void setVec(vector<T> & V, T value, int rows){
+ V.resize(rows);
+ setVec(V, value);
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline vector< vector<T> > dotMat(vector< vector<T> > & M1, vector< vector<T> > & M2){
+ // TODO(Baptiste)
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline vector< vector<T> > multiplyMatf(vector< vector<T> > & M1, T v){
+ vector< vector<T> > multiply;
+ initMat(multiply, M1.size(), M1[0].size());
+ for (int i=0; i<M1.size(); i++){
+ for (int j=0; j<M1[i].size(); j++){
+ multiply[i][j] = M1[i][j] * v;
+ }
+ }
+ return multiply;
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline vector< vector<T> > multiplyMatf(vector< vector<T> > & M1, vector< vector<T> > & M2){
+ assert(M1[0].size() == M2.size()); // columns in M1 == rows in M2
+ vector< vector<T> > multiply;
+ initMat(multiply, M1.size(), M2[0].size()); // rows in M1 x cols in M2
+ for (int i=0; i<M1.size(); i++){
+ for (int j=0; j<M2[i].size(); j++){
+ multiply[i][j] = 0.0f;
+ for(int k=0; k<M1[0].size(); k++){
+ multiply[i][j] += M1[i][k] * M2[k][j];
+ }
+
+ }
+ }
+ return multiply;
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline vector<T> multiplyMat(vector< vector<T> > & M1, vector< T> & Vect){
+ assert(Vect.size() == M1[0].size()); // columns in M1 == rows in Vect
+ vector<T> multiply;
+ initVec(multiply, Vect.size());
+ for (int i=0; i<M1.size(); i++){
+ multiply[i] = 0.0f;
+ for (int j=0; j<M1[i].size(); j++){
+ multiply[i] += M1[i][j] * Vect[j];
+ }
+ }
+ return multiply;
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline float getMeanVec(vector<T>& V){
+ float tSum = 0.0f;
+ for (int n=0; n<V.size(); n++){
+ tSum += V[n];
+ }
+ return tSum / (float)V.size();
+}
+
+template <typename T>
+inline vector<vector<float> > getRotationMatrix3d(T phi, T theta, T psi)
+{
+ vector< vector<float> > M;
+ initMat(M,3,3);
+
+ M[0][0] = cos(theta)*cos(psi);
+ M[0][1] = -cos(phi)*sin(psi)+sin(phi)*sin(theta)*cos(psi);
+ M[0][2] = sin(phi)*sin(psi)+cos(phi)*sin(theta)*cos(psi);
+
+ M[1][0] = cos(theta)*sin(psi);
+ M[1][1] = cos(phi)*cos(psi)+sin(phi)*sin(theta)*sin(psi);
+ M[1][2] = -sin(phi)*cos(psi)+cos(phi)*sin(theta)*sin(psi);
+
+ M[2][0] = -sin(theta);
+ M[2][1] = sin(phi)*cos(theta);
+ M[2][2] = cos(phi)*cos(theta);
+
+ return M;
+}
+
+template <typename T>
+float distance_weightedEuclidean(vector<T> x, vector<T> y, vector<T> w)
+{
+ int count = x.size();
+ if (count <= 0) return 0;
+ float dist = 0.0;
+ for(int k = 0; k < count; k++) dist += w[k] * pow((x[k] - y[k]), 2);
+ return dist;
+}
+
+////--------------------------------------------------------------
+//vector<vector<float> > getRotationMatrix3d(float phi, float theta, float psi)
+//{
+// vector< vector<float> > M;
+// initMat(M,3,3);
+//
+// M[0][0] = cos(theta)*cos(psi);
+// M[0][1] = -cos(phi)*sin(psi)+sin(phi)*sin(theta)*cos(psi);
+// M[0][2] = sin(phi)*sin(psi)+cos(phi)*sin(theta)*cos(psi);
+//
+// M[1][0] = cos(theta)*sin(psi);
+// M[1][1] = cos(phi)*cos(psi)+sin(phi)*sin(theta)*sin(psi);
+// M[1][2] = -sin(phi)*cos(psi)+cos(phi)*sin(theta)*sin(psi);
+//
+// M[2][0] = -sin(theta);
+// M[2][1] = sin(phi)*cos(theta);
+// M[2][2] = cos(phi)*cos(theta);
+//
+// return M;
+//}
+
+//float distance_weightedEuclidean(vector<float> x, vector<float> y, vector<float> w)
+//{
+// int count = x.size();
+// if (count <= 0) return 0;
+// float dist = 0.0;
+// for(int k = 0; k < count; k++) dist += w[k] * pow((x[k] - y[k]), 2);
+// return dist;
+//}
+
+#endif
diff --git a/dependencies/GVF/GVF.cpp b/dependencies/GVF/GVF.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..36ef7d53af50be299a2295bd55d14d3585ea0ea1
--- /dev/null
+++ b/dependencies/GVF/GVF.cpp
@@ -0,0 +1,1375 @@
+/**
+ * Gesture Variation Follower class allows for early gesture recognition and variation tracking
+ *
+ * @details Original algorithm designed and implemented in 2011 at Ircam Centre Pompidou
+ * by Baptiste Caramiaux and Nicola Montecchio. The library has been created and is maintained by Baptiste Caramiaux
+ *
+ * Copyright (C) 2015 Baptiste Caramiaux, Nicola Montecchio
+ * STMS lab Ircam-CRNS-UPMC, University of Padova, Goldsmiths College University of London
+ *
+ * The library is under the GNU Lesser General Public License (LGPL v3)
+ */
+
+#include "GVF.h"
+#include <string.h>
+#include <stdio.h>
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <memory>
+#include <algorithm>
+#include <numeric>
+
+//debug max
+//#include "ext.h"
+
+
+using namespace std;
+
+//--------------------------------------------------------------
+GVF::GVF()
+{
+ config.inputDimensions = 2;
+ config.translate = true;
+ config.segmentation = false;
+
+ parameters.numberParticles = 1000;
+ parameters.tolerance = 0.2f;
+ parameters.resamplingThreshold = 250;
+ parameters.distribution = 0.0f;
+ parameters.alignmentVariance = sqrt(0.000001f);
+ parameters.dynamicsVariance = vector<float>(1,sqrt(0.001f));
+ parameters.scalingsVariance = vector<float>(1,sqrt(0.00001f));
+ parameters.rotationsVariance = vector<float>(1,sqrt(0.0f));
+ parameters.predictionSteps = 1;
+ parameters.dimWeights = vector<float>(1,sqrt(1.0f));
+ parameters.alignmentSpreadingCenter = 0.0;
+ parameters.alignmentSpreadingRange = 0.2;
+ parameters.dynamicsSpreadingCenter = 1.0;
+ parameters.dynamicsSpreadingRange = 0.3;
+ parameters.scalingsSpreadingCenter = 1.0;
+ parameters.scalingsSpreadingRange = 0.3;
+ parameters.rotationsSpreadingCenter = 0.0;
+ parameters.rotationsSpreadingRange = 0.5;
+
+ tolerancesetmanually = false;
+ learningGesture = -1;
+
+ normgen = std::mt19937(rd());
+ rndnorm = new std::normal_distribution<float>(0.0,1.0);
+ unifgen = std::default_random_engine(rd());
+ rndunif = new std::uniform_real_distribution<float>(0.0,1.0);
+
+}
+
+////--------------------------------------------------------------
+//GVF::GVF(GVFConfig _config){
+// setup(_config);
+//}
+//
+////--------------------------------------------------------------
+//GVF::GVF(GVFConfig _config, GVFParameters _parameters){
+// setup(_config, _parameters);
+//}
+//
+////--------------------------------------------------------------
+//void GVF::setup(){
+//
+// // use defualt parameters
+// GVFConfig defaultConfig;
+//
+// defaultConfig.inputDimensions = 2;
+// defaultConfig.translate = true;
+// defaultConfig.segmentation = false;
+//
+// setup(defaultConfig);
+//}
+//
+////--------------------------------------------------------------
+//void GVF::setup(GVFConfig _config){
+//
+// clear(); // just in case
+//
+// learningGesture = -1;
+//
+// // Set configuration:
+// config = _config;
+//
+// // default parameters
+// GVFParameters defaultParameters;
+// defaultParameters.numberParticles = 1000;
+// defaultParameters.tolerance = 0.2f;
+// defaultParameters.resamplingThreshold = 250;
+// defaultParameters.distribution = 0.0f;
+// defaultParameters.alignmentVariance = sqrt(0.000001f);
+// defaultParameters.dynamicsVariance = vector<float>(1,sqrt(0.001f));
+// defaultParameters.scalingsVariance = vector<float>(1,sqrt(0.00001f));
+// defaultParameters.rotationsVariance = vector<float>(1,sqrt(0.0f));
+// defaultParameters.predictionSteps = 1;
+// defaultParameters.dimWeights = vector<float>(1,sqrt(1.0f));
+//
+// // default spreading
+// defaultParameters.alignmentSpreadingCenter = 0.0;
+// defaultParameters.alignmentSpreadingRange = 0.2;
+//
+// defaultParameters.dynamicsSpreadingCenter = 1.0;
+// defaultParameters.dynamicsSpreadingRange = 0.3;
+//
+// defaultParameters.scalingsSpreadingCenter = 1.0;
+// defaultParameters.scalingsSpreadingRange = 0.3;
+//
+// defaultParameters.rotationsSpreadingCenter = 0.0;
+// defaultParameters.rotationsSpreadingRange = 0.0;
+//
+// tolerancesetmanually = false;
+//
+// setup(_config, defaultParameters);
+//
+//}
+//
+////--------------------------------------------------------------
+//void GVF::setup(GVFConfig _config, GVFParameters _parameters)
+//{
+// clear(); // just in case
+// // Set configuration and parameters
+// config = _config;
+// parameters = _parameters;
+// // Init random generators
+// normgen = std::mt19937(rd());
+// rndnorm = new std::normal_distribution<float>(0.0,1.0);
+// unifgen = std::default_random_engine(rd());
+// rndunif = new std::uniform_real_distribution<float>(0.0,1.0);
+//}
+
+//--------------------------------------------------------------
+GVF::~GVF()
+{
+ if (rndnorm != NULL)
+ delete (rndnorm);
+ clear(); // not really necessary but it's polite ;)
+}
+
+//--------------------------------------------------------------
+void GVF::clear()
+{
+ state = STATE_CLEAR;
+ gestureTemplates.clear();
+ mostProbableIndex = -1;
+}
+
+//--------------------------------------------------------------
+void GVF::startGesture()
+{
+ if (state==STATE_FOLLOWING)
+ {
+ restart();
+ }
+ else if (state==STATE_LEARNING)
+ {
+ if (theGesture.getNumberOfTemplates()>0)
+ {
+ if (theGesture.getTemplateLength()>0)
+ addGestureTemplate(theGesture);
+ }
+ theGesture.clear();
+ }
+}
+
+//--------------------------------------------------------------
+void GVF::addObservation(vector<float> data)
+{
+ theGesture.addObservation(data);
+}
+
+//--------------------------------------------------------------
+void GVF::addGestureTemplate(GVFGesture & gestureTemplate)
+{
+
+ // if (getState() != GVF::STATE_LEARNING)
+ // setState(GVF::STATE_LEARNING);
+
+ int inputDimension = gestureTemplate.getNumberDimensions();
+ config.inputDimensions = inputDimension;
+
+ gestureTemplates.push_back(gestureTemplate);
+ activeGestures.push_back(gestureTemplates.size());
+
+ if(minRange.size() == 0){
+ minRange.resize(inputDimension);
+ maxRange.resize(inputDimension);
+ }
+
+ for(int j = 0; j < inputDimension; j++){
+ minRange[j] = INFINITY;
+ maxRange[j] = -INFINITY;
+ }
+
+ // compute min/max from the data
+ for(int i = 0; i < gestureTemplates.size(); i++){
+ GVFGesture& tGestureTemplate = gestureTemplates[i];
+ vector<float>& tMinRange = tGestureTemplate.getMinRange();
+ vector<float>& tMaxRange = tGestureTemplate.getMaxRange();
+ for(int j = 0; j < inputDimension; j++){
+ if(tMinRange[j] < minRange[j]) minRange[j] = tMinRange[j];
+ if(tMaxRange[j] > maxRange[j]) maxRange[j] = tMaxRange[j];
+ }
+ }
+
+ for(int i = 0; i < gestureTemplates.size(); i++){
+ GVFGesture& tGestureTemplate = gestureTemplates[i];
+ tGestureTemplate.setMinRange(minRange);
+ tGestureTemplate.setMaxRange(maxRange);
+ }
+ train();
+
+}
+
+//--------------------------------------------------------------
+void GVF::replaceGestureTemplate(GVFGesture & gestureTemplate, int index)
+{
+ if(gestureTemplate.getNumberDimensions()!=config.inputDimensions)
+ return;
+ if(minRange.size() == 0)
+ {
+ minRange.resize(config.inputDimensions);
+ maxRange.resize(config.inputDimensions);
+ }
+ for(int j = 0; j < config.inputDimensions; j++)
+ {
+ minRange[j] = INFINITY;
+ maxRange[j] = -INFINITY;
+ }
+ if (index<=gestureTemplates.size())
+ gestureTemplates[index-1]=gestureTemplate;
+ for(int i = 0; i < gestureTemplates.size(); i++)
+ {
+ GVFGesture& tGestureTemplate = gestureTemplates[i];
+ vector<float>& tMinRange = tGestureTemplate.getMinRange();
+ vector<float>& tMaxRange = tGestureTemplate.getMaxRange();
+ for(int j = 0; j < config.inputDimensions; j++){
+ if(tMinRange[j] < minRange[j]) minRange[j] = tMinRange[j];
+ if(tMaxRange[j] > maxRange[j]) maxRange[j] = tMaxRange[j];
+ }
+ }
+ for(int i = 0; i < gestureTemplates.size(); i++)
+ {
+ GVFGesture& tGestureTemplate = gestureTemplates[i];
+ tGestureTemplate.setMinRange(minRange);
+ tGestureTemplate.setMaxRange(maxRange);
+ }
+}
+
+////--------------------------------------------------------------
+//vector<float>& GVF::getGestureTemplateSample(int gestureIndex, float cursor)
+//{
+// int frameindex = min((int)(gestureTemplates[gestureIndex].getTemplateLength() - 1),
+// (int)(floor(cursor * gestureTemplates[gestureIndex].getTemplateLength() ) ) );
+// return gestureTemplates[gestureIndex].getTemplate()[frameindex];
+//}
+
+//--------------------------------------------------------------
+GVFGesture & GVF::getGestureTemplate(int index){
+ assert(index < gestureTemplates.size());
+ return gestureTemplates[index];
+}
+
+//--------------------------------------------------------------
+vector<GVFGesture> & GVF::getAllGestureTemplates(){
+ return gestureTemplates;
+}
+
+//--------------------------------------------------------------
+int GVF::getNumberOfGestureTemplates(){
+ return (int)gestureTemplates.size();
+}
+
+//--------------------------------------------------------------
+void GVF::removeGestureTemplate(int index){
+ assert(index < gestureTemplates.size());
+ gestureTemplates.erase(gestureTemplates.begin() + index);
+}
+
+//--------------------------------------------------------------
+void GVF::removeAllGestureTemplates(){
+ gestureTemplates.clear();
+}
+
+//----------------------------------------------
+void GVF::train(){
+
+ if (gestureTemplates.size() > 0)
+ {
+
+ // get the number of dimension in templates
+ config.inputDimensions = gestureTemplates[0].getTemplateDimension();
+
+ dynamicsDim = 2; // hard coded: just speed now
+ scalingsDim = config.inputDimensions;
+
+ // manage orientation
+ if (config.inputDimensions==2) rotationsDim=1;
+ else if (config.inputDimensions==3) rotationsDim=3;
+ else rotationsDim=0;
+
+ // Init state space
+ initVec(classes, parameters.numberParticles); // Vector of gesture class
+ initVec(alignment, parameters.numberParticles); // Vector of phase values (alignment)
+ initMat(dynamics, parameters.numberParticles, dynamicsDim); // Matric of dynamics
+ initMat(scalings, parameters.numberParticles, scalingsDim); // Matrix of scaling
+ if (rotationsDim!=0) initMat(rotations, parameters.numberParticles, rotationsDim); // Matrix of rotations
+ initMat(offsets, parameters.numberParticles, config.inputDimensions);
+ initVec(weights, parameters.numberParticles); // Weights
+
+ initMat(particles, parameters.numberParticles, 3);
+ // std::cout << particles.size() << " " << parameters.numberParticles << std::endl;
+
+ // bayesian elements
+ initVec(prior, parameters.numberParticles);
+ initVec(posterior, parameters.numberParticles);
+ initVec(likelihood, parameters.numberParticles);
+
+
+ initPrior(); // prior on init state values
+ initNoiseParameters(); // init noise parameters (transition and likelihood)
+
+
+ // weighted dimensions in case: default is not weighted
+ if (parameters.dimWeights.size()!=config.inputDimensions){
+ parameters.dimWeights = vector<float> (config.inputDimensions);
+ for(int k = 0; k < config.inputDimensions; k++) parameters.dimWeights[k] = 1.0 / config.inputDimensions;
+ }
+
+ // NORMALIZATION
+// if (config.normalization) { // update the global normaliation factor
+// globalNormalizationFactor = -1.0;
+// // loop on previous gestures already learned
+// // take the max of all the gesture learned ...
+// for (int k=0; k<getNumberOfGestureTemplates() ; k++){
+// for(int j = 0; j < config.inputDimensions; j++){
+// float rangetmp = fabs(getGestureTemplate(k).getMaxRange()[j]-getGestureTemplate(k).getMinRange()[j]);
+// if (rangetmp > globalNormalizationFactor)
+// globalNormalizationFactor=rangetmp;
+// }
+// }
+// }
+// // only for logs
+// if (config.logOn) {
+// vecRef = vector<vector<float> > (parameters.numberParticles);
+// vecObs = vector<float> (config.inputDimensions);
+// stateNoiseDist = vector<float> (parameters.numberParticles);
+// }
+ }
+}
+
+//--------------------------------------------------------------
+//void GVF::initPrior()
+//{
+//
+// // PATICLE FILTERING
+// for (int k = 0; k < parameters.numberParticles; k++)
+// {
+// initPrior(k);
+//
+// classes[k] = activeGestures[k % activeGestures.size()] - 1;
+// }
+//
+//}
+
+//--------------------------------------------------------------
+void GVF::initPrior() //int pf_n)
+{
+ for (int pf_n = 0; pf_n < parameters.numberParticles; pf_n++)
+ {
+ // alignment
+ alignment[pf_n] = ((*rndunif)(unifgen) - 0.5) * parameters.alignmentSpreadingRange + parameters.alignmentSpreadingCenter; // spread phase
+
+
+ // dynamics
+ dynamics[pf_n][0] = ((*rndunif)(unifgen) - 0.5) * parameters.dynamicsSpreadingRange + parameters.dynamicsSpreadingCenter; // spread speed
+ if (dynamics[pf_n].size()>1)
+ {
+ dynamics[pf_n][1] = ((*rndunif)(unifgen) - 0.5) * parameters.dynamicsSpreadingRange; // spread accel
+ }
+
+ // scalings
+ for(int l = 0; l < scalings[pf_n].size(); l++) {
+ scalings[pf_n][l] = ((*rndunif)(unifgen) - 0.5) * parameters.scalingsSpreadingRange + parameters.scalingsSpreadingCenter; // spread scalings
+ }
+
+ // rotations
+ if (rotationsDim!=0)
+ for(int l = 0; l < rotations[pf_n].size(); l++)
+ rotations[pf_n][l] = ((*rndunif)(unifgen) - 0.5) * parameters.rotationsSpreadingRange + parameters.rotationsSpreadingCenter; // spread rotations
+
+ if (config.translate) for(int l = 0; l < offsets[pf_n].size(); l++) offsets[pf_n][l] = 0.0;
+
+
+ prior[pf_n] = 1.0 / (float) parameters.numberParticles;
+
+ // set the posterior to the prior at the initialization
+ posterior[pf_n] = prior[pf_n];
+
+ classes[pf_n] = activeGestures[pf_n % activeGestures.size()] - 1;
+ }
+
+}
+
+//--------------------------------------------------------------
+void GVF::initNoiseParameters() {
+
+ // NOISE (ADDITIVE GAUSSIAN NOISE)
+ // ---------------------------
+
+ if (parameters.dynamicsVariance.size() != dynamicsDim)
+ {
+ float variance = parameters.dynamicsVariance[0];
+ parameters.dynamicsVariance.resize(dynamicsDim);
+ for (int k=0; k<dynamicsDim; k++)
+ parameters.dynamicsVariance[k] = variance;
+ }
+
+ if (parameters.scalingsVariance.size() != scalingsDim)
+ {
+ float variance = parameters.scalingsVariance[0];
+ parameters.scalingsVariance.resize(scalingsDim);
+ for (int k=0; k<scalingsDim; k++)
+ parameters.scalingsVariance[k] = variance;
+ }
+
+ if (rotationsDim!=0)
+ {
+ if (parameters.rotationsVariance.size() != rotationsDim)
+ {
+ float variance = parameters.rotationsVariance[0];
+ parameters.rotationsVariance.resize(rotationsDim);
+ for (int k=0; k<rotationsDim; k++)
+ parameters.rotationsVariance[k] = variance;
+ }
+ }
+
+ // ADAPTATION OF THE TOLERANCE IF DEFAULT PARAMTERS
+ // ---------------------------
+ if (!tolerancesetmanually){
+ float obsMeanRange = 0.0f;
+ for (int gt=0; gt<gestureTemplates.size(); gt++) {
+ for (int d=0; d<config.inputDimensions; d++)
+ obsMeanRange += (gestureTemplates[gt].getMaxRange()[d] - gestureTemplates[gt].getMinRange()[d])
+ /config.inputDimensions;
+ }
+ obsMeanRange /= gestureTemplates.size();
+ parameters.tolerance = obsMeanRange / 4.0f; // dividing by an heuristic factor [to be learned?]
+ }
+}
+
+//--------------------------------------------------------------
+void GVF::setState(GVFState _state, vector<int> indexes)
+{
+ switch (_state)
+ {
+ case STATE_CLEAR:
+ clear();
+ theGesture.clear();
+ break;
+
+ case STATE_LEARNING:
+ if ((state==STATE_LEARNING) && (theGesture.getNumberOfTemplates()>0))
+ {
+ if (learningGesture==-1)
+ addGestureTemplate(theGesture);
+ else
+ {
+ replaceGestureTemplate(theGesture, learningGesture);
+ learningGesture=-1;
+ }
+ if (indexes.size()!=0)
+ learningGesture=indexes[0];
+ }
+ state = _state;
+ theGesture.clear();
+ break;
+
+ case STATE_FOLLOWING:
+ if ((state==STATE_LEARNING) && (theGesture.getNumberOfTemplates()>0))
+ {
+ if (learningGesture==-1)
+ addGestureTemplate(theGesture);
+ else
+ {
+ replaceGestureTemplate(theGesture, learningGesture);
+ learningGesture=-1;
+ }
+ }
+ if (gestureTemplates.size() > 0)
+ {
+ train();
+ state = _state;
+ }
+ else
+ state = STATE_CLEAR;
+ theGesture.clear();
+ break;
+
+ default:
+ theGesture.clear();
+ break;
+ }
+}
+
+//--------------------------------------------------------------
+GVF::GVFState GVF::getState()
+{
+ return state;
+}
+
+////--------------------------------------------------------------
+//int GVF::getDynamicsDimension(){
+// return dynamicsDim;
+//}
+
+//--------------------------------------------------------------
+vector<int> GVF::getGestureClasses()
+{
+ return classes;
+}
+
+////--------------------------------------------------------------
+//vector<float> GVF::getAlignment(){
+// return alignment;
+//}
+//
+////--------------------------------------------------------------
+//vector<float> GVF::getEstimatedAlignment(){
+// return estimatedAlignment;
+//}
+//
+////--------------------------------------------------------------
+//vector< vector<float> > GVF::getDynamics(){
+// return dynamics;
+//}
+//
+////--------------------------------------------------------------
+//vector< vector<float> > GVF::getEstimatedDynamics(){
+// return estimatedDynamics;
+//}
+//
+////--------------------------------------------------------------
+//vector< vector<float> > GVF::getScalings(){
+// return scalings;
+//}
+//
+////--------------------------------------------------------------
+//vector< vector<float> > GVF::getEstimatedScalings(){
+// return estimatedScalings;
+//}
+//
+////--------------------------------------------------------------
+//vector< vector<float> > GVF::getRotations(){
+// return rotations;
+//}
+//
+////--------------------------------------------------------------
+//vector< vector<float> > GVF::getEstimatedRotations(){
+// return estimatedRotations;
+//}
+
+////--------------------------------------------------------------
+//vector<float> GVF::getEstimatedProbabilities(){
+// return estimatedProbabilities;
+//}
+//
+////--------------------------------------------------------------
+//vector<float> GVF::getEstimatedLikelihoods(){
+// return estimatedLikelihoods;
+//}
+//
+////--------------------------------------------------------------
+//vector<float> GVF::getWeights(){
+// return weights;
+//}
+//
+////--------------------------------------------------------------
+//vector<float> GVF::getPrior(){
+// return prior;
+//}
+
+////--------------------------------------------------------------
+//vector<vector<float> > GVF::getVecRef() {
+// return vecRef;
+//}
+//
+////--------------------------------------------------------------
+//vector<float> GVF::getVecObs() {
+// return vecObs;
+//}
+//
+////--------------------------------------------------------------
+//vector<float> GVF::getStateNoiseDist(){
+// return stateNoiseDist;
+//}
+
+////--------------------------------------------------------------
+//int GVF::getScalingsDim(){
+// return scalingsDim;
+//}
+//
+////--------------------------------------------------------------
+//int GVF::getRotationsDim(){
+// return rotationsDim;
+//}
+
+//--------------------------------------------------------------
+void GVF::restart()
+{
+ theGesture.clear();
+ initPrior();
+}
+
+#pragma mark - PARTICLE FILTERING
+
+//--------------------------------------------------------------
+void GVF::updatePrior(int n) {
+
+ // Update alignment / dynamics / scalings
+ float L = gestureTemplates[classes[n]].getTemplateLength();
+ alignment[n] += (*rndnorm)(normgen) * parameters.alignmentVariance + dynamics[n][0]/L; // + dynamics[n][1]/(L*L);
+
+ if (dynamics[n].size()>1){
+ dynamics[n][0] += (*rndnorm)(normgen) * parameters.dynamicsVariance[0] + dynamics[n][1]/L;
+ dynamics[n][1] += (*rndnorm)(normgen) * parameters.dynamicsVariance[1];
+ }
+ else {
+ dynamics[n][0] += (*rndnorm)(normgen) * parameters.dynamicsVariance[0];
+ }
+
+ // for(int l= 0; l < dynamics[n].size(); l++) dynamics[n][l] += (*rndnorm)(normgen) * parameters.dynamicsVariance[l];
+ for(int l= 0; l < scalings[n].size(); l++) scalings[n][l] += (*rndnorm)(normgen) * parameters.scalingsVariance[l];
+ if (rotationsDim!=0) for(int l= 0; l < rotations[n].size(); l++) rotations[n][l] += (*rndnorm)(normgen) * parameters.rotationsVariance[l];
+
+ // update prior (bayesian incremental inference)
+ prior[n] = posterior[n];
+}
+
+//--------------------------------------------------------------
+void GVF::updateLikelihood(vector<float> obs, int n)
+{
+
+// if (config.normalization) for (int kk=0; kk<vobs.size(); kk++) vobs[kk] = vobs[kk] / globalNormalizationFactor;
+
+ if(alignment[n] < 0.0)
+ {
+ alignment[n] = fabs(alignment[n]); // re-spread at the beginning
+// if (config.segmentation)
+// classes[n] = n % getNumberOfGestureTemplates();
+ }
+ else if(alignment[n] > 1.0)
+ {
+ if (config.segmentation)
+ {
+// alignment[n] = fabs(1.0-alignment[n]); // re-spread at the beginning
+ alignment[n] = fabs((*rndunif)(unifgen) * 0.5); //
+ classes[n] = n % getNumberOfGestureTemplates();
+ offsets[n] = obs;
+ // dynamics
+ dynamics[n][0] = ((*rndunif)(unifgen) - 0.5) * parameters.dynamicsSpreadingRange + parameters.dynamicsSpreadingCenter; // spread speed
+ if (dynamics[n].size()>1)
+ dynamics[n][1] = ((*rndunif)(unifgen) - 0.5) * parameters.dynamicsSpreadingRange;
+ // scalings
+ for(int l = 0; l < scalings[n].size(); l++)
+ scalings[n][l] = ((*rndunif)(unifgen) - 0.5) * parameters.scalingsSpreadingRange + parameters.scalingsSpreadingCenter; // spread scalings
+ // rotations
+ if (rotationsDim!=0)
+ for(int l = 0; l < rotations[n].size(); l++)
+ rotations[n][l] = ((*rndunif)(unifgen) - 0.5) * parameters.rotationsSpreadingRange + parameters.rotationsSpreadingCenter; // spread rotations
+ // prior
+ prior[n] = 1/(float)parameters.numberParticles;
+ }
+ else{
+ alignment[n] = fabs(2.0-alignment[n]); // re-spread at the end
+ }
+ }
+
+ vector<float> vobs(config.inputDimensions);
+ setVec(vobs, obs);
+
+ if (config.translate)
+ for (int j=0; j < config.inputDimensions; j++)
+ vobs[j] = vobs[j] - offsets[n][j];
+
+
+ // take vref from template at the given alignment
+ int gestureIndex = classes[n];
+ float cursor = alignment[n];
+ int frameindex = min((int)(gestureTemplates[gestureIndex].getTemplateLength() - 1),
+ (int)(floor(cursor * gestureTemplates[gestureIndex].getTemplateLength() ) ) );
+// return gestureTemplates[gestureIndex].getTemplate()[frameindex];
+ vector<float> vref = gestureTemplates[gestureIndex].getTemplate()[frameindex];; //getGestureTemplateSample(classes[n], alignment[n]);
+
+ // Apply scaling coefficients
+ for (int k=0;k < config.inputDimensions; k++)
+ {
+// if (config.normalization) vref[k] = vref[k] / globalNormalizationFactor;
+ vref[k] *= scalings[n][k];
+ }
+
+ // Apply rotation coefficients
+ if (config.inputDimensions==2) {
+ float tmp0=vref[0]; float tmp1=vref[1];
+ vref[0] = cos(rotations[n][0])*tmp0 - sin(rotations[n][0])*tmp1;
+ vref[1] = sin(rotations[n][0])*tmp0 + cos(rotations[n][0])*tmp1;
+ }
+ else if (config.inputDimensions==3) {
+ // Rotate template sample according to the estimated angles of rotations (3d)
+ vector<vector< float> > RotMatrix = getRotationMatrix3d(rotations[n][0],rotations[n][1],rotations[n][2]);
+ vref = multiplyMat(RotMatrix, vref);
+ }
+
+ // weighted euclidean distance
+ float dist = distance_weightedEuclidean(vref,vobs,parameters.dimWeights);
+
+ if(parameters.distribution == 0.0f){ // Gaussian distribution
+ likelihood[n] = exp(- dist * 1 / (parameters.tolerance * parameters.tolerance));
+ }
+ else { // Student's distribution
+ likelihood[n] = pow(dist/parameters.distribution + 1, -parameters.distribution/2 - 1); // dimension is 2 .. pay attention if editing]
+ }
+// // if log on keep track on vref and vobs
+// if (config.logOn){
+// vecRef.push_back(vref);
+// vecObs = vobs;
+// }
+}
+
+//--------------------------------------------------------------
+void GVF::updatePosterior(int n) {
+ posterior[n] = prior[n] * likelihood[n];
+}
+
+//--------------------------------------------------------------
+GVFOutcomes & GVF::update(vector<float> & observation)
+{
+
+ if (state != GVF::STATE_FOLLOWING) setState(GVF::STATE_FOLLOWING);
+
+ theGesture.addObservation(observation);
+ vector<float> obs = theGesture.getLastObservation();
+
+ // std::cout << obs[0] << " " << obs[0] << " "
+ // << gestureTemplates[0].getTemplate()[20][0] << " " << gestureTemplates[0].getTemplate()[20][1] << " "
+ // << gestureTemplates[1].getTemplate()[20][0] << " " << gestureTemplates[1].getTemplate()[20][1] << std::endl;
+
+
+ // for each particle: perform updates of state space / likelihood / prior (weights)
+ float sumw = 0.0;
+ for(int n = 0; n< parameters.numberParticles; n++)
+ {
+
+ for (int m=0; m<parameters.predictionSteps; m++)
+ {
+ updatePrior(n);
+ updateLikelihood(obs, n);
+ updatePosterior(n);
+ }
+
+ sumw += posterior[n]; // sum posterior to normalise the distrib afterwards
+
+ particles[n][0] = alignment[n];
+ particles[n][1] = scalings[n][0];
+ particles[n][2] = classes[n];
+ }
+
+ // normalize the weights and compute the resampling criterion
+ float dotProdw = 0.0;
+ for (int k = 0; k < parameters.numberParticles; k++){
+ posterior[k] /= sumw;
+ dotProdw += posterior[k] * posterior[k];
+ }
+ // avoid degeneracy (no particles active, i.e. weight = 0) by resampling
+ if( (1./dotProdw) < parameters.resamplingThreshold)
+ resampleAccordingToWeights(obs);
+
+ // estimate outcomes
+ estimates();
+
+ return outcomes;
+
+}
+
+//--------------------------------------------------------------
+void GVF::resampleAccordingToWeights(vector<float> obs)
+{
+ // covennient
+ int numOfPart = parameters.numberParticles;
+
+ // cumulative dist
+ vector<float> c(numOfPart);
+
+ // tmp matrices
+ vector<int> oldClasses;
+ vector<float> oldAlignment;
+ vector< vector<float> > oldDynamics;
+ vector< vector<float> > oldScalings;
+ vector< vector<float> > oldRotations;
+
+ setVec(oldClasses, classes);
+ setVec(oldAlignment, alignment);
+ setMat(oldDynamics, dynamics);
+ setMat(oldScalings, scalings);
+ if (rotationsDim!=0) setMat(oldRotations, rotations);
+
+
+ c[0] = 0;
+ for(int i = 1; i < numOfPart; i++) c[i] = c[i-1] + posterior[i];
+
+
+ float u0 = (*rndunif)(unifgen)/numOfPart;
+
+ int i = 0;
+ for (int j = 0; j < numOfPart; j++)
+ {
+ float uj = u0 + (j + 0.) / numOfPart;
+
+ while (uj > c[i] && i < numOfPart - 1){
+ i++;
+ }
+
+ classes[j] = oldClasses[i];
+ alignment[j] = oldAlignment[i];
+
+ for (int l=0;l<dynamicsDim;l++) dynamics[j][l] = oldDynamics[i][l];
+ for (int l=0;l<scalingsDim;l++) scalings[j][l] = oldScalings[i][l];
+ if (rotationsDim!=0) for (int l=0;l<rotationsDim;l++) rotations[j][l] = oldRotations[i][l];
+
+ // update posterior (partilces' weights)
+ posterior[j] = 1.0/(float)numOfPart;
+ }
+
+}
+
+
+//--------------------------------------------------------------
+void GVF::estimates(){
+
+
+ int numOfPart = parameters.numberParticles;
+ vector<float> probabilityNormalisation(getNumberOfGestureTemplates());
+ setVec(probabilityNormalisation, 0.0f, getNumberOfGestureTemplates()); // rows are gestures
+ setVec(estimatedAlignment, 0.0f, getNumberOfGestureTemplates()); // rows are gestures
+ setMat(estimatedDynamics, 0.0f, getNumberOfGestureTemplates(), dynamicsDim); // rows are gestures, cols are features + probabilities
+ setMat(estimatedScalings, 0.0f, getNumberOfGestureTemplates(), scalingsDim); // rows are gestures, cols are features + probabilities
+ if (rotationsDim!=0) setMat(estimatedRotations, 0.0f, getNumberOfGestureTemplates(), rotationsDim); // ..
+ setVec(estimatedProbabilities, 0.0f, getNumberOfGestureTemplates()); // rows are gestures
+ setVec(estimatedLikelihoods, 0.0f, getNumberOfGestureTemplates()); // rows are gestures
+
+ // float sumposterior = 0.;
+
+ for(int n = 0; n < numOfPart; n++)
+ {
+ probabilityNormalisation[classes[n]] += posterior[n];
+ }
+
+
+ // compute the estimated features and likelihoods
+ for(int n = 0; n < numOfPart; n++)
+ {
+
+ // sumposterior += posterior[n];
+ estimatedAlignment[classes[n]] += alignment[n] * posterior[n];
+
+ for(int m = 0; m < dynamicsDim; m++)
+ estimatedDynamics[classes[n]][m] += dynamics[n][m] * (posterior[n]/probabilityNormalisation[classes[n]]);
+
+ for(int m = 0; m < scalingsDim; m++)
+ estimatedScalings[classes[n]][m] += scalings[n][m] * (posterior[n]/probabilityNormalisation[classes[n]]);
+
+ if (rotationsDim!=0)
+ for(int m = 0; m < rotationsDim; m++)
+ estimatedRotations[classes[n]][m] += rotations[n][m] * (posterior[n]/probabilityNormalisation[classes[n]]);
+
+ if (!isnan(posterior[n]))
+ estimatedProbabilities[classes[n]] += posterior[n];
+ estimatedLikelihoods[classes[n]] += likelihood[n];
+ }
+
+ // calculate most probable index during scaling...
+ float maxProbability = 0.0f;
+ mostProbableIndex = -1;
+
+ for(int gi = 0; gi < getNumberOfGestureTemplates(); gi++)
+ {
+ if(estimatedProbabilities[gi] > maxProbability){
+ maxProbability = estimatedProbabilities[gi];
+ mostProbableIndex = gi;
+ }
+ }
+ // std::cout << estimatedProbabilities[0] << " " << estimatedProbabilities[1] << std::endl;
+
+ // outcomes.estimations.clear();
+ outcomes.likelihoods.clear();
+ outcomes.alignments.clear();
+ outcomes.scalings.clear();
+ outcomes.dynamics.clear();
+ outcomes.rotations.clear();
+
+ // most probable gesture index
+ outcomes.likeliestGesture = mostProbableIndex;
+
+ // Fill estimation for each gesture
+ for (int gi = 0; gi < gestureTemplates.size(); ++gi) {
+
+ // GVFEstimation estimation;
+ outcomes.likelihoods.push_back(estimatedProbabilities[gi]);
+ outcomes.alignments.push_back(estimatedAlignment[gi]);
+ // estimation.probability = estimatedProbabilities[gi];
+ // estimation.alignment = estimatedAlignment[gi];
+
+
+ vector<float> gDynamics(dynamicsDim, 0.0);
+ for (int j = 0; j < dynamicsDim; ++j) gDynamics[j] = estimatedDynamics[gi][j];
+ outcomes.dynamics.push_back(gDynamics);
+
+ vector<float> gScalings(scalingsDim, 0.0);
+ for (int j = 0; j < scalingsDim; ++j) gScalings[j] = estimatedScalings[gi][j];
+ outcomes.scalings.push_back(gScalings);
+
+ vector<float> gRotations;
+ if (rotationsDim!=0)
+ {
+ gRotations.resize(rotationsDim);
+ for (int j = 0; j < rotationsDim; ++j) gRotations[j] = estimatedRotations[gi][j];
+ outcomes.rotations.push_back(gRotations);
+ }
+
+ // estimation.likelihood = estimatedLikelihoods[gi];
+
+ // push estimation for gesture gi in outcomes
+ // outcomes.estimations.push_back(estimation);
+ }
+
+
+ // assert(outcomes.estimations.size() == gestureTemplates.size());
+
+}
+
+////--------------------------------------------------------------
+//int GVF::getMostProbableGestureIndex()
+//{
+// return mostProbableIndex;
+//}
+
+////--------------------------------------------------------------
+//GVFOutcomes GVF::getOutcomes()
+//{
+// return outcomes;
+//}
+
+////--------------------------------------------------------------
+//GVFEstimation GVF::getTemplateRecogInfo(int templateNumber)
+//{
+// if (getOutcomes().estimations.size() <= templateNumber) {
+// GVFEstimation estimation;
+// return estimation; // blank
+// }
+// else
+// return getOutcomes().estimations[templateNumber];
+//}
+//
+////--------------------------------------------------------------
+//GVFEstimation GVF::getRecogInfoOfMostProbable() // FIXME: Rename!
+//{
+// int indexMostProbable = getMostProbableGestureIndex();
+//
+// if ((getState() == GVF::STATE_FOLLOWING) && (getMostProbableGestureIndex() != -1)) {
+// return getTemplateRecogInfo(indexMostProbable);
+// }
+// else {
+// GVFEstimation estimation;
+// return estimation; // blank
+// }
+//}
+
+
+////--------------------------------------------------------------
+//vector<float> & GVF::getGestureProbabilities()
+//{
+// gestureProbabilities.resize(getNumberOfGestureTemplates());
+// setVec(gestureProbabilities, 0.0f);
+// for(int n = 0; n < parameters.numberParticles; n++)
+// gestureProbabilities[classes[n]] += posterior[n];
+//
+// return gestureProbabilities;
+//}
+
+//--------------------------------------------------------------
+const vector<vector<float> > & GVF::getParticlesPositions(){
+ return particles;
+}
+
+////--------------------------------------------------------------
+//void GVF::setParameters(GVFParameters _parameters){
+//
+// // if the number of particles has changed, we have to re-allocate matrices
+// if (_parameters.numberParticles != parameters.numberParticles)
+// {
+// parameters = _parameters;
+//
+// // minimum number of particles allowed
+// if (parameters.numberParticles < 4) parameters.numberParticles = 4;
+//
+// // re-learn
+// train();
+//
+// // adapt the resampling threshold in case if RT < NS
+// if (parameters.numberParticles <= parameters.resamplingThreshold)
+// parameters.resamplingThreshold = parameters.numberParticles / 4;
+//
+// }
+// else
+// parameters = _parameters;
+//
+//
+//}
+//
+//GVFParameters GVF::getParameters(){
+// return parameters;
+//}
+
+//--------------------------------------------------------------
+// Update the number of particles
+void GVF::setNumberOfParticles(int numberOfParticles){
+
+ parameters.numberParticles = numberOfParticles;
+
+ if (parameters.numberParticles < 4) // minimum number of particles allowed
+ parameters.numberParticles = 4;
+
+ train();
+
+ if (parameters.numberParticles <= parameters.resamplingThreshold) {
+ parameters.resamplingThreshold = parameters.numberParticles / 4;
+ }
+
+}
+
+//--------------------------------------------------------------
+int GVF::getNumberOfParticles(){
+ return parameters.numberParticles; // Return the number of particles
+}
+
+//--------------------------------------------------------------
+void GVF::setActiveGestures(vector<int> activeGestureIds)
+{
+ int argmax = *std::max_element(activeGestureIds.begin(), activeGestureIds.end());
+ if (activeGestureIds[argmax] <= gestureTemplates.size())
+ {
+ activeGestures = activeGestureIds;
+ }
+ else
+ {
+ activeGestures.resize(gestureTemplates.size());
+ std::iota(activeGestures.begin(), activeGestures.end(), 1);
+ }
+}
+
+//--------------------------------------------------------------
+void GVF::setPredictionSteps(int predictionSteps)
+{
+ if (predictionSteps<1)
+ parameters.predictionSteps = 1;
+ else
+ parameters.predictionSteps = predictionSteps;
+}
+
+//--------------------------------------------------------------
+int GVF::getPredictionSteps()
+{
+ return parameters.predictionSteps; // Return the number of particles
+}
+
+//--------------------------------------------------------------
+// Update the resampling threshold used to avoid degeneracy problem
+void GVF::setResamplingThreshold(int _resamplingThreshold){
+ if (_resamplingThreshold >= parameters.numberParticles)
+ _resamplingThreshold = floor(parameters.numberParticles/2.0f);
+ parameters.resamplingThreshold = _resamplingThreshold;
+}
+
+//--------------------------------------------------------------
+// Return the resampling threshold used to avoid degeneracy problem
+int GVF::getResamplingThreshold(){
+ return parameters.resamplingThreshold;
+}
+
+//--------------------------------------------------------------
+// Update the standard deviation of the observation distribution
+// this value acts as a tolerance for the algorithm
+// low value: less tolerant so more precise but can diverge
+// high value: more tolerant so less precise but converge more easily
+void GVF::setTolerance(float _tolerance){
+ if (_tolerance <= 0.0) _tolerance = 0.1;
+ parameters.tolerance = _tolerance;
+ tolerancesetmanually = true;
+}
+
+//--------------------------------------------------------------
+float GVF::getTolerance(){
+ return parameters.tolerance;
+}
+
+////--------------------------------------------------------------
+void GVF::setDistribution(float _distribution){
+ //nu = _distribution;
+ parameters.distribution = _distribution;
+}
+//
+////--------------------------------------------------------------
+//float GVF::getDistribution(){
+// return parameters.distribution;
+//}
+
+//void GVF::setDimWeights(vector<float> dimWeights){
+// if (dimWeights.size()!=parameters.dimWeights.size())
+// parameters.dimWeights.resize(dimWeights.size());
+// parameters.dimWeights = dimWeights;
+//}
+//
+//vector<float> GVF::getDimWeights(){
+// return parameters.dimWeights;
+//}
+
+
+//// VARIANCE COEFFICIENTS: PHASE
+////--------------------------------------------------------------
+//void GVF::setAlignmentVariance(float alignmentVariance){
+// parameters.alignmentVariance = sqrt(alignmentVariance);
+//}
+////--------------------------------------------------------------
+//float GVF::getAlignmentVariance(){
+// return parameters.alignmentVariance;
+//}
+
+
+// VARIANCE COEFFICIENTS: DYNAMICS
+//--------------------------------------------------------------
+//void GVF::setDynamicsVariance(float dynVariance)
+//{
+// for (int k=0; k< parameters.dynamicsVariance.size(); k++)
+// parameters.dynamicsVariance[k] = dynVariance;
+//}
+//--------------------------------------------------------------
+void GVF::setDynamicsVariance(float dynVariance, int dim)
+{
+ if (dim == -1)
+ {
+ for (int k=0; k< parameters.dynamicsVariance.size(); k++)
+ parameters.dynamicsVariance[k] = dynVariance;
+ }
+ else
+ {
+ if (dim<parameters.dynamicsVariance.size())
+ parameters.dynamicsVariance[dim-1] = dynVariance;
+ }
+}
+
+//--------------------------------------------------------------
+void GVF::setDynamicsVariance(vector<float> dynVariance)
+{
+ parameters.dynamicsVariance = dynVariance;
+}
+//--------------------------------------------------------------
+vector<float> GVF::getDynamicsVariance()
+{
+ return parameters.dynamicsVariance;
+}
+
+//--------------------------------------------------------------
+void GVF::setScalingsVariance(float scaleVariance, int dim)
+{
+ if (dim == -1)
+ {
+ for (int k=0; k< parameters.scalingsVariance.size(); k++)
+ parameters.scalingsVariance[k] = scaleVariance;
+ }
+ else
+ {
+ if (dim<parameters.scalingsVariance.size())
+ parameters.scalingsVariance[dim-1] = scaleVariance;
+ }
+}
+
+//--------------------------------------------------------------
+void GVF::setScalingsVariance(vector<float> scaleVariance)
+{
+ parameters.scaleVariance = scaleVariance;
+}
+
+//--------------------------------------------------------------
+vector<float> GVF::getScalingsVariance()
+{
+ return parameters.scalingsVariance;
+}
+
+//--------------------------------------------------------------
+void GVF::setRotationsVariance(float rotationVariance, int dim)
+{
+ if (dim == -1)
+ {
+ for (int k=0; k< parameters.rotationsVariance.size(); k++)
+ parameters.rotationsVariance[k] = rotationVariance;
+ }
+ else
+ {
+ if (dim<parameters.rotationsVariance.size())
+ parameters.scalingsVariance[dim-1] = rotationVariance;
+ }
+}
+
+//--------------------------------------------------------------
+void GVF::setRotationsVariance(vector<float> rotationVariance)
+{
+ parameters.scaleVariance = rotationVariance;
+}
+
+//--------------------------------------------------------------
+vector<float> GVF::getRotationsVariance()
+{
+ return parameters.rotationsVariance;
+}
+
+//--------------------------------------------------------------
+void GVF::setSpreadDynamics(float center, float range, int dim)
+{
+ parameters.dynamicsSpreadingCenter = center;
+ parameters.dynamicsSpreadingRange = range;
+}
+
+//--------------------------------------------------------------
+void GVF::setSpreadScalings(float center, float range, int dim)
+{
+ parameters.scalingsSpreadingCenter = center;
+ parameters.scalingsSpreadingRange = range;
+}
+
+//--------------------------------------------------------------
+void GVF::setSpreadRotations(float center, float range, int dim)
+{
+ parameters.rotationsSpreadingCenter = center;
+ parameters.rotationsSpreadingRange = range;
+}
+
+//--------------------------------------------------------------
+void GVF::translate(bool translateFlag)
+{
+ config.translate = translateFlag;
+}
+
+//--------------------------------------------------------------
+void GVF::segmentation(bool segmentationFlag)
+{
+ config.segmentation = segmentationFlag;
+}
+
+
+// UTILITIES
+
+//--------------------------------------------------------------
+// Save function. This function is used by applications to save the
+// vocabulary in a text file given by filename (filename is also the complete path + filename)
+void GVF::saveTemplates(string filename){
+
+ std::string directory = filename;
+
+ std::ofstream file_write(directory.c_str());
+
+ for(int i=0; i < gestureTemplates.size(); i++) // Number of gesture templates
+ {
+ file_write << "template " << i << " " << config.inputDimensions << endl;
+ vector<vector<float> > templateTmp = gestureTemplates[i].getTemplate();
+ for(int j = 0; j < templateTmp.size(); j++)
+ {
+ for(int k = 0; k < config.inputDimensions; k++)
+ file_write << templateTmp[j][k] << " ";
+ file_write << endl;
+ }
+ }
+ file_write.close();
+
+}
+
+
+
+
+//--------------------------------------------------------------
+// Load function. This function is used by applications to load a vocabulary
+// given by filename (filename is also the complete path + filename)
+void GVF::loadTemplates(string filename){
+ // clear();
+ //
+
+ GVFGesture loadedGesture;
+ loadedGesture.clear();
+
+ ifstream infile;
+ stringstream doung;
+
+ infile.open (filename.c_str(), ifstream::in);
+ //
+ string line;
+ vector<string> list;
+ int cl = -1;
+ while(!infile.eof())
+ {
+ cl++;
+ infile >> line;
+
+ list.push_back(line);
+ }
+
+ int k = 0;
+ int template_id = -1;
+ int template_dim = 0;
+
+
+ while (k < (list.size() - 1)){ // TODO to be changed if dim>2
+
+
+ if (!strcmp(list[k].c_str(),"template"))
+ {
+ template_id = atoi(list[k+1].c_str());
+ template_dim = atoi(list[k+2].c_str());
+ k = k + 3;
+
+ if (loadedGesture.getNumberOfTemplates() > 0){
+ addGestureTemplate(loadedGesture);
+ loadedGesture.clear();
+ }
+ }
+
+ if (template_dim <= 0){
+ //post("bug dim = -1");
+ }
+ else{
+
+ vector<float> vect(template_dim);
+
+ for (int kk = 0; kk < template_dim; kk++)
+ vect[kk] = (float) atof(list[k + kk].c_str());
+
+ loadedGesture.addObservation(vect);
+ }
+ k += template_dim;
+
+ }
+
+ if (loadedGesture.getTemplateLength() > 0){
+ addGestureTemplate(loadedGesture);
+ loadedGesture.clear();
+ }
+
+ infile.close();
+}
+
+
+
+
+
+
diff --git a/dependencies/GVF/GVF.h b/dependencies/GVF/GVF.h
new file mode 100755
index 0000000000000000000000000000000000000000..647fe7c6ea372e9ec8fc265a946fb3a44c91abcc
--- /dev/null
+++ b/dependencies/GVF/GVF.h
@@ -0,0 +1,487 @@
+/**
+ * Gesture Variation Follower class allows for early gesture recognition and variation tracking
+ *
+ * @details Original algorithm designed and implemented in 2011 at Ircam Centre Pompidou
+ * by Baptiste Caramiaux and Nicola Montecchio. The library has been created and is maintained by Baptiste Caramiaux
+ *
+ * Copyright (C) 2015 Baptiste Caramiaux, Nicola Montecchio
+ * STMS lab Ircam-CRNS-UPMC, University of Padova, Goldsmiths College University of London
+ *
+ * The library is under the GNU Lesser General Public License (LGPL v3)
+ */
+
+
+#ifndef _H_GVF
+#define _H_GVF
+
+#include "GVFUtils.h"
+#include "GVFGesture.h"
+#include <random>
+#include <iostream>
+#include <iomanip>
+#include <string>
+#include <map>
+#include <random>
+#include <cmath>
+
+
+using namespace std;
+
+class GVF
+{
+
+public:
+
+ /**
+ * GVF possible states
+ */
+ enum GVFState
+ {
+ STATE_CLEAR = 0, /**< STATE_CLEAR: clear the GVF and be in standby */
+ STATE_LEARNING, /**< STATE_LEARNING: recording mode, input gestures are added to the templates */
+ STATE_FOLLOWING, /**< STATE_FOLLOWING: tracking mode, input gestures are classifed and their variations tracked (need the GVF to be trained) */
+ STATE_BYPASS /**< STATE_BYPASS: by pass GVF but does not erase templates or training */
+ };
+
+
+#pragma mark - Constructors
+
+ /**
+ * GVF default constructor
+ * @details use default configuration and parameters, can be changed using accessors
+ */
+ GVF();
+
+ /**
+ * GVF default destructor
+ */
+ ~GVF();
+
+#pragma mark - Gesture templates
+
+ /**
+ * Start a gesture either to be recorded or followed
+ */
+ void startGesture();
+
+ /**
+ * Add an observation to a gesture template
+ * @details
+ * @param data vector of features
+ */
+ void addObservation(vector<float> data);
+
+ /**
+ * Add gesture template to the vocabulary
+ *
+ * @details a gesture template is a GVFGesture object and can be added directly to the vocabulqry or
+ * recorded gesture templates by using this method
+ * @param gestureTemplate the gesture template to be recorded
+ */
+ void addGestureTemplate(GVFGesture & gestureTemplate);
+
+ /**
+ * Replace a specific gesture template by another
+ *
+ * @param gestureTemplate the gesture template to be used
+ * @param index the gesture index (as integer) to be replaced
+ */
+ void replaceGestureTemplate(GVFGesture & gestureTemplate, int index);
+
+ /**
+ * Remove a specific template
+ *
+ * @param index the gesture index (as integer) to be removed
+ */
+ void removeGestureTemplate(int index);
+
+ /**
+ * Remove every recorded gesture template
+ */
+ void removeAllGestureTemplates();
+
+ /**
+ * Get a specific gesture template a gesture template by another
+ *
+ * @param index the index of the template to be returned
+ * @return the template
+ */
+ GVFGesture & getGestureTemplate(int index);
+
+ /**
+ * Get every recorded gesture template
+ *
+ * @return the vecotr of gesture templates
+ */
+ vector<GVFGesture> & getAllGestureTemplates();
+
+ /**
+ * Get number of gesture templates in the vocabulary
+ * @return the number of templates
+ */
+ int getNumberOfGestureTemplates();
+
+ /**
+ * Get gesture classes
+ */
+ vector<int> getGestureClasses();
+
+
+#pragma mark - Recognition and tracking
+
+ /**
+ * Set the state of GVF
+ * @param _state the state to be given to GVF, it is a GVFState
+ * @param indexes an optional argument providing a list of gesture index.
+ * In learning mode the index of the gesture being recorded can be given as an argument
+ * since the type is vector<int>, it should be something like '{3}'. In following mode, the list of indexes
+ * is the list of active gestures to be considered in the recognition/tracking.
+ */
+ void setState(GVFState _state, vector<int> indexes = vector<int>());
+
+ /**
+ * Return the current state of GVF
+ * @return GVFState the current state
+ */
+ GVFState getState();
+
+ /**
+ * Compute the estimated gesture and its potential variations
+ *
+ * @details infers the probability that the current observation belongs to
+ * one of the recorded gesture template and track the variations of this gesture
+ * according to each template
+ *
+ * @param observation vector of the observation data at current time
+ * @return the estimated probabilities and variaitons relative to each template
+ */
+ GVFOutcomes & update(vector<float> & observation);
+
+ /**
+ * Define a subset of gesture templates on which to perform the recognition
+ * and variation tracking
+ *
+ * @details By default every recorded gesture template is considered
+ * @param set of gesture template index to consider
+ */
+ void setActiveGestures(vector<int> activeGestureIds);
+
+ /**
+ * Restart GVF
+ * @details re-sample particles at the origin (i.e. initial prior)
+ */
+ void restart();
+
+ /**
+ * Clear GVF
+ * @details delete templates
+ */
+ void clear();
+
+ /**
+ * Translate data according to the first point
+ * @details substract each gesture feature by the first point of the gesture
+ * @param boolean to activate or deactivate translation
+ */
+ void translate(bool translateFlag);
+
+ /**
+ * Segment gestures within a continuous gesture stream
+ * @details if segmentation is true, the method will segment a continuous gesture into a sequence
+ * of gestures. In other words no need to call the method startGesture(), it is done automatically
+ * @param segmentationFlag boolean to activate or deactivate segmentation
+ */
+ void segmentation(bool segmentationFlag);
+
+#pragma mark - [ Accessors ]
+#pragma mark > Parameters
+ /**
+ * Set tolerance between observation and estimation
+ * @details tolerance depends on the range of the data
+ * typially tolerance = (data range)/3.0;
+ * @param tolerance value
+ */
+ void setTolerance(float tolerance);
+
+ /**
+ * Get the obervation tolerance value
+ * @details see setTolerance(float tolerance)
+ * @return the current toleranc value
+ */
+ float getTolerance();
+
+ void setDistribution(float _distribution);
+
+ /**
+ * Set number of particles used in estimation
+ * @details default valye is 1000, note that the computational
+ * cost directly depends on the number of particles
+ * @param new number of particles
+ */
+ void setNumberOfParticles(int numberOfParticles);
+
+ /**
+ * Get the current number of particles
+ * @return the current number of particles
+ */
+ int getNumberOfParticles();
+
+ /**
+ * Number of prediciton steps
+ * @details it is possible to leave GVF to perform few steps of prediction
+ * ahead which can be useful to estimate more fastly the variations. Default value is 1
+ * which means no prediction ahead
+ * @param the number of prediction steps
+ */
+ void setPredictionSteps(int predictionSteps);
+
+ /**
+ * Get the current number of prediction steps
+ * @return current number of prediciton steps
+ */
+ int getPredictionSteps();
+
+ /**
+ * Set resampling threshold
+ * @details resampling threshold is the minimum number of active particles
+ * before resampling all the particles by the estimated posterior distribution.
+ * in other words, it re-targets particles around the best current estimates
+ * @param the minimum number of particles (default is (number of particles)/2)
+ */
+ void setResamplingThreshold(int resamplingThreshold);
+
+ /**
+ * Get the current resampling threshold
+ * @return resampling threshold
+ */
+ int getResamplingThreshold();
+
+#pragma mark > Dynamics
+ /**
+ * Change variance of adaptation in dynamics
+ * @details if dynamics adaptation variance is high the method will adapt faster to
+ * fast changes in dynamics. Dynamics is 2-dimensional: the first dimension is the speed
+ * The second dimension is the acceleration.
+ *
+ * Typically the variance is the average amount the speed or acceleration can change from
+ * one sample to another. As an example, if the relative estimated speed can change from 1.1 to 1.2
+ * from one sample to another, the variance should allow a change of 0.1 in speed. So the variance
+ * should be set to 0.1*0.1 = 0.01
+ *
+ * @param dynVariance dynamics variance value
+ * @param dim optional dimension of the dynamics for which the change of variance is applied (default value is 1)
+ */
+ void setDynamicsVariance(float dynVariance, int dim = -1);
+
+ /**
+ * Change variance of adaptation in dynamics
+ * @details See setDynamicsVariance(float dynVariance, int dim) for more details
+ * @param dynVariance vector of dynamics variances, each vector index is the variance to be applied to
+ * each dynamics dimension (consequently the vector should be 2-dimensional).
+ */
+ void setDynamicsVariance(vector<float> dynVariance);
+
+ /**
+ * Get dynamics variances
+ * @return the vector of variances (the returned vector is 2-dimensional)
+ */
+ vector<float> getDynamicsVariance();
+
+#pragma mark > Scalings
+ /**
+ * Change variance of adaptation in scalings
+ * @details if scalings adaptation variance is high the method will adapt faster to
+ * fast changes in relative sizes. There is one scaling variance for each dimension
+ * of the input gesture. If the gesture is 2-dimensional, the scalings variances will
+ * also be 2-dimensional.
+ *
+ * Typically the variance is the average amount the size can change from
+ * one sample to another. As an example, if the relative estimated size changes from 1.1 to 1.15
+ * from one sample to another, the variance should allow a change of 0.05 in size. So the variance
+ * should be set to 0.05*0.05 = 0.0025
+ *
+ * @param scalings variance value
+ * @param dimension of the scalings for which the change of variance is applied
+ */
+ void setScalingsVariance(float scaleVariance, int dim = -1);
+
+ /**
+ * Change variance of adaptation in dynamics
+ * @details See setScalingsVariance(float scaleVariance, int dim) for more details
+ * @param vector of scalings variances, each vector index is the variance to be applied to
+ * each scaling dimension.
+ * @param vector of variances (should be the size of the template gestures dimension)
+ */
+ void setScalingsVariance(vector<float> scaleVariance);
+
+ /**
+ * Get scalings variances
+ * @return the vector of variances
+ */
+ vector<float> getScalingsVariance();
+
+#pragma mark > Rotations
+ /**
+ * Change variance of adaptation in orientation
+ * @details if rotation adaptation variance is high the method will adapt faster to
+ * fast changes in relative orientation. If the gesture is 2-dimensional, there is
+ * one variance value since the rotation can be defined by only one angle of rotation. If
+ * the gesture is 3-dimensional, there are 3 variance values since the rotation in 3-d is
+ * defined by 3 rotation angles. For any other dimension, the rotation is not defined.
+ *
+ * The variance is the average amount the orientation can change from one sample to another.
+ * As an example, if the relative orientation in rad changes from 0.1 to 0.2 from one observation
+ * to another, the variance should allow a change of 0.1 in rotation angle. So the variance
+ * should be set to 0.1*0.1 = 0.01
+ *
+ * @param rotationsVariance rotation variance value
+ * @param dim optional dimension of the rotation for which the change of variance is applied
+ */
+ void setRotationsVariance(float rotationsVariance, int dim = -1);
+
+ /**
+ * Change variance of adaptation in orientation
+ * @details See setRotationsVariance(float rotationsVariance, int dim) for more details
+ * @param vector of rotation variances, each vector index is the variance to be applied to
+ * each rotation angle (1 or 3)
+ * @param vector of variances (should be 1 if the the template gestures are 2-dim or 3 if
+ * they are 3-dim)
+ */
+ void setRotationsVariance(vector<float> rotationsVariance);
+
+ /**
+ * Get rotation variances
+ * @return the vector of variances
+ */
+ vector<float> getRotationsVariance();
+
+
+#pragma mark > Others
+
+ /**
+ * Get particle values
+ * @return vector of list of estimated particles
+ */
+ const vector<vector<float> > & getParticlesPositions();
+
+ /**
+ * Set the interval on which the dynamics values should be spread at the beginning (before adaptation)
+ * @details this interval can be used to concentrate the potential dynamics value on a narrow interval,
+ * typically around 1 (the default value), for instance between -0.05 and 0.05, or to allow at the very
+ * beginning, high changes in dynamics by spreading, for instance between 0.0 and 2.0
+ * @param min lower value of the inital values for dynamics
+ * @param max higher value of the inital values for dynamics
+ * @param dim the dimension on which the change of initial interval should be applied (optional)
+ */
+ void setSpreadDynamics(float min, float max, int dim = -1);
+
+ /**
+ * Set the interval on which the scalings values should be spread at the beginning (before adaptation)
+ * @details this interval can be used to concentrate the potential scalings value on a narrow interval,
+ * typically around 1.0 (the default value), for instance between 0.95 and 1.05, or to allow at the very
+ * beginning high changes in dynamics by spreading, for instance, between 0.0 and 2.0
+ * @param min lower value of the inital values for scalings
+ * @param max higher value of the inital values for scalings
+ * @param dim the dimension on which the change of initial interval should be applied (optional)
+ */
+ void setSpreadScalings(float min, float max, int dim = -1);
+
+ /**
+ * Set the interval on which the angle of rotation values should be spread at the beginning (before adaptation)
+ * @details this interval can be used to concentrate the potential angle values on a narrow interval,
+ * typically around 0.0 (the default value), for instance between -0.05 and 0.05, or to allow at the very
+ * beginning, high changes in orientation by spreading, for instance, between -0.5 and 0.5
+ * @param min lower value of the inital values for angle of rotation
+ * @param max higher value of the inital values for angle of rotation
+ * @param dim the dimension on which the change of initial interval should be applied (optional)
+ */
+ void setSpreadRotations(float min, float max, int dim = -1);
+
+#pragma mark - Import/Export templates
+ /**
+ * Export template data in a filename
+ * @param filename file name as a string
+ */
+ void saveTemplates(string filename);
+
+ /**
+ * Import template data in a filename
+ * @details needs to respect a given format provided by saveTemplates()
+ * @param file name as a string
+ */
+ void loadTemplates(string filename);
+
+protected:
+
+ GVFConfig config; // Structure storing the configuration of GVF (in GVFUtils.h)
+ GVFParameters parameters; // Structure storing the parameters of GVF (in GVFUtils.h)
+ GVFOutcomes outcomes; // Structure storing the outputs of GVF (in GVFUtils.h)
+ GVFState state; // State (defined above)
+ GVFGesture theGesture; // GVFGesture object to handle incoming data in learning and following modes
+
+ vector<GVFGesture> gestureTemplates; // vector storing the gesture templates recorded when using the methods addObservation(vector<float> data) or addGestureTemplate(GVFGesture & gestureTemplate)
+
+ vector<float> dimWeights; // TOOD: to be put in parameters?
+ vector<float> maxRange;
+ vector<float> minRange;
+ int dynamicsDim; // dynamics state dimension
+ int scalingsDim; // scalings state dimension
+ int rotationsDim; // rotation state dimension
+ float globalNormalizationFactor; // flagged if normalization
+ int mostProbableIndex; // cached most probable index
+ int learningGesture;
+
+ vector<int> classes; // gesture index for each particle [ns x 1]
+ vector<float > alignment; // alignment index (between 0 and 1) [ns x 1]
+ vector<vector<float> > dynamics; // dynamics estimation [ns x 2]
+ vector<vector<float> > scalings; // scalings estimation [ns x D]
+ vector<vector<float> > rotations; // rotations estimation [ns x A]
+ vector<float> weights; // weight of each particle [ns x 1]
+ vector<float> prior; // prior of each particle [ns x 1]
+ vector<float> posterior; // poserior of each particle [ns x 1]
+ vector<float> likelihood; // likelihood of each particle [ns x 1]
+
+ // estimations
+ vector<float> estimatedGesture; // ..
+ vector<float> estimatedAlignment; // ..
+ vector<vector<float> > estimatedDynamics; // ..
+ vector<vector<float> > estimatedScalings; // ..
+ vector<vector<float> > estimatedRotations; // ..
+ vector<float> estimatedProbabilities; // ..
+ vector<float> estimatedLikelihoods; // ..
+ vector<float> absoluteLikelihoods; // ..
+
+ bool tolerancesetmanually;
+
+
+ vector<vector<float> > offsets; // translation offset
+
+ vector<int> activeGestures;
+
+ vector<float> gestureProbabilities;
+ vector< vector<float> > particles;
+
+private:
+
+ // random number generator
+ std::random_device rd;
+ std::mt19937 normgen;
+ std::normal_distribution<float> *rndnorm;
+ std::default_random_engine unifgen;
+ std::uniform_real_distribution<float> *rndunif;
+
+#pragma mark - Private methods for model mechanics
+ void initPrior();
+ void initNoiseParameters();
+ void updateLikelihood(vector<float> obs, int n);
+ void updatePrior(int n);
+ void updatePosterior(int n);
+ void resampleAccordingToWeights(vector<float> obs);
+ void estimates(); // update estimated outcome
+ void train();
+
+
+};
+
+
+#endif
\ No newline at end of file
diff --git a/dependencies/GVF/GVFGesture.h b/dependencies/GVF/GVFGesture.h
new file mode 100644
index 0000000000000000000000000000000000000000..d556fb0a9ed0f9ff84f7f09e48a157fd6357ab3b
--- /dev/null
+++ b/dependencies/GVF/GVFGesture.h
@@ -0,0 +1,240 @@
+//
+// GVFGesture.h
+// gvf
+//
+// Created by Baptiste Caramiaux on 22/01/16.
+//
+//
+
+#ifndef GVFGesture_h
+#define GVFGesture_h
+
+#ifndef MAX
+#define MAX(a,b) (((a) > (b)) ? (a) : (b))
+#endif
+
+#ifndef MIN
+#define MIN(a,b) (((a) < (b)) ? (a) : (b))
+#endif
+
+
+class GVFGesture
+{
+public:
+
+ GVFGesture()
+ {
+ inputDimensions = 2;
+ setAutoAdjustRanges(true);
+ templatesRaw = vector<vector<vector<float > > >();
+ templatesNormal = vector<vector<vector<float > > >();
+ clear();
+ }
+
+ GVFGesture(int inputDimension){
+ inputDimensions = inputDimension;
+ setAutoAdjustRanges(true);
+ templatesRaw = vector<vector<vector<float > > >();
+ templatesNormal = vector<vector<vector<float > > >();
+ clear();
+ }
+
+ ~GVFGesture(){
+ clear();
+ }
+
+ void setNumberDimensions(int dimensions){
+ assert(dimensions > 0);
+ inputDimensions = dimensions;
+ }
+
+ void setAutoAdjustRanges(bool b){
+ // if(b) bIsRangeMinSet = bIsRangeMaxSet = false;
+ bAutoAdjustNormalRange = b;
+ }
+
+ void setMax(float x, float y){
+ assert(inputDimensions == 2);
+ vector<float> r(2);
+ r[0] = x; r[1] = y;
+ setMaxRange(r);
+ }
+
+ void setMin(float x, float y){
+ assert(inputDimensions == 2);
+ vector<float> r(2);
+ r[0] = x; r[1] = y;
+ setMinRange(r);
+ }
+
+ void setMax(float x, float y, float z){
+ assert(inputDimensions == 3);
+ vector<float> r(3);
+ r[0] = x; r[1] = y; r[2] = z;
+ setMaxRange(r);
+ }
+
+ void setMin(float x, float y, float z){
+ assert(inputDimensions == 3);
+ vector<float> r(3);
+ r[0] = x; r[1] = y; r[2] = z;
+ setMinRange(r);
+ }
+
+ void setMaxRange(vector<float> observationRangeMax){
+ this->observationRangeMax = observationRangeMax;
+ // bIsRangeMaxSet = true;
+ normalise();
+ }
+
+ void setMinRange(vector<float> observationRangeMin){
+ this->observationRangeMin = observationRangeMin;
+ // bIsRangeMinSet = true;
+ normalise();
+ }
+
+ vector<float>& getMaxRange(){
+ return observationRangeMax;
+ }
+
+ vector<float>& getMinRange(){
+ return observationRangeMin;
+ }
+
+ void autoAdjustMinMax(vector<float> & observation){
+ if(observationRangeMax.size() < inputDimensions){
+ observationRangeMax.assign(inputDimensions, -INFINITY);
+ observationRangeMin.assign(inputDimensions, INFINITY);
+ }
+ for(int i = 0; i < inputDimensions; i++){
+ observationRangeMax[i] = MAX(observationRangeMax[i], observation[i]);
+ observationRangeMin[i] = MIN(observationRangeMin[i], observation[i]);
+ }
+ }
+
+ void addObservation(vector<float> observation, int templateIndex = 0){
+ if (observation.size() != inputDimensions)
+ inputDimensions = observation.size();
+
+ // check we have a valid templateIndex and correct number of input dimensions
+ assert(templateIndex <= templatesRaw.size());
+ assert(observation.size() == inputDimensions);
+
+ // if the template index is same as the number of temlates make a new template
+ if(templateIndex == templatesRaw.size()){ // make a new template
+
+ // reserve space in raw and normal template storage
+ templatesRaw.resize(templatesRaw.size() + 1);
+ templatesNormal.resize(templatesNormal.size() + 1);
+
+ }
+
+ if(templatesRaw[templateIndex].size() == 0)
+ {
+ templateInitialObservation = observation;
+ templateInitialNormal = observation;
+ }
+
+ for(int j = 0; j < observation.size(); j++)
+ observation[j] = observation[j] - templateInitialObservation[j];
+
+ // store the raw observation
+ templatesRaw[templateIndex].push_back(observation);
+
+ autoAdjustMinMax(observation);
+
+ normalise();
+ }
+
+
+
+ void normalise()
+ {
+ templatesNormal.resize(templatesRaw.size());
+ for(int t = 0; t < templatesRaw.size(); t++)
+ {
+ templatesNormal[t].resize(templatesRaw[t].size());
+ for(int o = 0; o < templatesRaw[t].size(); o++)
+ {
+ templatesNormal[t][o].resize(inputDimensions);
+ for(int d = 0; d < inputDimensions; d++)
+ {
+ templatesNormal[t][o][d] = templatesRaw[t][o][d] / (observationRangeMax[d] - observationRangeMin[d]);
+ templateInitialNormal[d] = templateInitialObservation[d] / (observationRangeMax[d] - observationRangeMin[d]);
+ }
+ }
+ }
+ }
+
+ void setTemplate(vector< vector<float> > & observations, int templateIndex = 0){
+ for(int i = 0; i < observations.size(); i++){
+ addObservation(observations[i], templateIndex);
+ }
+ }
+
+ vector< vector<float> > & getTemplate(int templateIndex = 0){
+ assert(templateIndex < templatesRaw.size());
+ return templatesRaw[templateIndex];
+ }
+
+ int getNumberOfTemplates(){
+ return templatesRaw.size();
+ }
+
+ int getNumberDimensions(){
+ return inputDimensions;
+ }
+
+ int getTemplateLength(int templateIndex = 0){
+ return templatesRaw[templateIndex].size();
+ }
+
+ int getTemplateDimension(int templateIndex = 0){
+ return templatesRaw[templateIndex][0].size();
+ }
+
+ vector<float>& getLastObservation(int templateIndex = 0){
+ return templatesRaw[templateIndex][templatesRaw[templateIndex].size() - 1];
+ }
+
+ vector< vector< vector<float> > >& getTemplates(){
+ return templatesRaw;
+ }
+
+ vector<float>& getInitialObservation(){
+ return templateInitialObservation;
+ }
+
+ void deleteTemplate(int templateIndex = 0)
+ {
+ assert(templateIndex < templatesRaw.size());
+ templatesRaw[templateIndex].clear();
+ templatesNormal[templateIndex].clear();
+ }
+
+ void clear()
+ {
+ templatesRaw.clear();
+ templatesNormal.clear();
+ observationRangeMax.assign(inputDimensions, -INFINITY);
+ observationRangeMin.assign(inputDimensions, INFINITY);
+ }
+
+private:
+
+ int inputDimensions;
+ bool bAutoAdjustNormalRange;
+
+ vector<float> observationRangeMax;
+ vector<float> observationRangeMin;
+
+ vector<float> templateInitialObservation;
+ vector<float> templateInitialNormal;
+
+ vector< vector< vector<float> > > templatesRaw;
+ vector< vector< vector<float> > > templatesNormal;
+
+ vector<vector<float> > gestureDataFromFile;
+};
+
+#endif /* GVFGesture_h */
diff --git a/dependencies/GVF/GVFUtils.h b/dependencies/GVF/GVFUtils.h
new file mode 100755
index 0000000000000000000000000000000000000000..125676c29c01593c17833a180171f22199f17ee1
--- /dev/null
+++ b/dependencies/GVF/GVFUtils.h
@@ -0,0 +1,309 @@
+//
+// GVFTypesAndUtils.h
+//
+//
+//
+
+#ifndef __H_GVFTYPES
+#define __H_GVFTYPES
+
+#include <map>
+#include <vector>
+#include <iostream>
+#include <random>
+#include <iostream>
+#include <math.h>
+#include <assert.h>
+
+using namespace std;
+
+/**
+ * Configuration structure
+ */
+typedef struct
+{
+ int inputDimensions; /**< input dimesnion */
+ bool translate; /**< translate flag */
+ bool segmentation; /**< segmentation flag */
+} GVFConfig;
+
+/**
+ * Parameters structure
+ */
+typedef struct
+{
+ float tolerance; /**< input dimesnion */
+ float distribution;
+ int numberParticles;
+ int resamplingThreshold;
+ float alignmentVariance;
+ float speedVariance;
+ vector<float> scaleVariance;
+ vector<float> dynamicsVariance;
+ vector<float> scalingsVariance;
+ vector<float> rotationsVariance;
+ // spreadings
+ float alignmentSpreadingCenter;
+ float alignmentSpreadingRange;
+ float dynamicsSpreadingCenter;
+ float dynamicsSpreadingRange;
+ float scalingsSpreadingCenter;
+ float scalingsSpreadingRange;
+ float rotationsSpreadingCenter;
+ float rotationsSpreadingRange;
+
+ int predictionSteps;
+ vector<float> dimWeights;
+} GVFParameters;
+
+// Outcomes structure
+typedef struct
+{
+ int likeliestGesture;
+ vector<float> likelihoods;
+ vector<float> alignments;
+ vector<vector<float> > dynamics;
+ vector<vector<float> > scalings;
+ vector<vector<float> > rotations;
+} GVFOutcomes;
+
+
+//--------------------------------------------------------------
+// init matrix by allocating memory
+template <typename T>
+inline void initMat(vector< vector<T> > & M, int rows, int cols){
+ M.resize(rows);
+ for (int n=0; n<rows; n++){
+ M[n].resize(cols);
+ }
+}
+
+//--------------------------------------------------------------
+// init matrix and copy values from another matrix
+template <typename T>
+inline void setMat(vector< vector<T> > & C, vector< vector<float> > & M){
+ int rows = M.size();
+ int cols = M[0].size();
+ //C.resize(rows);
+ C = vector<vector<T> >(rows);
+ for (int n=0; n<rows; n++){
+ //C[n].resize(cols);
+ C[n] = vector<T>(cols);
+ for (int m=0;m<cols;m++){
+ C[n][m] = M[n][m];
+ }
+ }
+}
+
+//--------------------------------------------------------------
+// init matrix by allocating memory and fill with T value
+template <typename T>
+inline void setMat(vector< vector<T> > & M, T value, int rows, int cols){
+ M.resize(rows);
+ for (int n=0; n<rows; n++){
+ M[n].resize(cols);
+ for (int m=0; m<cols; m++){
+ M[n][m] = value;
+ }
+ }
+}
+
+//--------------------------------------------------------------
+// set matrix filled with T value
+template <typename T>
+inline void setMat(vector< vector<T> > & M, T value){
+ for (int n=0; n<M.size(); n++){
+ for (int m=0; m<M[n].size(); m++){
+ M[n][m] = value;
+ }
+ }
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline void printMat(vector< vector<T> > & M){
+ for (int k=0; k<M.size(); k++){
+ cout << k << ": ";
+ for (int l=0; l<M[0].size(); l++){
+ cout << M[k][l] << " ";
+ }
+ cout << endl;
+ }
+ cout << endl;
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline void printVec(vector<T> & V){
+ for (int k=0; k<V.size(); k++){
+ cout << k << ": " << V[k] << (k == V.size() - 1 ? "" : " ,");
+ }
+ cout << endl;
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline void initVec(vector<T> & V, int rows){
+ V.resize(rows);
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline void setVec(vector<T> & C, vector<int> &V){
+ int rows = V.size();
+ C = vector<T>(rows);
+ //C.resize(rows);
+ for (int n=0; n<rows; n++){
+ C[n] = V[n];
+ }
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline void setVec(vector<T> & C, vector<float> & V){
+ int rows = V.size();
+ C.resize(rows);
+ for (int n=0; n<rows; n++){
+ C[n] = V[n];
+ }
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline void setVec(vector<T> & V, T value){
+ for (int n=0; n<V.size(); n++){
+ V[n] = value;
+ }
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline void setVec(vector<T> & V, T value, int rows){
+ V.resize(rows);
+ setVec(V, value);
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline vector< vector<T> > dotMat(vector< vector<T> > & M1, vector< vector<T> > & M2){
+ // TODO(Baptiste)
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline vector< vector<T> > multiplyMatf(vector< vector<T> > & M1, T v){
+ vector< vector<T> > multiply;
+ initMat(multiply, M1.size(), M1[0].size());
+ for (int i=0; i<M1.size(); i++){
+ for (int j=0; j<M1[i].size(); j++){
+ multiply[i][j] = M1[i][j] * v;
+ }
+ }
+ return multiply;
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline vector< vector<T> > multiplyMatf(vector< vector<T> > & M1, vector< vector<T> > & M2){
+ assert(M1[0].size() == M2.size()); // columns in M1 == rows in M2
+ vector< vector<T> > multiply;
+ initMat(multiply, M1.size(), M2[0].size()); // rows in M1 x cols in M2
+ for (int i=0; i<M1.size(); i++){
+ for (int j=0; j<M2[i].size(); j++){
+ multiply[i][j] = 0.0f;
+ for(int k=0; k<M1[0].size(); k++){
+ multiply[i][j] += M1[i][k] * M2[k][j];
+ }
+
+ }
+ }
+ return multiply;
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline vector<T> multiplyMat(vector< vector<T> > & M1, vector< T> & Vect){
+ assert(Vect.size() == M1[0].size()); // columns in M1 == rows in Vect
+ vector<T> multiply;
+ initVec(multiply, Vect.size());
+ for (int i=0; i<M1.size(); i++){
+ multiply[i] = 0.0f;
+ for (int j=0; j<M1[i].size(); j++){
+ multiply[i] += M1[i][j] * Vect[j];
+ }
+ }
+ return multiply;
+}
+
+//--------------------------------------------------------------
+template <typename T>
+inline float getMeanVec(vector<T>& V){
+ float tSum = 0.0f;
+ for (int n=0; n<V.size(); n++){
+ tSum += V[n];
+ }
+ return tSum / (float)V.size();
+}
+
+template <typename T>
+inline vector<vector<float> > getRotationMatrix3d(T phi, T theta, T psi)
+{
+ vector< vector<float> > M;
+ initMat(M,3,3);
+
+ M[0][0] = cos(theta)*cos(psi);
+ M[0][1] = -cos(phi)*sin(psi)+sin(phi)*sin(theta)*cos(psi);
+ M[0][2] = sin(phi)*sin(psi)+cos(phi)*sin(theta)*cos(psi);
+
+ M[1][0] = cos(theta)*sin(psi);
+ M[1][1] = cos(phi)*cos(psi)+sin(phi)*sin(theta)*sin(psi);
+ M[1][2] = -sin(phi)*cos(psi)+cos(phi)*sin(theta)*sin(psi);
+
+ M[2][0] = -sin(theta);
+ M[2][1] = sin(phi)*cos(theta);
+ M[2][2] = cos(phi)*cos(theta);
+
+ return M;
+}
+
+template <typename T>
+float distance_weightedEuclidean(vector<T> x, vector<T> y, vector<T> w)
+{
+ int count = x.size();
+ if (count <= 0) return 0;
+ float dist = 0.0;
+ for(int k = 0; k < count; k++) dist += w[k] * pow((x[k] - y[k]), 2);
+ return dist;
+}
+
+////--------------------------------------------------------------
+//vector<vector<float> > getRotationMatrix3d(float phi, float theta, float psi)
+//{
+// vector< vector<float> > M;
+// initMat(M,3,3);
+//
+// M[0][0] = cos(theta)*cos(psi);
+// M[0][1] = -cos(phi)*sin(psi)+sin(phi)*sin(theta)*cos(psi);
+// M[0][2] = sin(phi)*sin(psi)+cos(phi)*sin(theta)*cos(psi);
+//
+// M[1][0] = cos(theta)*sin(psi);
+// M[1][1] = cos(phi)*cos(psi)+sin(phi)*sin(theta)*sin(psi);
+// M[1][2] = -sin(phi)*cos(psi)+cos(phi)*sin(theta)*sin(psi);
+//
+// M[2][0] = -sin(theta);
+// M[2][1] = sin(phi)*cos(theta);
+// M[2][2] = cos(phi)*cos(theta);
+//
+// return M;
+//}
+
+//float distance_weightedEuclidean(vector<float> x, vector<float> y, vector<float> w)
+//{
+// int count = x.size();
+// if (count <= 0) return 0;
+// float dist = 0.0;
+// for(int k = 0; k < count; k++) dist += w[k] * pow((x[k] - y[k]), 2);
+// return dist;
+//}
+
+#endif