From ab877feaf989de3201f31e090deea4ad803930be Mon Sep 17 00:00:00 2001 From: Jonathan Bosson Date: Wed, 12 Apr 2017 14:37:28 -0600 Subject: [PATCH] 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 | 2 + modules/touch/ext/levmarq.cpp | 221 +++++++++++++++++++++++ modules/touch/ext/levmarq.h | 51 ++++++ modules/touch/include/TouchInteraction.h | 2 + modules/touch/src/TouchInteraction.cpp | 83 ++++++--- modules/touch/touchmodule.cpp | 3 + 6 files changed, 336 insertions(+), 26 deletions(-) create mode 100644 modules/touch/ext/levmarq.cpp create mode 100644 modules/touch/ext/levmarq.h diff --git a/modules/touch/CMakeLists.txt b/modules/touch/CMakeLists.txt index 0951da2c6b..48f12b0afb 100644 --- a/modules/touch/CMakeLists.txt +++ b/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 ) diff --git a/modules/touch/ext/levmarq.cpp b/modules/touch/ext/levmarq.cpp new file mode 100644 index 0000000000..349ffe99a4 --- /dev/null +++ b/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 +#include +#include + +#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 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) && (-derrfinal_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= 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; +} diff --git a/modules/touch/ext/levmarq.h b/modules/touch/ext/levmarq.h new file mode 100644 index 0000000000..3b5117edd7 --- /dev/null +++ b/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); diff --git a/modules/touch/include/TouchInteraction.h b/modules/touch/include/TouchInteraction.h index ee7062f143..f7673ebf2a 100644 --- a/modules/touch/include/TouchInteraction.h +++ b/modules/touch/include/TouchInteraction.h @@ -27,6 +27,7 @@ #include #include +#include #include #include @@ -130,6 +131,7 @@ class TouchInteraction : public properties::PropertyOwner std::vector _selected; InteractionType _action; + LMstat _lmstat; glm::dvec3 _centroid; VelocityStates _vel; ScaleFactor _friction; diff --git a/modules/touch/src/TouchInteraction.cpp b/modules/touch/src/TouchInteraction.cpp index 1560a09a6f..bc44d1bd93 100644 --- a/modules/touch/src/TouchInteraction.cpp +++ b/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& list, std::vector& 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::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& list) { @@ -132,10 +163,10 @@ void TouchInteraction::trace(const std::vector& 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& 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& 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; diff --git a/modules/touch/touchmodule.cpp b/modules/touch/touchmodule.cpp index 22a3b1e96a..0a6bbba6de 100644 --- a/modules/touch/touchmodule.cpp +++ b/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);