first step for LM algorithm on screen-space points, camera/focusnode causes crashes if not initialized, where do we do this best?

modules/touch/CMakeLists.txt

@@ -25,12 +25,14 @@
 include(${OPENSPACE_CMAKE_EXT_DIR}/module_definition.cmake)
 
 set(HEADER_FILES
+	${CMAKE_CURRENT_SOURCE_DIR}/ext/levmarq.h
     ${CMAKE_CURRENT_SOURCE_DIR}/include/TuioEar.h
     ${CMAKE_CURRENT_SOURCE_DIR}/include/TouchInteraction.h
 )
 source_group("Header Files" FILES ${HEADER_FILES})
 
 set(SOURCE_FILES
+	${CMAKE_CURRENT_SOURCE_DIR}/ext/levmarq.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/src/TuioEar.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/src/TouchInteraction.cpp
 )

modules/touch/ext/levmarq.cpp

@@ -0,0 +1,221 @@
+/*
+Permission is hereby granted, free of charge, to any person
+obtaining a copy of this software and associated documentation
+files(the "Software"), to deal in the Software without
+restriction, including without limitation the rights to use,
+copy, modify, merge, publish, distribute, sublicense, and / or sell
+copies of the Software, and to permit persons to whom the
+Software is furnished to do so, subject to the following
+conditions :
+
+The above copyright notice and this permission notice shall be
+included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+NONINFRINGEMENT.IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <stdio.h>
+#include <math.h>
+#include <modules/touch/ext/levmarq.h>
+
+#define TOL 1e-30 // smallest value allowed in cholesky_decomp()
+
+
+// set parameters required by levmarq() to default values 
+void levmarq_init(LMstat *lmstat) {
+	lmstat->verbose = 0;
+	lmstat->max_it = 10000;
+	lmstat->init_lambda = 0.0001;
+	lmstat->up_factor = 10;
+	lmstat->down_factor = 10;
+	lmstat->target_derr = 1e-12;
+}
+
+
+/* 
+perform least-squares minimization using the Levenberg-Marquardt algorithm.  
+The arguments are as follows:
+   npar    number of parameters
+   par     array of parameters to be varied
+   ny      number of measurements to be fit
+   y       array of measurements
+   dysq    array of error in measurements, squared
+           (set dysq=NULL for unweighted least-squares)
+   func    function to be fit
+   grad    gradient of "func" with respect to the input parameters
+   fdata   pointer to any additional data required by the function
+   lmstat  pointer to the "status" structure, where minimization parameters
+           are set and the final status is returned.
+
+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,
+	    double (*func)(double *, int, void *),
+	    void (*grad)(double *, double *, int, void *),
+	    void *fdata, LMstat *lmstat) {
+
+	int x, i, j, it, nit, ill, verbose;
+	double lambda, up, down, mult, weight, err, newerr, derr, target_derr;
+
+	// allocate the arrays
+	double** h = new double*[npar];
+	double** ch = new double*[npar];
+	for (int i = 0; i < npar; i++) {
+		h[i] = new double[npar];
+		ch[i] = new double[npar];
+	}
+	double* g = new double[npar];
+	double* d = new double[npar];
+	double* delta = new double[npar];
+	double* newpar = new double[npar];
+
+	verbose = lmstat->verbose;
+	nit = lmstat->max_it;
+	lambda = lmstat->init_lambda;
+	up = lmstat->up_factor;
+	down = 1/lmstat->down_factor;
+	target_derr = lmstat->target_derr;
+	weight = 1;
+	derr = newerr = 0; // to avoid compiler warnings
+
+	// calculate the initial error ("chi-squared")
+	err = error_func(par, ny, y, dysq, func, fdata);
+
+	// main iteration
+	for (it = 0; it < nit; it++) {
+		// calculate the approximation to the Hessian and the "derivative" d
+		for (i = 0; i < npar; i++) {
+			d[i] = 0;
+			for (j = 0; j <= i; j++)
+				h[i][j] = 0;
+		}
+		for (x = 0; x < ny; x++) {
+			if (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;
+				for (j = 0; j <= i; j++)
+					h[i][j] += g[i] * g[j] * weight;
+			}
+		}
+		// 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++)
+				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);
+				derr = newerr - err;
+				ill = (derr > 0);
+			} 
+			if (verbose) 
+				printf("it = %4d,   lambda = %10g,   err = %10g,   derr = %10g\n", it, lambda, err, derr);
+			if (ill) {
+				mult = (1 + lambda * up) / (1 + lambda);
+				lambda *= up;
+				it++;
+			}
+		}
+		for (i = 0; i < npar; i++)
+			par[i] = newpar[i];
+		err = newerr;
+		lambda *= down;  
+
+		if ((!ill) && (-derr<target_derr)) 
+			break;
+	}
+
+	lmstat->final_it = it;
+	lmstat->final_err = err;
+	lmstat->final_derr = derr;
+
+	// deallocate the arrays
+	for (int i = 0; i < npar; i++) {
+		delete[] h[i];
+		delete[] ch[i];
+	}
+	delete[] h;
+	delete[] ch;
+	delete[] g;
+	delete[] d;
+	delete[] delta;
+	delete[] newpar;
+
+	return (it==nit);
+}
+
+
+// calculate the error function (chi-squared)
+double error_func(double *par, int ny, double *y, 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];
+		if (dysq) // weighted least-squares 
+			e += res*res/dysq[x];
+		else
+			e += res*res;
+	}
+	return e;
+}
+
+
+// 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) {
+	int i,j;
+	double sum;
+	// solve L*y = b for y (where x[] is used to store y)
+	for (i = 0; i < n; i++) {
+		sum = 0;
+		for (j = 0; j < i; j++)
+			sum += l[i][j] * x[j];
+		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--) {
+		sum = 0;
+		for (j = i+1; j < n; j++)
+			sum += l[j][i] * x[j];
+		x[i] = (x[i] - sum) / l[i][i];
+	}
+}
+
+
+
+// symmetric, positive-definite matrix "a" and returns its (lower-triangular) Cholesky factor in "l", if l=a the decomposition is performed in place, elements above the diagonal are ignored.
+int cholesky_decomp(int n, double** l, double** a) {
+	int i,j,k;
+	double sum;
+	for (i = 0; i < n; i++) {
+		for (j = 0; j < i; j++) {
+			sum = 0;
+			for (k = 0; k < j; k++)
+				sum += l[i][k] * l[j][k];
+			l[i][j] = (a[i][j] - sum) / l[j][j];
+		}
+		sum = 0;
+		for (k = 0; k < i; k++)
+			sum += l[i][k] * l[i][k];
+		sum = a[i][i] - sum;
+		if (sum < TOL) 
+			return 1; // not positive-definite
+		l[i][i] = sqrt(sum);
+	}
+	return 0;
+}

modules/touch/ext/levmarq.h

@@ -0,0 +1,51 @@
+/*
+levmarq.c and levmarq.h are provided under the MIT license.
+Copyright(c) 2008 - 2016 Ron Babich
+
+Permission is hereby granted, free of charge, to any person
+obtaining a copy of this software and associated documentation
+files(the "Software"), to deal in the Software without
+restriction, including without limitation the rights to use,
+copy, modify, merge, publish, distribute, sublicense, and / or sell
+copies of the Software, and to permit persons to whom the
+Software is furnished to do so, subject to the following
+conditions :
+
+The above copyright notice and this permission notice shall be
+included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+NONINFRINGEMENT.IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+typedef struct {
+	int verbose;
+	int max_it;
+	double init_lambda;
+	double up_factor;
+	double down_factor;
+	double target_derr;
+	int final_it;
+	double final_err;
+	double final_derr;
+} LMstat;
+
+void levmarq_init(LMstat *lmstat);
+
+int levmarq(int npar, double *par, int ny, double *y, 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 (*func)(double *, int, void *), void *fdata);
+
+void solve_axb_cholesky(int n, double** l, double* x, double* b);
+
+int cholesky_decomp(int n, double** l, double** a);

modules/touch/include/TouchInteraction.h

@@ -27,6 +27,7 @@
 
 #include <modules/touch/include/TuioEar.h>
 #include <modules/touch/touchmodule.h>
+#include <modules/touch/ext/levmarq.h>
 
 #include <openspace/util/camera.h>
 #include <openspace/scene/scenegraphnode.h>
@@ -130,6 +131,7 @@ class TouchInteraction : public properties::PropertyOwner
 		std::vector<SelectedBody> _selected;
 		InteractionType _action;
 		
+		LMstat _lmstat;
 		glm::dvec3 _centroid;
 		VelocityStates _vel;
 		ScaleFactor _friction;

modules/touch/src/TouchInteraction.cpp

@@ -59,13 +59,15 @@ TouchInteraction::TouchInteraction()
 	_baseSensitivity{ 0.1 }, _baseFriction{ 0.02 },
 	_vel{ 0.0, glm::dvec2(0.0), glm::dvec2(0.0), 0.0, 0.0 },
 	_friction{ _baseFriction, _baseFriction/2.0, _baseFriction, _baseFriction, _baseFriction },
-	_touchScreenSize("normalizer", "Touch Screen Normalizer", glm::vec2(122, 68), glm::vec2(0), glm::vec2(1000)), // glm::vec2(width, height) in cm. (13.81, 6.7) for iphone 6s plus
+	_touchScreenSize("TouchScreenSize", "Touch Screen Normalizer", glm::vec2(122, 68), glm::vec2(0), glm::vec2(1000)), // glm::vec2(width, height) in cm. (13.81, 6.7) for iphone 6s plus
 	_centroid{ glm::dvec3(0.0) },
 	_sensitivity{ 2.0, 0.1, 0.1, 0.1, 0.4 }, 
 	_projectionScaleFactor{ 1.000004 }, // calculated with two vectors with known diff in length, then projDiffLength/diffLength.
+	_currentRadius{ 1.0 },
 	_directTouchMode{ false }, _tap{ false }
 {
 	addProperty(_touchScreenSize);
+	levmarq_init(&_lmstat);
 	_origin.onChange([this]() {
 		SceneGraphNode* node = sceneGraphNode(_origin.value());
 		if (!node) {
@@ -79,35 +81,64 @@ TouchInteraction::TouchInteraction()
 TouchInteraction::~TouchInteraction() { }
 
 void TouchInteraction::update(const std::vector<TuioCursor>& list, std::vector<Point>& lastProcessed) {
-	setCamera(OsEng.interactionHandler().camera());
-	setFocusNode(OsEng.interactionHandler().focusNode()); // since functions cant be called directly (TouchInteraction not a subclass of InteractionMode)
-
 	trace(list);
-	if (_currentRadius > 0.3 && _selected.size() == list.size()) { // good value to make any planet sufficiently large for direct-touch
+	if (/*_currentRadius > 0.3 &&*/ _selected.size() == list.size()) { // good value to make any planet sufficiently large for direct-touch, needs better definition
 		_directTouchMode = true;
 	}
 	else {
 		_directTouchMode = false;
 	}
+
+	if (_directTouchMode) {
+		/*
+		1, define s(xi,q): newXi = T(tx,ty,tz)Q(rx,ry,rz)xi, s(xi,q) = modelToScreenSpace(newXi)
+		2, calculate minimum error E = sum( ||s(xi,q)-pi||^2 ) (and define q in the process)
+			* xi is the old modelview position (_selected.at(i).coordinates),
+			* q the 6DOF vector (Trans(x,y,z)Quat(x,y,z)) to be defined that will move xi to a new pos,
+			* pi the current point in screen space (list.at(i).getXY)
+		3, Do the inverse rotation of M(q) on the camera, map interactions to different number of direct touch points
+		*/
+		// define these according to the M(q)
+		auto func = [](double* par, int x, void* fdata) {
+			return par[0] + (par[1] - par[0]) * exp(-par[2] * x);
+		};
+		auto grad = [](double* g, double* par, int x, void* fdata) {
+			g[0] = 1.0 + exp(-par[2] * x);
+			g[1] = exp(-par[2] * x);
+			g[2] = -x * (par[1] - par[0]) * exp(-par[2] * x);
+		};
+
+
+
+		const int nDOF = 6; // 6 degrees of freedom
+		//glm::dquat quat = glm::quat_cast(_selected.at(0).node->rotationMatrix());
+		//glm::dvec3 trans = _sekected.at(0).node->worldPosition();
+		double q[nDOF] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; // initial values of q or 0.0? (ie current model or no rotation/translation)
+		const int nFingers = _selected.size() * 2;
+		double* contactPoints = new double[nFingers];
+		double* squaredError = new double[nFingers];
+		int i = 0;
+		for (const SelectedBody& sb : _selected) {
+			glm::dvec2 screenPoint = modelToScreenSpace(sb); // transform to screen-space
+			contactPoints[i] = screenPoint.x;
+			contactPoints[i + 1] = screenPoint.y;
+
+			std::vector<TuioCursor>::const_iterator cursor = find_if(list.begin(), list.end(), [&sb](const TuioCursor& c) { return sb.id == c.getSessionID(); });
+			squaredError[i] = pow(screenPoint.x - cursor->getX(), 2); // squared error to calculate weighted least-square
+			squaredError[i + 1] = pow(screenPoint.y - cursor->getY(), 2);
+
+			i += 2;
+		}
+		levmarq_init(&_lmstat);
+		int nIterations = levmarq(nDOF, q, nFingers, contactPoints, squaredError, func, grad, NULL, &_lmstat);
+		delete[] contactPoints;
+		delete[] squaredError;
+	}
+	//else {
+		interpret(list, lastProcessed);
+		accelerate(list, lastProcessed);
+	//}
 	
-
-
-	/*
-	if (_directTouchMode)
-	assumes all contact points are direct --> if(_selected.size() == list.size())
-	1, check if _selected is initialized
-	2, define s(xi,q): newXi = T(tx,ty,tz)Q(rx,ry,rz)xi, s(xi,q) = modelToScreenSpace(newXi)
-	3, calculate minimum error E = sum( ||s(xi,q)-pi||^2 ) (and define q in the process)
-		* xi is the old modelview position (_selected.at(i).coordinates),
-		* q the 6DOF vector (Trans(x,y,z)Quat(x,y,z)) to be defined that will move xi to a new pos,
-		* pi the current point in screen space (list.at(i).getXY)
-	4, Do the inverse rotation of M(q) on the camera, map interactions to different number of direct touch points
-
-
-	else
-	*/
-	interpret(list, lastProcessed);
-	accelerate(list, lastProcessed);
 }
 
 void TouchInteraction::trace(const std::vector<TuioCursor>& list) {
@@ -132,10 +163,10 @@ void TouchInteraction::trace(const std::vector<TuioCursor>& list) {
 		double yCo = -2 * (c.getY() - 0.5); // normalized -1 to 1 coordinates on screen
 		glm::dvec3 cursorInWorldSpace = camToWorldSpace * glm::dvec3(xCo, yCo, -3.2596558);
 		glm::dvec3 raytrace = glm::normalize(cursorInWorldSpace);
+		int id = c.getSessionID();
 		for (SceneGraphNode* node : selectableNodes) {
 			double boundingSphere = node->boundingSphere().lengthd();
 			glm::dvec3 camToSelectable = node->worldPosition() - camPos;
-			int id = c.getSessionID();
 			double dist = length(glm::cross(cursorInWorldSpace, camToSelectable)) / glm::length(cursorInWorldSpace) - boundingSphere;
 			if (dist <= 0.0) {
 				// finds intersection closest point between boundingsphere and line in world coordinates, assumes line direction is normalized
@@ -302,6 +333,7 @@ void TouchInteraction::accelerate(const std::vector<TuioCursor>& list, const std
 		_vel.localRoll += -rollFactor * _sensitivity.localRoll;
 	}
 	if (_action.pick) { // pick something in the scene as focus node
+		
 		if (_selected.size() == 1 && _selected.at(0).node != _focusNode) {
 			_focusNode = _selected.at(0).node; // rotate camera to look at new focus
 			OsEng.interactionHandler().setFocusNode(_focusNode); // cant do setFocusNode since TouchInteraction is not subclass of InteractionMode
@@ -328,7 +360,6 @@ void TouchInteraction::accelerate(const std::vector<TuioCursor>& list, const std
 void TouchInteraction::step(double dt) {
 	using namespace glm;
 
-	setCamera(OsEng.interactionHandler().camera());
 	setFocusNode(OsEng.interactionHandler().focusNode()); // since functions cant be called directly (TouchInteraction not a subclass of InteractionMode)
 	if (_focusNode && _camera) {
 		// Create variables from current state
@@ -355,7 +386,7 @@ void TouchInteraction::step(double dt) {
 
 		double distance = std::max(length(centerToCamera) - boundingSphere, 0.0);
 		_currentRadius = boundingSphere / std::max(distance * _projectionScaleFactor, 1.0);
-
+		
 		{ // Roll
 			dquat camRollRot = angleAxis(_vel.localRoll*dt, dvec3(0.0, 0.0, 1.0));
 			localCamRot = localCamRot * camRollRot;

modules/touch/touchmodule.cpp

@@ -123,6 +123,9 @@ TouchModule::TouchModule()
 	OsEng.registerModuleCallback(
 		OpenSpaceEngine::CallbackOption::PreSync,
 		[&]() {
+		touch->setCamera(OsEng.interactionHandler().camera());
+		touch->setFocusNode(OsEng.interactionHandler().focusNode());
+
 		if (gotNewInput() && OsEng.windowWrapper().isMaster()) {
 			touch->update(list, lastProcessed);