diff --git a/modules/touch/ext/levmarq.cpp b/modules/touch/ext/levmarq.cpp
index 50fb947047..8482ef4b2b 100644
--- a/modules/touch/ext/levmarq.cpp
+++ b/modules/touch/ext/levmarq.cpp
@@ -30,8 +30,8 @@ OTHER DEALINGS IN THE SOFTWARE.
 
 // set parameters required by levmarq() to default values 
 void levmarq_init(LMstat *lmstat) {
-	lmstat->verbose = 0;
-	lmstat->max_it = 10000;
+	lmstat->verbose = 1;
+	lmstat->max_it = 20;
 	lmstat->init_lambda = 0.0001;
 	lmstat->up_factor = 10;
 	lmstat->down_factor = 10;
@@ -57,7 +57,7 @@ The arguments are as follows:
 Before calling levmarq, several of the parameters in lmstat must be set.
 For default values, call levmarq_init(lmstat).
 */
-int levmarq(int npar, double *par, int ny, double *y, double *dysq,
+int levmarq(int npar, double *par, int ny, double *dysq,
 	    double (*func)(double *, int, void *),
 	    void (*grad)(double *, double *, int, void *),
 	    void *fdata, LMstat *lmstat) {
@@ -87,7 +87,7 @@ int levmarq(int npar, double *par, int ny, double *y, double *dysq,
 	derr = newerr = 0; // to avoid compiler warnings
 
 	// calculate the initial error ("chi-squared")
-	err = error_func(par, ny, y, dysq, func, fdata);
+	err = error_func(par, ny, dysq, func, fdata);
 
 	// main iteration
 	for (it = 0; it < nit; it++) {
@@ -102,7 +102,7 @@ int levmarq(int npar, double *par, int ny, double *y, double *dysq,
 				weight = 1/dysq[x]; // for weighted least-squares
 			grad(g, par, x, fdata);
 			for (i = 0; i < npar; i++) {
-				d[i] += (y[x] - func(par, x, fdata)) * g[i] * weight;
+				d[i] += func(par, x, fdata) * g[i] * weight; //(y[x] - func(par, x, fdata)) * g[i] * weight;
 				for (j = 0; j <= i; j++)
 					h[i][j] += g[i] * g[j] * weight;
 			}
@@ -110,20 +110,29 @@ int levmarq(int npar, double *par, int ny, double *y, double *dysq,
 		// make a step "delta."  If the step is rejected, increase lambda and try again
 		mult = 1 + lambda;
 		ill = 1; // ill-conditioned?
-		while (ill && (it<nit)) {
-			for (i=0; i<npar; i++)
+		while (ill && (it < nit)) {
+			for (i = 0; i < npar; i++)
 				h[i][i] = h[i][i]*mult;
 			ill = cholesky_decomp(npar, ch, h);
 			if (!ill) {
 				solve_axb_cholesky(npar, ch, delta, d);
 				for (i = 0; i < npar; i++)
 					newpar[i] = par[i] + delta[i];
-				newerr = error_func(newpar, ny, y, dysq, func, fdata);
+				newerr = error_func(newpar, ny, dysq, func, fdata);
 				derr = newerr - err;
 				ill = (derr > 0);
 			} 
-			if (verbose) 
+			if (verbose) {
 				printf("it = %4d,   lambda = %10g,   err = %10g,   derr = %10g\n", it, lambda, err, derr);
+				for (i = 0; i < npar; i++) {
+					printf("%f:", par[i]);
+				}
+				printf("\n");
+				for (i = 0; i < npar; ++i) {
+					printf("%f:", delta[i]);
+				}
+				printf("\n");
+			}
 			if (ill) {
 				mult = (1 + lambda * up) / (1 + lambda);
 				lambda *= up;
@@ -133,9 +142,9 @@ int levmarq(int npar, double *par, int ny, double *y, double *dysq,
 		for (i = 0; i < npar; i++)
 			par[i] = newpar[i];
 		err = newerr;
-		lambda *= down;  
+		lambda *= down;
 
-		if ((!ill) && (-derr<target_derr)) 
+		if ((!ill) && (-derr < target_derr)) 
 			break;
 	}
 
@@ -160,13 +169,13 @@ int levmarq(int npar, double *par, int ny, double *y, double *dysq,
 
 
 // calculate the error function (chi-squared)
-double error_func(double *par, int ny, double *y, double *dysq,
+double error_func(double *par, int ny, double *dysq,
 		double (*func)(double *, int, void *), void *fdata) {
 	int x;
 	double res, e = 0;
 
 	for (x = 0; x < ny; x++) {
-		res = func(par, x, fdata) - y[x];
+		res = func(par, x, fdata);
 		if (dysq) // weighted least-squares 
 			e += res*res/dysq[x];
 		else
@@ -177,7 +186,7 @@ double error_func(double *par, int ny, double *y, double *dysq,
 
 
 // solve Ax=b for a symmetric positive-definite matrix A using the Cholesky decomposition A=LL^T, L is passed in "l", elements above the diagonal are ignored.
-void solve_axb_cholesky(int n, double** l, double* x, double* b) {
+void solve_axb_cholesky(int n, double** l, double* x, double* b) { // n = npar, l = ch, x = delta (solution), b = d (func(par, x, fdata) * g[i]);
 	int i,j;
 	double sum;
 	// solve L*y = b for y (where x[] is used to store y)
@@ -185,7 +194,7 @@ void solve_axb_cholesky(int n, double** l, double* x, double* b) {
 		sum = 0;
 		for (j = 0; j < i; j++)
 			sum += l[i][j] * x[j];
-		x[i] = (b[i] - sum) / l[i][i];      
+		x[i] = (b[i] - sum) / l[i][i]; 
 	}
 	// solve L^T*x = y for x (where x[] is used to store both y and x)
 	for (i = n-1; i >= 0; i--) {
diff --git a/modules/touch/ext/levmarq.h b/modules/touch/ext/levmarq.h
index 3b5117edd7..610cefa3ff 100644
--- a/modules/touch/ext/levmarq.h
+++ b/modules/touch/ext/levmarq.h
@@ -38,12 +38,12 @@ typedef struct {
 
 void levmarq_init(LMstat *lmstat);
 
-int levmarq(int npar, double *par, int ny, double *y, double *dysq,
+int levmarq(int npar, double *par, int ny, double *dysq,
 	double (*func)(double *, int, void *),
 	void (*grad)(double *, double *, int, void *),
 	void *fdata, LMstat *lmstat);
 
-double error_func(double *par, int ny, double *y, double *dysq,
+double error_func(double *par, int ny, double *dysq,
 	double (*func)(double *, int, void *), void *fdata);
 
 void solve_axb_cholesky(int n, double** l, double* x, double* b);
diff --git a/modules/touch/include/TouchInteraction.h b/modules/touch/include/TouchInteraction.h
index d6a49c689b..b1f9a0b39c 100644
--- a/modules/touch/include/TouchInteraction.h
+++ b/modules/touch/include/TouchInteraction.h
@@ -83,6 +83,7 @@ struct SelectedBody {
 
 struct FunctionData {
 	std::vector<glm::dvec3> selectedPoints;
+	std::vector<glm::dvec2> screenPoints;
 	int nDOF;
 	glm::dvec2(*toScreen)(glm::dvec3, Camera*, SceneGraphNode*, double);
 	Camera* camera;
diff --git a/modules/touch/src/TouchInteraction.cpp b/modules/touch/src/TouchInteraction.cpp
index 7fee9e3e69..11afe8435c 100644
--- a/modules/touch/src/TouchInteraction.cpp
+++ b/modules/touch/src/TouchInteraction.cpp
@@ -83,9 +83,10 @@ TouchInteraction::~TouchInteraction() { }
 void TouchInteraction::update(const std::vector<TuioCursor>& list, std::vector<Point>& lastProcessed) {
 	if (_directTouchMode && _selected.size() > 0 && list.size() == _selected.size()) { // should just be a function call
 		// Returns the screen point s(xi,par) dependant the transform M(par) and object point xi
-		auto func = [](double* par, int x, void* fdata) {
+		auto distToMinimize = [](double* par, int x, void* fdata) {
 			FunctionData* ptr = reinterpret_cast<FunctionData*>(fdata);
-			glm::dvec3 surfacePoint = ptr->selectedPoints.at(x / 2); 
+			glm::dvec3 surfacePoint = ptr->selectedPoints.at(x); 
+
 			// Create transformation matrix M(q) and apply transform for newPointInModelView
 			glm::dvec3 T = glm::dvec3(par[0], par[1], 0.0);
 			glm::dquat Q;
@@ -100,80 +101,64 @@ void TouchInteraction::update(const std::vector<TuioCursor>& list, std::vector<P
 			}
 			double len = Q.x*Q.x + Q.y*Q.y + Q.z*Q.z;
 			Q.w = sqrt(1.0 - len);
-			glm::dvec3 newSurfacePoint = Q * (surfacePoint + T);
+			glm::dvec3 newSurfacePoint = (Q * surfacePoint) + T;
 			glm::dvec2 newScreenPoint = ptr->toScreen(newSurfacePoint, ptr->camera, ptr->node, ptr->aspectRatio); // go back to screen-space
-			
-			if (x % 2) // return right variable
-				return newScreenPoint.y;
-			else
-				return newScreenPoint.x;
-		};
-		// Gradient of func w.r.t par
-		auto grad = [](double* g, double* par, int x, void* fdata) { // should g[i] = 1.0 or the derivative -> project to screen -> .x or .y?
-			FunctionData* ptr = reinterpret_cast<FunctionData*>(fdata);
-			glm::dvec3 surfacePoint = ptr->selectedPoints.at(x / 2);
 
-			g[0] = 1.0;
-			g[1] = 1.0;
-			std::vector<glm::dvec2> transform;
-			transform.push_back(ptr->toScreen(glm::dvec3(1.0, 0.0, 0.0), ptr->camera, ptr->node, ptr->aspectRatio)); // Tx
-			transform.push_back(ptr->toScreen(glm::dvec3(0.0, 1.0, 0.0), ptr->camera, ptr->node, ptr->aspectRatio)); // Ty
+			return glm::length(ptr->screenPoints.at(x) - newScreenPoint);
+		};
+		// Gradient of distToMinimize w.r.t par
+		auto gradient = [](double* g, double* par, int x, void* fdata) { // should g[i] = 1.0 or the derivative -> project to screen -> .x or .y?
+			FunctionData* ptr = reinterpret_cast<FunctionData*>(fdata);
+			glm::dvec3 surfacePoint = ptr->selectedPoints.at(x);
+			g[0] = glm::length(-ptr->toScreen(glm::dvec3(1.0, 0.0, 0.0), ptr->camera, ptr->node, ptr->aspectRatio)); // Tx
+			//g[0] = 1.0;
+			g[1] = glm::length(-ptr->toScreen(glm::dvec3(0.0, 1.0 , 0.0), ptr->camera, ptr->node, ptr->aspectRatio)); // Ty
+			//g[1] = 1.0;
 			if (ptr->nDOF > 2) {
-				transform.push_back(ptr->toScreen(glm::dvec3(0.0, 0.0, 1.0), ptr->camera, ptr->node, ptr->aspectRatio)); // Tz
+				g[2] = glm::length(-ptr->toScreen(glm::dvec3(0.0, 0.0, 1.0), ptr->camera, ptr->node, ptr->aspectRatio)); // Tz
+				//g[2] = 1.0;
 				glm::dquat Q;
-				Q.x = par[3];
+				Q.x = 1.0;
 				Q.y = Q.z = 0.0;
 				Q.w = sqrt(1.0 - Q.x*Q.x);
-				transform.push_back(ptr->toScreen(Q * surfacePoint, ptr->camera, ptr->node, ptr->aspectRatio)); // Rx
-
-				g[2] = 1.0;
-				g[3] = 1.0;
+				g[3] = glm::length(-ptr->toScreen(Q * surfacePoint, ptr->camera, ptr->node, ptr->aspectRatio)); // Rx
+				//g[3] = 1.0;
 				if (ptr->nDOF > 4) {
-					Q.y = par[4];
+					Q.y = 1.0;
 					Q.x = Q.z = 0.0;
 					Q.w = sqrt(1.0 - Q.y*Q.y);
-					transform.push_back(ptr->toScreen(Q * surfacePoint, ptr->camera, ptr->node, ptr->aspectRatio)); // Ry
+					g[4] = glm::length(-ptr->toScreen(Q * surfacePoint, ptr->camera, ptr->node, ptr->aspectRatio)); // Ry
+					//g[4] = 1.0;
 
-					Q.z = par[5];
+					Q.z = 1.0;
 					Q.x = Q.y = 0.0;
 					Q.w = sqrt(1.0 - Q.z*Q.z);
-					transform.push_back(ptr->toScreen(Q * surfacePoint, ptr->camera, ptr->node, ptr->aspectRatio)); // Rz
-
-					g[4] = 1.0;
-					g[5] = 1.0;
+					g[5] = glm::length(-ptr->toScreen(Q * surfacePoint, ptr->camera, ptr->node, ptr->aspectRatio)); // Rz
+					//g[5] = 1.0;
 				}
 			}
 
-			/*for (int i = 0; i < ptr->nDOF; ++i) {
-				if (x % 2)
-					g[i] = transform.at(i).y;
-				else
-					g[i] = transform.at(i).x;
-			}*/
 		};
 
 		SceneGraphNode* node = _selected.at(0).node;
-		const int nCoord = list.size() * 2;
-		int nDOF = std::min(nCoord, 6);
+		const int nFingers = list.size();
+		int nDOF = std::min(static_cast<int>(list.size() * 2), 6);
 		double* par = new double[nDOF];
 		double tPar[6] = { node->worldPosition().x, node->worldPosition().y, node->worldPosition().z, 0.0, 0.0, 0.0 };
 		for (int i = 0; i < nDOF; ++i) // initial values of q or 0.0? (ie current model or no rotation/translation)
 			par[i] = 0.0;
 		std::vector<glm::dvec3> selectedPoints;
+		std::vector<glm::dvec2> screenPoints;
 		for (const SelectedBody& sb : _selected) {
 			selectedPoints.push_back(sb.coordinates);
+
+			std::vector<TuioCursor>::const_iterator c = find_if(list.begin(), list.end(), [&sb](const TuioCursor& c) { return c.getSessionID() == sb.id; });
+			screenPoints.push_back(glm::dvec2(c->getX(), c->getY()));
 		}
-		double* screenPoints = new double[nCoord];
-		double* squaredError = new double[nCoord]; // probably not be needed
-		int i = 0;
-		for (const TuioCursor& c : list) {
-			screenPoints[i] = c.getX();
-			screenPoints[i + 1] = c.getY();
-
-			squaredError[i] = pow(c.getX() - modelToScreenSpace(selectedPoints.at(i/2), node).x, 2) ;
-			squaredError[i + 1] = pow(c.getY() - modelToScreenSpace(selectedPoints.at(i / 2), node).x, 2);
-
-			i += 2;
+		double* squaredError = new double[nFingers]; // probably not needed
+		for (int i = 0; i < nFingers; ++i) {
+			double err = glm::length(screenPoints.at(i) - modelToScreenSpace(selectedPoints.at(i), node));
+			squaredError[i] = err*err;
 		}
 		
 		auto toScreen = [](glm::dvec3 vec, Camera* camera, SceneGraphNode* node, double aspectRatio) {
@@ -184,11 +169,11 @@ void TouchInteraction::update(const std::vector<TuioCursor>& list, std::vector<P
 		};
 
 		glm::dvec2 res = OsEng.windowWrapper().currentWindowResolution();
-		FunctionData fData = { selectedPoints, nDOF, toScreen, _camera, node, res.x / res.y};
+		FunctionData fData = { selectedPoints, screenPoints, nDOF, toScreen, _camera, node, res.x / res.y};
 		void* dataPtr = reinterpret_cast<void*>(&fData);
 
 		levmarq_init(&_lmstat);
-		int nIterations = levmarq(nDOF, par, nCoord, screenPoints, NULL, func, grad, dataPtr, &_lmstat); // finds best transform values and stores them in par
+		int nIterations = levmarq(nDOF, par, list.size(), NULL, distToMinimize, gradient, dataPtr, &_lmstat); // finds best transform values and stores them in par
 
 		double temp[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
 		for (int i = 0; i < nDOF; ++i)
@@ -205,7 +190,7 @@ void TouchInteraction::update(const std::vector<TuioCursor>& list, std::vector<P
 		for (int i = 0; i < nDOF; ++i) {
 			os << par[i] << ", ";
 		}
-		std::cout << "Levmarq success after " << nIterations << " iterations, Print par[nDOF]: " << os.str() << "\n";
+		std::cout << "\nLevmarq success after " << nIterations << " iterations, Print par[nDOF]: " << os.str() << "\n";
 
 
 
@@ -216,11 +201,10 @@ void TouchInteraction::update(const std::vector<TuioCursor>& list, std::vector<P
 			* change lmstat init
 		
 		*/
-		_camera->setPositionVec3(_camera->positionVec3() - T);
-		_camera->rotate(glm::inverse(Q));
+		//_camera->setPositionVec3(_camera->positionVec3() - T);
+		//_camera->rotate(glm::inverse(Q));
 
 		// cleanup
-		delete[] screenPoints;
 		delete[] squaredError;
 		delete[] par;
 	}