diff --git a/modules/touch/ext/levmarq.cpp b/modules/touch/ext/levmarq.cpp index 31729c0c23..8255246e0a 100644 --- a/modules/touch/ext/levmarq.cpp +++ b/modules/touch/ext/levmarq.cpp @@ -58,8 +58,8 @@ Before calling levmarq, several of the parameters in lmstat must be set. For default values, call levmarq_init(lmstat). */ bool levmarq(int npar, double *par, int ny, double *dysq, - double (*func)(double *, int, void *), - void (*grad)(double *, double *, int, void *), + double (*func)(double *, int, void *, LMstat*), + void (*grad)(double *, double *, int, void *, LMstat*), void *fdata, LMstat *lmstat) { int x, i, j, it, nit, ill; @@ -89,11 +89,36 @@ bool levmarq(int npar, double *par, int ny, double *dysq, derr = newerr = 0; // to avoid compiler warnings if (verbose) { - data = "it,err,derr,q,g,d\n"; + std::ostringstream qs, gs, ds, ps, oss; + for (int i = 0; i < npar; ++i) { + qs << "q" << i; + gs << "g" << i; + ds << "q" << i; + if (i + 1 < npar) { + qs << ","; + gs << ","; + ds << ","; + } + } + int k = 1; + for (int i = 0; i < ny; ++i) { + for (int j = 0; j < ny; ++j) { + ps << "pos" << i << j; + if (j + 1 < ny) { + ps << ","; + } + } + if (i + 1 < ny) { + ps << ","; + } + } + oss << "it,err,derr," << qs.str() << "," << gs.str() << "," << ds.str() << "," << ps.str() << "\n"; + data = oss.str(); } // calculate the initial error ("chi-squared") - err = error_func(par, ny, dysq, func, fdata); + lmstat->pos.clear(); + err = error_func(par, ny, dysq, func, fdata, lmstat); // main iteration for (it = 0; it < nit; it++) { @@ -106,9 +131,9 @@ bool levmarq(int npar, double *par, int ny, double *dysq, for (x = 0; x < ny; x++) { if (dysq) weight = 1/dysq[x]; // for weighted least-squares - grad(g, par, x, fdata); + grad(g, par, x, fdata, lmstat); for (i = 0; i < npar; i++) { - d[i] += (0.0 - func(par, x, fdata)) * g[i] * weight; //(y[x] - func(par, x, fdata)) * g[i] * weight; + d[i] += (0.0 - func(par, x, fdata, lmstat)) * 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; } @@ -124,7 +149,8 @@ bool levmarq(int npar, double *par, int ny, double *dysq, solve_axb_cholesky(npar, ch, delta, d); for (i = 0; i < npar; i++) newpar[i] = par[i] + delta[i]; - newerr = error_func(newpar, ny, dysq, func, fdata); + lmstat->pos.clear(); + newerr = error_func(newpar, ny, dysq, func, fdata, lmstat); derr = newerr - err; ill = (derr > 0); } @@ -135,18 +161,24 @@ bool levmarq(int npar, double *par, int ny, double *dysq, } printf("\n");*/ - std::ostringstream gString, qString, dString, os; + std::ostringstream gString, qString, dString, pString, os; for (int i = 0; i < npar; ++i) { gString << g[i]; qString << par[i]; dString << d[i]; if (i + 1 < npar) { - gString << " "; - qString << " "; - dString << " "; + gString << ","; + qString << ","; + dString << ","; } } - os << it << "," << err << "," << derr << "," << qString.str() << "," << gString.str() << "," << dString.str() << "\n"; + for (int i = 0; i < ny; ++i) { + pString << lmstat->pos.at(i).x << "," << lmstat->pos.at(i).y; + if (i + 1 < ny) { + pString << ","; + } + } + os << it << "," << err << "," << derr << "," << qString.str() << "," << gString.str() << "," << dString.str() << "," << pString.str() << "\n"; data.append(os.str()); } if (ill) { @@ -187,12 +219,12 @@ bool levmarq(int npar, double *par, int ny, double *dysq, // calculate the error function (chi-squared) double error_func(double *par, int ny, double *dysq, - double (*func)(double *, int, void *), void *fdata) { + double (*func)(double *, int, void *, LMstat*), void *fdata, LMstat *lmstat) { int x; double res, e = 0; for (x = 0; x < ny; x++) { - res = func(par, x, fdata); + res = func(par, x, fdata, lmstat); if (dysq) // weighted least-squares e += res*res/dysq[x]; else diff --git a/modules/touch/ext/levmarq.h b/modules/touch/ext/levmarq.h index 2c8f6407f6..d3c7d77892 100644 --- a/modules/touch/ext/levmarq.h +++ b/modules/touch/ext/levmarq.h @@ -25,10 +25,13 @@ OTHER DEALINGS IN THE SOFTWARE. */ #include #include +#include +#include typedef struct { bool verbose; std::string data; + std::vector pos; int max_it; double init_lambda; double up_factor; @@ -42,12 +45,12 @@ typedef struct { void levmarq_init(LMstat *lmstat); bool levmarq(int npar, double *par, int ny, double *dysq, - double (*func)(double *, int, void *), - void (*grad)(double *, double *, int, void *), + double (*func)(double *, int, void *, LMstat*), + void (*grad)(double *, double *, int, void *, LMstat*), void *fdata, LMstat *lmstat); double error_func(double *par, int ny, double *dysq, - double (*func)(double *, int, void *), void *fdata); + double (*func)(double *, int, void *, LMstat*), void *fdata, LMstat* lmstat); 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 8de72fc6de..0de5509b98 100644 --- a/modules/touch/include/TouchInteraction.h +++ b/modules/touch/include/TouchInteraction.h @@ -71,10 +71,11 @@ struct FunctionData { std::vector screenPoints; int nDOF; glm::dvec2(*castToNDC)(glm::dvec3, Camera&, SceneGraphNode*, double); - double(*distToMinimize)(double* par, int x, void* fdata); + double(*distToMinimize)(double* par, int x, void* fdata, LMstat* lmstat); Camera* camera; SceneGraphNode* node; double aspectRatio; + LMstat stats; }; #define ROT 0 diff --git a/modules/touch/src/TouchInteraction.cpp b/modules/touch/src/TouchInteraction.cpp index 3eea25f6e1..8039e4984f 100644 --- a/modules/touch/src/TouchInteraction.cpp +++ b/modules/touch/src/TouchInteraction.cpp @@ -164,7 +164,7 @@ bool TouchInteraction::gui(const std::vector& list) { // Sets _vel to update _camera according to direct-manipulation (L2 error) void TouchInteraction::manipulate(const std::vector& list) { // Returns the screen point s(xi,par) dependant the transform M(par) and object point xi - auto distToMinimize = [](double* par, int x, void* fdata) { + auto distToMinimize = [](double* par, int x, void* fdata, LMstat* lmstat) { FunctionData* ptr = reinterpret_cast(fdata); // Apply transform to camera and find the new screen point of the updated camera state @@ -228,13 +228,13 @@ void TouchInteraction::manipulate(const std::vector& list) { // we now have a new position and orientation of camera, project surfacePoint to the new screen to get distance to minimize glm::dvec2 newScreenPoint = ptr->castToNDC(ptr->selectedPoints.at(x), cam, ptr->node, ptr->aspectRatio); - + lmstat->pos.push_back(newScreenPoint); return glm::length(ptr->screenPoints.at(x) - newScreenPoint); }; // Gradient of distToMinimize w.r.t par (using forward difference) - auto gradient = [](double* g, double* par, int x, void* fdata) { + auto gradient = [](double* g, double* par, int x, void* fdata, LMstat* lmstat) { FunctionData* ptr = reinterpret_cast(fdata); - double f0 = ptr->distToMinimize(par, x, fdata); + double f0 = ptr->distToMinimize(par, x, fdata, lmstat); double f1, der, minStep = 1e-11; glm::dvec3 camPos = ptr->camera->positionVec3(); glm::dvec3 selectedPoint = (ptr->node->rotationMatrix() * ptr->selectedPoints.at(x)) + ptr->node->worldPosition(); @@ -247,7 +247,7 @@ void TouchInteraction::manipulate(const std::vector& list) { for (int i = 0; i < ptr->nDOF; ++i) { h = (i == 2) ? 1e-4 : h; // the 'zoom'-DOF is so big a smaller step creates NAN dPar[i] += h; - f1 = ptr->distToMinimize(dPar, x, fdata); + f1 = ptr->distToMinimize(dPar, x, fdata, lmstat); dPar[i] -= h; der = (f1 - f0) / h; @@ -283,7 +283,7 @@ void TouchInteraction::manipulate(const std::vector& list) { screenPoints.push_back(glm::dvec2(xCo, yCo)); } //glm::dvec2 res = OsEng.windowWrapper().currentWindowResolution(); - FunctionData fData = { selectedPoints, screenPoints, nDOF, castToNDC, distToMinimize, _camera, node, 1.88 }; + FunctionData fData = { selectedPoints, screenPoints, nDOF, castToNDC, distToMinimize, _camera, node, 1.88, _lmstat }; void* dataPtr = reinterpret_cast(&fData); _lmSuccess = levmarq(nDOF, par, nFingers, NULL, distToMinimize, gradient, dataPtr, &_lmstat); // finds best transform values and stores them in par @@ -625,9 +625,11 @@ void TouchInteraction::unitTest() { // set _selected pos and new pos (on screen) std::vector lastFrame; - lastFrame.push_back(TuioCursor(0, 1, 0.2, 0.5)); // session id, cursor id, x, y + lastFrame.push_back(TuioCursor(0, 10, 0.45, 0.4)); // session id, cursor id, x, y + lastFrame.push_back(TuioCursor(1, 11, 0.55, 0.6)); std::vector currFrame; - currFrame.push_back(TuioCursor(0, 1, 0.8, 0.5)); + currFrame.push_back(TuioCursor(0, 10, 0.2, 0.6)); // (-0.6,0) + currFrame.push_back(TuioCursor(1, 11, 0.8, 0.4)); // (0.6, 0) // call update trace(lastFrame);