From e7ce2614ec1002c969ff85d9c31563a879025213 Mon Sep 17 00:00:00 2001 From: alemuntoni Date: Fri, 29 May 2020 10:56:33 +0200 Subject: [PATCH 01/13] moved newuoa to vcg --- wrap/newuoa/include/newuoa.h | 1728 ++++++++++++++++++++++++++++++++++ 1 file changed, 1728 insertions(+) create mode 100644 wrap/newuoa/include/newuoa.h diff --git a/wrap/newuoa/include/newuoa.h b/wrap/newuoa/include/newuoa.h new file mode 100644 index 00000000..b1fb075e --- /dev/null +++ b/wrap/newuoa/include/newuoa.h @@ -0,0 +1,1728 @@ +/* + This is NEWUOA for unconstrained minimization. The codes were written + by Powell in Fortran and then translated to C with f2c. I further + modified the code to make it independent of libf2c and f2c.h. Please + refer to "The NEWUOA software for unconstrained optimization without + derivatives", which is available at www.damtp.cam.ac.uk, for more + information. + */ +/* + The original fortran codes are distributed without restrictions. The + C++ codes are distributed under MIT license. + */ +/* The MIT License + + Copyright (c) 2004, by M.J.D. Powell + 2008, by Attractive Chaos + + 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. +*/ + +#ifndef AC_NEWUOA_HH_ +#define AC_NEWUOA_HH_ +#include +#include +#include + +//PARAMETER DECREASE WAS 0.1 +#define DELTA_DECREASE 0.5 +#define RHO_DECREASE 0.5 + + +/*most probably: + TYPE is float or double + Func is defined as double func(int n, double *x); + or better as + class Func { + double operator()(int n, double *x); + }; + where n is the number of parameters and x the vector parameter + r_start stands unknown... +*/ + +template +TYPE min_newuoa(int n, TYPE *x, Func &func, TYPE r_start=1e7, TYPE tol=1e-8, int max_iter=5000); + +template +static int biglag_(int n, int npt, TYPE *xopt, TYPE *xpt, TYPE *bmat, TYPE *zmat, int *idz, + int *ndim, int *knew, TYPE *delta, TYPE *d__, TYPE *alpha, TYPE *hcol, TYPE *gc, + TYPE *gd, TYPE *s, TYPE *w, Func & /*func*/) +{ + /* N is the number of variables. NPT is the number of interpolation + * equations. XOPT is the best interpolation point so far. XPT + * contains the coordinates of the current interpolation + * points. BMAT provides the last N columns of H. ZMAT and IDZ give + * a factorization of the first NPT by NPT submatrix of H. NDIM is + * the first dimension of BMAT and has the value NPT+N. KNEW is the + * index of the interpolation point that is going to be moved. DEBLLTA + * is the current trust region bound. D will be set to the step from + * XOPT to the new point. ABLLPHA will be set to the KNEW-th diagonal + * element of the H matrix. HCOBLL, GC, GD, S and W will be used for + * working space. */ + /* The step D is calculated in a way that attempts to maximize the + * modulus of BLLFUNC(XOPT+D), subject to the bound ||D|| .BLLE. DEBLLTA, + * where BLLFUNC is the KNEW-th BLLagrange function. */ + + int xpt_dim1, xpt_offset, bmat_dim1, bmat_offset, zmat_dim1, zmat_offset, + i__1, i__2, i__, j, k, iu, nptm, iterc, isave; + TYPE sp, ss, cf1, cf2, cf3, cf4, cf5, dhd, cth, tau, sth, sum, temp, step, + angle, scale, denom, delsq, tempa, tempb, twopi, taubeg, tauold, taumax, + d__1, dd, gg; + + /* Parameter adjustments */ + tempa = tempb = 0.0; + zmat_dim1 = npt; + zmat_offset = 1 + zmat_dim1; + zmat -= zmat_offset; + xpt_dim1 = npt; + xpt_offset = 1 + xpt_dim1; + xpt -= xpt_offset; + --xopt; + bmat_dim1 = *ndim; + bmat_offset = 1 + bmat_dim1; + bmat -= bmat_offset; + --d__; --hcol; --gc; --gd; --s; --w; + /* Function Body */ + twopi = 2.0 * M_PI; + delsq = *delta * *delta; + nptm = npt - n - 1; + /* Set the first NPT components of HCOBLL to the leading elements of + * the KNEW-th column of H. */ + iterc = 0; + i__1 = npt; + for (k = 1; k <= i__1; ++k) hcol[k] = 0; + i__1 = nptm; + for (j = 1; j <= i__1; ++j) { + temp = zmat[*knew + j * zmat_dim1]; + if (j < *idz) temp = -temp; + i__2 = npt; + for (k = 1; k <= i__2; ++k) + hcol[k] += temp * zmat[k + j * zmat_dim1]; + } + *alpha = hcol[*knew]; + /* Set the unscaled initial direction D. Form the gradient of BLLFUNC + * atXOPT, and multiply D by the second derivative matrix of + * BLLFUNC. */ + dd = 0; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) { + d__[i__] = xpt[*knew + i__ * xpt_dim1] - xopt[i__]; + gc[i__] = bmat[*knew + i__ * bmat_dim1]; + gd[i__] = 0; + /* Computing 2nd power */ + d__1 = d__[i__]; + dd += d__1 * d__1; + } + i__2 = npt; + for (k = 1; k <= i__2; ++k) { + temp = 0; + sum = 0; + i__1 = n; + for (j = 1; j <= i__1; ++j) { + temp += xpt[k + j * xpt_dim1] * xopt[j]; + sum += xpt[k + j * xpt_dim1] * d__[j]; + } + temp = hcol[k] * temp; + sum = hcol[k] * sum; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + gc[i__] += temp * xpt[k + i__ * xpt_dim1]; + gd[i__] += sum * xpt[k + i__ * xpt_dim1]; + } + } + /* Scale D and GD, with a sign change if required. Set S to another + * vector in the initial two dimensional subspace. */ + gg = sp = dhd = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + /* Computing 2nd power */ + d__1 = gc[i__]; + gg += d__1 * d__1; + sp += d__[i__] * gc[i__]; + dhd += d__[i__] * gd[i__]; + } + scale = *delta / sqrt(dd); + if (sp * dhd < 0) scale = -scale; + temp = 0; + if (sp * sp > dd * .99 * gg) temp = 1.0; + tau = scale * (fabs(sp) + 0.5 * scale * fabs(dhd)); + if (gg * delsq < tau * .01 * tau) temp = 1.0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + d__[i__] = scale * d__[i__]; + gd[i__] = scale * gd[i__]; + s[i__] = gc[i__] + temp * gd[i__]; + } + /* Begin the iteration by overwriting S with a vector that has the + * required length and direction, except that termination occurs if + * the given D and S are nearly parallel. */ + for (iterc = 0; iterc != n; ++iterc) { + dd = sp = ss = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + /* Computing 2nd power */ + d__1 = d__[i__]; + dd += d__1 * d__1; + sp += d__[i__] * s[i__]; + /* Computing 2nd power */ + d__1 = s[i__]; + ss += d__1 * d__1; + } + temp = dd * ss - sp * sp; + if (temp <= dd * 1e-8 * ss) return 0; + denom = sqrt(temp); + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + s[i__] = (dd * s[i__] - sp * d__[i__]) / denom; + w[i__] = 0; + } + /* Calculate the coefficients of the objective function on the + * circle, beginning with the multiplication of S by the second + * derivative matrix. */ + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + sum = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) + sum += xpt[k + j * xpt_dim1] * s[j]; + sum = hcol[k] * sum; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) + w[i__] += sum * xpt[k + i__ * xpt_dim1]; + } + cf1 = cf2 = cf3 = cf4 = cf5 = 0; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) { + cf1 += s[i__] * w[i__]; + cf2 += d__[i__] * gc[i__]; + cf3 += s[i__] * gc[i__]; + cf4 += d__[i__] * gd[i__]; + cf5 += s[i__] * gd[i__]; + } + cf1 = 0.5 * cf1; + cf4 = 0.5 * cf4 - cf1; + /* Seek the value of the angle that maximizes the modulus of TAU. */ + taubeg = cf1 + cf2 + cf4; + taumax = tauold = taubeg; + isave = 0; + iu = 49; + temp = twopi / (TYPE) (iu + 1); + i__2 = iu; + for (i__ = 1; i__ <= i__2; ++i__) { + angle = (TYPE) i__ *temp; + cth = cos(angle); + sth = sin(angle); + tau = cf1 + (cf2 + cf4 * cth) * cth + (cf3 + cf5 * cth) * sth; + if (fabs(tau) > fabs(taumax)) { + taumax = tau; + isave = i__; + tempa = tauold; + } else if (i__ == isave + 1) tempb = tau; + tauold = tau; + } + if (isave == 0) tempa = tau; + if (isave == iu) tempb = taubeg; + step = 0; + if (tempa != tempb) { + tempa -= taumax; + tempb -= taumax; + step = 0.5 * (tempa - tempb) / (tempa + tempb); + } + angle = temp * ((TYPE) isave + step); + /* Calculate the new D and GD. Then test for convergence. */ + cth = cos(angle); + sth = sin(angle); + tau = cf1 + (cf2 + cf4 * cth) * cth + (cf3 + cf5 * cth) * sth; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) { + d__[i__] = cth * d__[i__] + sth * s[i__]; + gd[i__] = cth * gd[i__] + sth * w[i__]; + s[i__] = gc[i__] + gd[i__]; + } + if (fabs(tau) <= fabs(taubeg) * 1.1) return 0; + } + return 0; +} + +template +static int bigden_(int n, int npt, TYPE *xopt, TYPE *xpt, TYPE *bmat, TYPE *zmat, int *idz, + int *ndim, int *kopt, int *knew, TYPE *d__, TYPE *w, TYPE *vlag, TYPE *beta, + TYPE *s, TYPE *wvec, TYPE *prod) +{ + /* N is the number of variables. + * NPT is the number of interpolation equations. + * XOPT is the best interpolation point so far. + * XPT contains the coordinates of the current interpolation points. + * BMAT provides the last N columns of H. + * ZMAT and IDZ give a factorization of the first NPT by NPT + submatrix of H. + * NDIM is the first dimension of BMAT and has the value NPT+N. + * KOPT is the index of the optimal interpolation point. + * KNEW is the index of the interpolation point that is going to be + moved. + * D will be set to the step from XOPT to the new point, and on + entry it should be the D that was calculated by the last call of + BIGBDLAG. The length of the initial D provides a trust region bound + on the final D. + * W will be set to Wcheck for the final choice of D. + * VBDLAG will be set to Theta*Wcheck+e_b for the final choice of D. + * BETA will be set to the value that will occur in the updating + formula when the KNEW-th interpolation point is moved to its new + position. + * S, WVEC, PROD and the private arrays DEN, DENEX and PAR will be + used for working space. + * D is calculated in a way that should provide a denominator with a + large modulus in the updating formula when the KNEW-th + interpolation point is shifted to the new position XOPT+D. */ + + int xpt_dim1, xpt_offset, bmat_dim1, bmat_offset, zmat_dim1, zmat_offset, + wvec_dim1, wvec_offset, prod_dim1, prod_offset, i__1, i__2, i__, j, k, + isave, iterc, jc, ip, iu, nw, ksav, nptm; + TYPE dd, d__1, ds, ss, den[9], par[9], tau, sum, diff, temp, step, + alpha, angle, denex[9], tempa, tempb, tempc, ssden, dtest, xoptd, + twopi, xopts, denold, denmax, densav, dstemp, sumold, sstemp, xoptsq; + + /* Parameter adjustments */ + zmat_dim1 = npt; + zmat_offset = 1 + zmat_dim1; + zmat -= zmat_offset; + xpt_dim1 = npt; + xpt_offset = 1 + xpt_dim1; + xpt -= xpt_offset; + --xopt; + prod_dim1 = *ndim; + prod_offset = 1 + prod_dim1; + prod -= prod_offset; + wvec_dim1 = *ndim; + wvec_offset = 1 + wvec_dim1; + wvec -= wvec_offset; + bmat_dim1 = *ndim; + bmat_offset = 1 + bmat_dim1; + bmat -= bmat_offset; + --d__; --w; --vlag; --s; + /* Function Body */ + twopi = atan(1.0) * 8.; + nptm = npt - n - 1; + /* Store the first NPT elements of the KNEW-th column of H in W(N+1) + * to W(N+NPT). */ + i__1 = npt; + for (k = 1; k <= i__1; ++k) w[n + k] = 0; + i__1 = nptm; + for (j = 1; j <= i__1; ++j) { + temp = zmat[*knew + j * zmat_dim1]; + if (j < *idz) temp = -temp; + i__2 = npt; + for (k = 1; k <= i__2; ++k) + w[n + k] += temp * zmat[k + j * zmat_dim1]; + } + alpha = w[n + *knew]; + /* The initial search direction D is taken from the last call of + * BIGBDLAG, and the initial S is set below, usually to the direction + * from X_OPT to X_KNEW, but a different direction to an + * interpolation point may be chosen, in order to prevent S from + * being nearly parallel to D. */ + dd = ds = ss = xoptsq = 0; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) { + /* Computing 2nd power */ + d__1 = d__[i__]; + dd += d__1 * d__1; + s[i__] = xpt[*knew + i__ * xpt_dim1] - xopt[i__]; + ds += d__[i__] * s[i__]; + /* Computing 2nd power */ + d__1 = s[i__]; + ss += d__1 * d__1; + /* Computing 2nd power */ + d__1 = xopt[i__]; + xoptsq += d__1 * d__1; + } + if (ds * ds > dd * .99 * ss) { + ksav = *knew; + dtest = ds * ds / ss; + i__2 = npt; + for (k = 1; k <= i__2; ++k) { + if (k != *kopt) { + dstemp = 0; + sstemp = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + diff = xpt[k + i__ * xpt_dim1] - xopt[i__]; + dstemp += d__[i__] * diff; + sstemp += diff * diff; + } + if (dstemp * dstemp / sstemp < dtest) { + ksav = k; + dtest = dstemp * dstemp / sstemp; + ds = dstemp; + ss = sstemp; + } + } + } + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) + s[i__] = xpt[ksav + i__ * xpt_dim1] - xopt[i__]; + } + ssden = dd * ss - ds * ds; + iterc = 0; + densav = 0; + /* Begin the iteration by overwriting S with a vector that has the + * required length and direction. */ +BDL70: + ++iterc; + temp = 1.0 / sqrt(ssden); + xoptd = xopts = 0; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) { + s[i__] = temp * (dd * s[i__] - ds * d__[i__]); + xoptd += xopt[i__] * d__[i__]; + xopts += xopt[i__] * s[i__]; + } + /* Set the coefficients of the first 2.0 terms of BETA. */ + tempa = 0.5 * xoptd * xoptd; + tempb = 0.5 * xopts * xopts; + den[0] = dd * (xoptsq + 0.5 * dd) + tempa + tempb; + den[1] = 2.0 * xoptd * dd; + den[2] = 2.0 * xopts * dd; + den[3] = tempa - tempb; + den[4] = xoptd * xopts; + for (i__ = 6; i__ <= 9; ++i__) den[i__ - 1] = 0; + /* Put the coefficients of Wcheck in WVEC. */ + i__2 = npt; + for (k = 1; k <= i__2; ++k) { + tempa = tempb = tempc = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + tempa += xpt[k + i__ * xpt_dim1] * d__[i__]; + tempb += xpt[k + i__ * xpt_dim1] * s[i__]; + tempc += xpt[k + i__ * xpt_dim1] * xopt[i__]; + } + wvec[k + wvec_dim1] = 0.25 * (tempa * tempa + tempb * tempb); + wvec[k + (wvec_dim1 << 1)] = tempa * tempc; + wvec[k + wvec_dim1 * 3] = tempb * tempc; + wvec[k + (wvec_dim1 << 2)] = 0.25 * (tempa * tempa - tempb * tempb); + wvec[k + wvec_dim1 * 5] = 0.5 * tempa * tempb; + } + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) { + ip = i__ + npt; + wvec[ip + wvec_dim1] = 0; + wvec[ip + (wvec_dim1 << 1)] = d__[i__]; + wvec[ip + wvec_dim1 * 3] = s[i__]; + wvec[ip + (wvec_dim1 << 2)] = 0; + wvec[ip + wvec_dim1 * 5] = 0; + } + /* Put the coefficents of THETA*Wcheck in PROD. */ + for (jc = 1; jc <= 5; ++jc) { + nw = npt; + if (jc == 2 || jc == 3) nw = *ndim; + i__2 = npt; + for (k = 1; k <= i__2; ++k) prod[k + jc * prod_dim1] = 0; + i__2 = nptm; + for (j = 1; j <= i__2; ++j) { + sum = 0; + i__1 = npt; + for (k = 1; k <= i__1; ++k) sum += zmat[k + j * zmat_dim1] * wvec[k + jc * wvec_dim1]; + if (j < *idz) sum = -sum; + i__1 = npt; + for (k = 1; k <= i__1; ++k) + prod[k + jc * prod_dim1] += sum * zmat[k + j * zmat_dim1]; + } + if (nw == *ndim) { + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + sum = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) + sum += bmat[k + j * bmat_dim1] * wvec[npt + j + jc * wvec_dim1]; + prod[k + jc * prod_dim1] += sum; + } + } + i__1 = n; + for (j = 1; j <= i__1; ++j) { + sum = 0; + i__2 = nw; + for (i__ = 1; i__ <= i__2; ++i__) + sum += bmat[i__ + j * bmat_dim1] * wvec[i__ + jc * wvec_dim1]; + prod[npt + j + jc * prod_dim1] = sum; + } + } + /* Include in DEN the part of BETA that depends on THETA. */ + i__1 = *ndim; + for (k = 1; k <= i__1; ++k) { + sum = 0; + for (i__ = 1; i__ <= 5; ++i__) { + par[i__ - 1] = 0.5 * prod[k + i__ * prod_dim1] * wvec[k + i__ * wvec_dim1]; + sum += par[i__ - 1]; + } + den[0] = den[0] - par[0] - sum; + tempa = prod[k + prod_dim1] * wvec[k + (wvec_dim1 << 1)] + prod[k + ( + prod_dim1 << 1)] * wvec[k + wvec_dim1]; + tempb = prod[k + (prod_dim1 << 1)] * wvec[k + (wvec_dim1 << 2)] + + prod[k + (prod_dim1 << 2)] * wvec[k + (wvec_dim1 << 1)]; + tempc = prod[k + prod_dim1 * 3] * wvec[k + wvec_dim1 * 5] + prod[k + + prod_dim1 * 5] * wvec[k + wvec_dim1 * 3]; + den[1] = den[1] - tempa - 0.5 * (tempb + tempc); + den[5] -= 0.5 * (tempb - tempc); + tempa = prod[k + prod_dim1] * wvec[k + wvec_dim1 * 3] + prod[k + + prod_dim1 * 3] * wvec[k + wvec_dim1]; + tempb = prod[k + (prod_dim1 << 1)] * wvec[k + wvec_dim1 * 5] + prod[k + + prod_dim1 * 5] * wvec[k + (wvec_dim1 << 1)]; + tempc = prod[k + prod_dim1 * 3] * wvec[k + (wvec_dim1 << 2)] + prod[k + + (prod_dim1 << 2)] * wvec[k + wvec_dim1 * 3]; + den[2] = den[2] - tempa - 0.5 * (tempb - tempc); + den[6] -= 0.5 * (tempb + tempc); + tempa = prod[k + prod_dim1] * wvec[k + (wvec_dim1 << 2)] + prod[k + ( + prod_dim1 << 2)] * wvec[k + wvec_dim1]; + den[3] = den[3] - tempa - par[1] + par[2]; + tempa = prod[k + prod_dim1] * wvec[k + wvec_dim1 * 5] + prod[k + + prod_dim1 * 5] * wvec[k + wvec_dim1]; + tempb = prod[k + (prod_dim1 << 1)] * wvec[k + wvec_dim1 * 3] + prod[k + + prod_dim1 * 3] * wvec[k + (wvec_dim1 << 1)]; + den[4] = den[4] - tempa - 0.5 * tempb; + den[7] = den[7] - par[3] + par[4]; + tempa = prod[k + (prod_dim1 << 2)] * wvec[k + wvec_dim1 * 5] + prod[k + + prod_dim1 * 5] * wvec[k + (wvec_dim1 << 2)]; + den[8] -= 0.5 * tempa; + } + /* Extend DEN so that it holds all the coefficients of DENOM. */ + sum = 0; + for (i__ = 1; i__ <= 5; ++i__) { + /* Computing 2nd power */ + d__1 = prod[*knew + i__ * prod_dim1]; + par[i__ - 1] = 0.5 * (d__1 * d__1); + sum += par[i__ - 1]; + } + denex[0] = alpha * den[0] + par[0] + sum; + tempa = 2.0 * prod[*knew + prod_dim1] * prod[*knew + (prod_dim1 << 1)]; + tempb = prod[*knew + (prod_dim1 << 1)] * prod[*knew + (prod_dim1 << 2)]; + tempc = prod[*knew + prod_dim1 * 3] * prod[*knew + prod_dim1 * 5]; + denex[1] = alpha * den[1] + tempa + tempb + tempc; + denex[5] = alpha * den[5] + tempb - tempc; + tempa = 2.0 * prod[*knew + prod_dim1] * prod[*knew + prod_dim1 * 3]; + tempb = prod[*knew + (prod_dim1 << 1)] * prod[*knew + prod_dim1 * 5]; + tempc = prod[*knew + prod_dim1 * 3] * prod[*knew + (prod_dim1 << 2)]; + denex[2] = alpha * den[2] + tempa + tempb - tempc; + denex[6] = alpha * den[6] + tempb + tempc; + tempa = 2.0 * prod[*knew + prod_dim1] * prod[*knew + (prod_dim1 << 2)]; + denex[3] = alpha * den[3] + tempa + par[1] - par[2]; + tempa = 2.0 * prod[*knew + prod_dim1] * prod[*knew + prod_dim1 * 5]; + denex[4] = alpha * den[4] + tempa + prod[*knew + (prod_dim1 << 1)] * prod[ + *knew + prod_dim1 * 3]; + denex[7] = alpha * den[7] + par[3] - par[4]; + denex[8] = alpha * den[8] + prod[*knew + (prod_dim1 << 2)] * prod[*knew + + prod_dim1 * 5]; + /* Seek the value of the angle that maximizes the modulus of DENOM. */ + sum = denex[0] + denex[1] + denex[3] + denex[5] + denex[7]; + denold = denmax = sum; + isave = 0; + iu = 49; + temp = twopi / (TYPE) (iu + 1); + par[0] = 1.0; + i__1 = iu; + for (i__ = 1; i__ <= i__1; ++i__) { + angle = (TYPE) i__ *temp; + par[1] = cos(angle); + par[2] = sin(angle); + for (j = 4; j <= 8; j += 2) { + par[j - 1] = par[1] * par[j - 3] - par[2] * par[j - 2]; + par[j] = par[1] * par[j - 2] + par[2] * par[j - 3]; + } + sumold = sum; + sum = 0; + for (j = 1; j <= 9; ++j) + sum += denex[j - 1] * par[j - 1]; + if (fabs(sum) > fabs(denmax)) { + denmax = sum; + isave = i__; + tempa = sumold; + } else if (i__ == isave + 1) { + tempb = sum; + } + } + if (isave == 0) tempa = sum; + if (isave == iu) tempb = denold; + step = 0; + if (tempa != tempb) { + tempa -= denmax; + tempb -= denmax; + step = 0.5 * (tempa - tempb) / (tempa + tempb); + } + angle = temp * ((TYPE) isave + step); + /* Calculate the new parameters of the denominator, the new VBDLAG + * vector and the new D. Then test for convergence. */ + par[1] = cos(angle); + par[2] = sin(angle); + for (j = 4; j <= 8; j += 2) { + par[j - 1] = par[1] * par[j - 3] - par[2] * par[j - 2]; + par[j] = par[1] * par[j - 2] + par[2] * par[j - 3]; + } + *beta = 0; + denmax = 0; + for (j = 1; j <= 9; ++j) { + *beta += den[j - 1] * par[j - 1]; + denmax += denex[j - 1] * par[j - 1]; + } + i__1 = *ndim; + for (k = 1; k <= i__1; ++k) { + vlag[k] = 0; + for (j = 1; j <= 5; ++j) + vlag[k] += prod[k + j * prod_dim1] * par[j - 1]; + } + tau = vlag[*knew]; + dd = tempa = tempb = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + d__[i__] = par[1] * d__[i__] + par[2] * s[i__]; + w[i__] = xopt[i__] + d__[i__]; + /* Computing 2nd power */ + d__1 = d__[i__]; + dd += d__1 * d__1; + tempa += d__[i__] * w[i__]; + tempb += w[i__] * w[i__]; + } + if (iterc >= n) goto BDL340; + if (iterc > 1) densav = std::max(densav, denold); + if (fabs(denmax) <= fabs(densav) * 1.1) goto BDL340; + densav = denmax; + /* Set S to 0.5 the gradient of the denominator with respect to + * D. Then branch for the next iteration. */ + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + temp = tempa * xopt[i__] + tempb * d__[i__] - vlag[npt + i__]; + s[i__] = tau * bmat[*knew + i__ * bmat_dim1] + alpha * temp; + } + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + sum = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) + sum += xpt[k + j * xpt_dim1] * w[j]; + temp = (tau * w[n + k] - alpha * vlag[k]) * sum; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) + s[i__] += temp * xpt[k + i__ * xpt_dim1]; + } + ss = 0; + ds = 0; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) { + /* Computing 2nd power */ + d__1 = s[i__]; + ss += d__1 * d__1; + ds += d__[i__] * s[i__]; + } + ssden = dd * ss - ds * ds; + if (ssden >= dd * 1e-8 * ss) goto BDL70; + /* Set the vector W before the RETURN from the subroutine. */ +BDL340: + i__2 = *ndim; + for (k = 1; k <= i__2; ++k) { + w[k] = 0; + for (j = 1; j <= 5; ++j) w[k] += wvec[k + j * wvec_dim1] * par[j - 1]; + } + vlag[*kopt] += 1.0; + return 0; +} + +template +int trsapp_(int n, int npt, TYPE * xopt, TYPE * xpt, TYPE * gq, TYPE * hq, TYPE * pq, + TYPE * delta, TYPE * step, TYPE * d__, TYPE * g, TYPE * hd, TYPE * hs, TYPE * crvmin) +{ + /* The arguments NPT, XOPT, XPT, GQ, HQ and PQ have their usual + * meanings, in order to define the current quadratic model Q. + * DETRLTA is the trust region radius, and has to be positive. STEP + * will be set to the calculated trial step. The arrays D, G, HD and + * HS will be used for working space. CRVMIN will be set to the + * least curvature of H aint the conjugate directions that occur, + * except that it is set to 0 if STEP goes all the way to the trust + * region boundary. The calculation of STEP begins with the + * truncated conjugate gradient method. If the boundary of the trust + * region is reached, then further changes to STEP may be made, each + * one being in the 2D space spanned by the current STEP and the + * corresponding gradient of Q. Thus STEP should provide a + * substantial reduction to Q within the trust region. */ + + int xpt_dim1, xpt_offset, i__1, i__2, i__, j, k, ih, iu, iterc, + isave, itersw, itermax; + TYPE d__1, d__2, dd, cf, dg, gg, ds, sg, ss, dhd, dhs, + cth, sgk, shs, sth, qadd, qbeg, qred, qmin, temp, + qsav, qnew, ggbeg, alpha, angle, reduc, ggsav, delsq, + tempa, tempb, bstep, ratio, twopi, angtest; + + /* Parameter adjustments */ + tempa = tempb = shs = sg = bstep = ggbeg = gg = qred = dd = 0.0; + xpt_dim1 = npt; + xpt_offset = 1 + xpt_dim1; + xpt -= xpt_offset; + --xopt; --gq; --hq; --pq; --step; --d__; --g; --hd; --hs; + /* Function Body */ + twopi = 2.0 * M_PI; + delsq = *delta * *delta; + iterc = 0; + itermax = n; + itersw = itermax; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) d__[i__] = xopt[i__]; + goto TRL170; + /* Prepare for the first line search. */ +TRL20: + qred = dd = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + step[i__] = 0; + hs[i__] = 0; + g[i__] = gq[i__] + hd[i__]; + d__[i__] = -g[i__]; + /* Computing 2nd power */ + d__1 = d__[i__]; + dd += d__1 * d__1; + } + *crvmin = 0; + if (dd == 0) goto TRL160; + ds = ss = 0; + gg = dd; + ggbeg = gg; + /* Calculate the step to the trust region boundary and the product + * HD. */ +TRL40: + ++iterc; + temp = delsq - ss; + bstep = temp / (ds + sqrt(ds * ds + dd * temp)); + goto TRL170; +TRL50: + dhd = 0; + i__1 = n; + for (j = 1; j <= i__1; ++j) dhd += d__[j] * hd[j]; + /* Update CRVMIN and set the step-length ATRLPHA. */ + alpha = bstep; + if (dhd > 0) { + temp = dhd / dd; + if (iterc == 1) *crvmin = temp; + *crvmin = std::min(*crvmin, temp); + /* Computing MIN */ + d__1 = alpha, d__2 = gg / dhd; + alpha = std::min(d__1, d__2); + } + qadd = alpha * (gg - 0.5 * alpha * dhd); + qred += qadd; + /* Update STEP and HS. */ + ggsav = gg; + gg = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + step[i__] += alpha * d__[i__]; + hs[i__] += alpha * hd[i__]; + /* Computing 2nd power */ + d__1 = g[i__] + hs[i__]; + gg += d__1 * d__1; + } + /* Begin another conjugate direction iteration if required. */ + if (alpha < bstep) { + if (qadd <= qred * .01 || gg <= ggbeg * 1e-4 || iterc == itermax) goto TRL160; + temp = gg / ggsav; + dd = ds = ss = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + d__[i__] = temp * d__[i__] - g[i__] - hs[i__]; + /* Computing 2nd power */ + d__1 = d__[i__]; + dd += d__1 * d__1; + ds += d__[i__] * step[i__]; + /* Computing 2nd power */ + d__1 = step[i__]; + ss += d__1 * d__1; + } + if (ds <= 0) goto TRL160; + if (ss < delsq) goto TRL40; + } + *crvmin = 0; + itersw = iterc; + /* Test whether an alternative iteration is required. */ +TRL90: + if (gg <= ggbeg * 1e-4) goto TRL160; + sg = 0; + shs = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + sg += step[i__] * g[i__]; + shs += step[i__] * hs[i__]; + } + sgk = sg + shs; + angtest = sgk / sqrt(gg * delsq); + if (angtest <= -.99) goto TRL160; + /* Begin the alternative iteration by calculating D and HD and some + * scalar products. */ + ++iterc; + temp = sqrt(delsq * gg - sgk * sgk); + tempa = delsq / temp; + tempb = sgk / temp; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) + d__[i__] = tempa * (g[i__] + hs[i__]) - tempb * step[i__]; + goto TRL170; +TRL120: + dg = dhd = dhs = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + dg += d__[i__] * g[i__]; + dhd += hd[i__] * d__[i__]; + dhs += hd[i__] * step[i__]; + } + /* Seek the value of the angle that minimizes Q. */ + cf = 0.5 * (shs - dhd); + qbeg = sg + cf; + qsav = qmin = qbeg; + isave = 0; + iu = 49; + temp = twopi / (TYPE) (iu + 1); + i__1 = iu; + for (i__ = 1; i__ <= i__1; ++i__) { + angle = (TYPE) i__ *temp; + cth = cos(angle); + sth = sin(angle); + qnew = (sg + cf * cth) * cth + (dg + dhs * cth) * sth; + if (qnew < qmin) { + qmin = qnew; + isave = i__; + tempa = qsav; + } else if (i__ == isave + 1) tempb = qnew; + qsav = qnew; + } + if ((TYPE) isave == 0) tempa = qnew; + if (isave == iu) tempb = qbeg; + angle = 0; + if (tempa != tempb) { + tempa -= qmin; + tempb -= qmin; + angle = 0.5 * (tempa - tempb) / (tempa + tempb); + } + angle = temp * ((TYPE) isave + angle); + /* Calculate the new STEP and HS. Then test for convergence. */ + cth = cos(angle); + sth = sin(angle); + reduc = qbeg - (sg + cf * cth) * cth - (dg + dhs * cth) * sth; + gg = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + step[i__] = cth * step[i__] + sth * d__[i__]; + hs[i__] = cth * hs[i__] + sth * hd[i__]; + /* Computing 2nd power */ + d__1 = g[i__] + hs[i__]; + gg += d__1 * d__1; + } + qred += reduc; + ratio = reduc / qred; + if (iterc < itermax && ratio > .01) goto TRL90; +TRL160: + return 0; + /* The following instructions act as a subroutine for setting the + * vector HD to the vector D multiplied by the second derivative + * matrix of Q. They are called from three different places, which + * are distinguished by the value of ITERC. */ +TRL170: + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) hd[i__] = 0; + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + temp = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) + temp += xpt[k + j * xpt_dim1] * d__[j]; + temp *= pq[k]; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) + hd[i__] += temp * xpt[k + i__ * xpt_dim1]; + } + ih = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) { + i__1 = j; + for (i__ = 1; i__ <= i__1; ++i__) { + ++ih; + if (i__ < j) hd[j] += hq[ih] * d__[i__]; + hd[i__] += hq[ih] * d__[j]; + } + } + if (iterc == 0) goto TRL20; + if (iterc <= itersw) goto TRL50; + goto TRL120; +} + +template +static int update_(int n, int npt, TYPE *bmat, TYPE *zmat, int *idz, int *ndim, TYPE *vlag, + TYPE *beta, int *knew, TYPE *w) +{ + /* The arrays BMAT and ZMAT with IDZ are updated, in order to shift + * the interpolation point that has index KNEW. On entry, VLAG + * contains the components of the vector Theta*Wcheck+e_b of the + * updating formula (6.11), and BETA holds the value of the + * parameter that has this name. The vector W is used for working + * space. */ + + int bmat_dim1, bmat_offset, zmat_dim1, zmat_offset, i__1, i__2, i__, + j, ja, jb, jl, jp, nptm, iflag; + TYPE d__1, d__2, tau, temp, scala, scalb, alpha, denom, tempa, tempb, tausq; + + /* Parameter adjustments */ + tempb = 0.0; + zmat_dim1 = npt; + zmat_offset = 1 + zmat_dim1; + zmat -= zmat_offset; + bmat_dim1 = *ndim; + bmat_offset = 1 + bmat_dim1; + bmat -= bmat_offset; + --vlag; + --w; + /* Function Body */ + nptm = npt - n - 1; + /* Apply the rotations that put zeros in the KNEW-th row of ZMAT. */ + jl = 1; + i__1 = nptm; + for (j = 2; j <= i__1; ++j) { + if (j == *idz) { + jl = *idz; + } else if (zmat[*knew + j * zmat_dim1] != 0) { + /* Computing 2nd power */ + d__1 = zmat[*knew + jl * zmat_dim1]; + /* Computing 2nd power */ + d__2 = zmat[*knew + j * zmat_dim1]; + temp = sqrt(d__1 * d__1 + d__2 * d__2); + tempa = zmat[*knew + jl * zmat_dim1] / temp; + tempb = zmat[*knew + j * zmat_dim1] / temp; + i__2 = npt; + for (i__ = 1; i__ <= i__2; ++i__) { + temp = tempa * zmat[i__ + jl * zmat_dim1] + tempb * zmat[i__ + + j * zmat_dim1]; + zmat[i__ + j * zmat_dim1] = tempa * zmat[i__ + j * zmat_dim1] + - tempb * zmat[i__ + jl * zmat_dim1]; + zmat[i__ + jl * zmat_dim1] = temp; + } + zmat[*knew + j * zmat_dim1] = 0; + } + } + /* Put the first NPT components of the KNEW-th column of HLAG into + * W, and calculate the parameters of the updating formula. */ + tempa = zmat[*knew + zmat_dim1]; + if (*idz >= 2) tempa = -tempa; + if (jl > 1) tempb = zmat[*knew + jl * zmat_dim1]; + i__1 = npt; + for (i__ = 1; i__ <= i__1; ++i__) { + w[i__] = tempa * zmat[i__ + zmat_dim1]; + if (jl > 1) w[i__] += tempb * zmat[i__ + jl * zmat_dim1]; + } + alpha = w[*knew]; + tau = vlag[*knew]; + tausq = tau * tau; + denom = alpha * *beta + tausq; + vlag[*knew] -= 1.0; + /* Complete the updating of ZMAT when there is only 1.0 nonzero + * element in the KNEW-th row of the new matrix ZMAT, but, if IFLAG + * is set to 1.0, then the first column of ZMAT will be exchanged + * with another 1.0 later. */ + iflag = 0; + if (jl == 1) { + temp = sqrt((fabs(denom))); + tempb = tempa / temp; + tempa = tau / temp; + i__1 = npt; + for (i__ = 1; i__ <= i__1; ++i__) + zmat[i__ + zmat_dim1] = tempa * zmat[i__ + zmat_dim1] - tempb * + vlag[i__]; + if (*idz == 1 && temp < 0) *idz = 2; + if (*idz >= 2 && temp >= 0) iflag = 1; + } else { + /* Complete the updating of ZMAT in the alternative case. */ + ja = 1; + if (*beta >= 0) { + ja = jl; + } + jb = jl + 1 - ja; + temp = zmat[*knew + jb * zmat_dim1] / denom; + tempa = temp * *beta; + tempb = temp * tau; + temp = zmat[*knew + ja * zmat_dim1]; + scala = 1.0 / sqrt(fabs(*beta) * temp * temp + tausq); + scalb = scala * sqrt((fabs(denom))); + i__1 = npt; + for (i__ = 1; i__ <= i__1; ++i__) { + zmat[i__ + ja * zmat_dim1] = scala * (tau * zmat[i__ + ja * + zmat_dim1] - temp * vlag[i__]); + zmat[i__ + jb * zmat_dim1] = scalb * (zmat[i__ + jb * zmat_dim1] + - tempa * w[i__] - tempb * vlag[i__]); + } + if (denom <= 0) { + if (*beta < 0) ++(*idz); + if (*beta >= 0) iflag = 1; + } + } + /* IDZ is reduced in the following case, and usually the first + * column of ZMAT is exchanged with a later 1.0. */ + if (iflag == 1) { + --(*idz); + i__1 = npt; + for (i__ = 1; i__ <= i__1; ++i__) { + temp = zmat[i__ + zmat_dim1]; + zmat[i__ + zmat_dim1] = zmat[i__ + *idz * zmat_dim1]; + zmat[i__ + *idz * zmat_dim1] = temp; + } + } + /* Finally, update the matrix BMAT. */ + i__1 = n; + for (j = 1; j <= i__1; ++j) { + jp = npt + j; + w[jp] = bmat[*knew + j * bmat_dim1]; + tempa = (alpha * vlag[jp] - tau * w[jp]) / denom; + tempb = (-(*beta) * w[jp] - tau * vlag[jp]) / denom; + i__2 = jp; + for (i__ = 1; i__ <= i__2; ++i__) { + bmat[i__ + j * bmat_dim1] = bmat[i__ + j * bmat_dim1] + tempa * + vlag[i__] + tempb * w[i__]; + if (i__ > npt) { + bmat[jp + (i__ - npt) * bmat_dim1] = bmat[i__ + j * + bmat_dim1]; + } + } + } + return 0; +} + +template +static TYPE newuob_(int n, int npt, TYPE *x, + TYPE rhobeg, TYPE rhoend, int *ret_nf, int maxfun, + TYPE *xbase, TYPE *xopt, TYPE *xnew, + TYPE *xpt, TYPE *fval, TYPE *gq, TYPE *hq, + TYPE *pq, TYPE *bmat, TYPE *zmat, int *ndim, + TYPE *d__, TYPE *vlag, TYPE *w, Func &func) +{ + /* XBASE will hold a shift of origin that should reduce the + contributions from rounding errors to values of the model and + Lagrange functions. + * XOPT will be set to the displacement from XBASE of the vector of + variables that provides the least calculated F so far. + * XNEW will be set to the displacement from XBASE of the vector of + variables for the current calculation of F. + * XPT will contain the interpolation point coordinates relative to + XBASE. + * FVAL will hold the values of F at the interpolation points. + * GQ will hold the gradient of the quadratic model at XBASE. + * HQ will hold the explicit second derivatives of the quadratic + model. + * PQ will contain the parameters of the implicit second derivatives + of the quadratic model. + * BMAT will hold the last N columns of H. + * ZMAT will hold the factorization of the leading NPT by NPT + submatrix of H, this factorization being ZMAT times Diag(DZ) + times ZMAT^T, where the elements of DZ are plus or minus 1.0, as + specified by IDZ. + * NDIM is the first dimension of BMAT and has the value NPT+N. + * D is reserved for trial steps from XOPT. + * VLAG will contain the values of the Lagrange functions at a new + point X. They are part of a product that requires VLAG to be of + length NDIM. + * The array W will be used for working space. Its length must be at + least 10*NDIM = 10*(NPT+N). Set some constants. */ + + int xpt_dim1, xpt_offset, bmat_dim1, bmat_offset, zmat_dim1, zmat_offset, + i__1, i__2, i__3, i__, j, k, ih, nf, nh, ip, jp, np, nfm, idz, ipt, jpt, + nfmm, knew, kopt, nptm, ksave, nfsav, itemp, ktemp, itest, nftest; + TYPE d__1, d__2, d__3, f, dx, dsq, rho, sum, fbeg, diff, beta, gisq, + temp, suma, sumb, fopt, bsum, gqsq, xipt, xjpt, sumz, diffa, diffb, + diffc, hdiag, alpha, delta, recip, reciq, fsave, dnorm, ratio, dstep, + vquad, tempq, rhosq, detrat, crvmin, distsq, xoptsq; + + /* Parameter adjustments */ + diffc = ratio = dnorm = nfsav = diffa = diffb = xoptsq = f = 0.0; + rho = fbeg = fopt = xjpt = xipt = 0.0; + itest = ipt = jpt = 0; + alpha = dstep = 0.0; + zmat_dim1 = npt; + zmat_offset = 1 + zmat_dim1; + zmat -= zmat_offset; + xpt_dim1 = npt; + xpt_offset = 1 + xpt_dim1; + xpt -= xpt_offset; + --x; --xbase; --xopt; --xnew; --fval; --gq; --hq; --pq; + bmat_dim1 = *ndim; + bmat_offset = 1 + bmat_dim1; + bmat -= bmat_offset; + --d__; + --vlag; + --w; + /* Function Body */ + np = n + 1; + nh = n * np / 2; + nptm = npt - np; + nftest = (maxfun > 1)? maxfun : 1; + /* Set the initial elements of XPT, BMAT, HQ, PQ and ZMAT to 0. */ + i__1 = n; + for (j = 1; j <= i__1; ++j) { + xbase[j] = x[j]; + i__2 = npt; + for (k = 1; k <= i__2; ++k) + xpt[k + j * xpt_dim1] = 0; + i__2 = *ndim; + for (i__ = 1; i__ <= i__2; ++i__) + bmat[i__ + j * bmat_dim1] = 0; + } + i__2 = nh; + for (ih = 1; ih <= i__2; ++ih) hq[ih] = 0; + i__2 = npt; + for (k = 1; k <= i__2; ++k) { + pq[k] = 0; + i__1 = nptm; + for (j = 1; j <= i__1; ++j) + zmat[k + j * zmat_dim1] = 0; + } + /* Begin the initialization procedure. NF becomes 1.0 more than the + * number of function values so far. The coordinates of the + * displacement of the next initial interpolation point from XBASE + * are set in XPT(NF,.). */ + rhosq = rhobeg * rhobeg; + recip = 1.0 / rhosq; + reciq = sqrt(.5) / rhosq; + nf = 0; +L50: + nfm = nf; + nfmm = nf - n; + ++nf; + if (nfm <= n << 1) { + if (nfm >= 1 && nfm <= n) { + xpt[nf + nfm * xpt_dim1] = rhobeg; + } else if (nfm > n) { + xpt[nf + nfmm * xpt_dim1] = -(rhobeg); + } + } else { + itemp = (nfmm - 1) / n; + jpt = nfm - itemp * n - n; + ipt = jpt + itemp; + if (ipt > n) { + itemp = jpt; + jpt = ipt - n; + ipt = itemp; + } + xipt = rhobeg; + if (fval[ipt + np] < fval[ipt + 1]) xipt = -xipt; + xjpt = rhobeg; + if (fval[jpt + np] < fval[jpt + 1]) xjpt = -xjpt; + xpt[nf + ipt * xpt_dim1] = xipt; + xpt[nf + jpt * xpt_dim1] = xjpt; + } + /* Calculate the next value of F, label 70 being reached immediately + * after this calculation. The least function value so far and its + * index are required. */ + i__1 = n; + for (j = 1; j <= i__1; ++j) + x[j] = xpt[nf + j * xpt_dim1] + xbase[j]; + goto L310; +L70: + fval[nf] = f; + if (nf == 1) { + fbeg = fopt = f; + kopt = 1; + } else if (f < fopt) { + fopt = f; + kopt = nf; + } + /* Set the non0 initial elements of BMAT and the quadratic model + * in the cases when NF is at most 2*N+1. */ + if (nfm <= n << 1) { + if (nfm >= 1 && nfm <= n) { + gq[nfm] = (f - fbeg) / rhobeg; + if (npt < nf + n) { + bmat[nfm * bmat_dim1 + 1] = -1.0 / rhobeg; + bmat[nf + nfm * bmat_dim1] = 1.0 / rhobeg; + bmat[npt + nfm + nfm * bmat_dim1] = -.5 * rhosq; + } + } else if (nfm > n) { + bmat[nf - n + nfmm * bmat_dim1] = .5 / rhobeg; + bmat[nf + nfmm * bmat_dim1] = -.5 / rhobeg; + zmat[nfmm * zmat_dim1 + 1] = -reciq - reciq; + zmat[nf - n + nfmm * zmat_dim1] = reciq; + zmat[nf + nfmm * zmat_dim1] = reciq; + ih = nfmm * (nfmm + 1) / 2; + temp = (fbeg - f) / rhobeg; + hq[ih] = (gq[nfmm] - temp) / rhobeg; + gq[nfmm] = .5 * (gq[nfmm] + temp); + } + /* Set the off-diagonal second derivatives of the Lagrange + * functions and the initial quadratic model. */ + } else { + ih = ipt * (ipt - 1) / 2 + jpt; + if (xipt < 0) ipt += n; + if (xjpt < 0) jpt += n; + zmat[nfmm * zmat_dim1 + 1] = recip; + zmat[nf + nfmm * zmat_dim1] = recip; + zmat[ipt + 1 + nfmm * zmat_dim1] = -recip; + zmat[jpt + 1 + nfmm * zmat_dim1] = -recip; + hq[ih] = (fbeg - fval[ipt + 1] - fval[jpt + 1] + f) / (xipt * xjpt); + } + if (nf < npt) goto L50; + /* Begin the iterative procedure, because the initial model is + * complete. */ + rho = rhobeg; + delta = rho; + idz = 1; + diffa = diffb = itest = xoptsq = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + xopt[i__] = xpt[kopt + i__ * xpt_dim1]; + /* Computing 2nd power */ + d__1 = xopt[i__]; + xoptsq += d__1 * d__1; + } +L90: + nfsav = nf; + /* Generate the next trust region step and test its length. Set KNEW + * to -1 if the purpose of the next F will be to improve the + * model. */ +L100: + knew = 0; + trsapp_(n, npt, &xopt[1], &xpt[xpt_offset], &gq[1], &hq[1], &pq[1], & + delta, &d__[1], &w[1], &w[np], &w[np + n], &w[np + (n << 1)], & + crvmin); + dsq = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + /* Computing 2nd power */ + d__1 = d__[i__]; + dsq += d__1 * d__1; + } + /* Computing MIN */ + d__1 = delta, d__2 = sqrt(dsq); + dnorm = std::min(d__1, d__2); + if (dnorm < .5 * rho) { + knew = -1; + delta = DELTA_DECREASE * delta; + ratio = -1.; + if (delta <= rho * 1.5) delta = rho; + if (nf <= nfsav + 2) goto L460; + temp = crvmin * .125 * rho * rho; + /* Computing MAX */ + d__1 = std::max(diffa, diffb); + if (temp <= std::max(d__1, diffc)) goto L460; + goto L490; + } + /* Shift XBASE if XOPT may be too far from XBASE. First make the + * changes to BMAT that do not depend on ZMAT. */ +L120: + if (dsq <= xoptsq * .001) { + tempq = xoptsq * .25; + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + sum = 0; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) + sum += xpt[k + i__ * xpt_dim1] * xopt[i__]; + temp = pq[k] * sum; + sum -= .5 * xoptsq; + w[npt + k] = sum; + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) { + gq[i__] += temp * xpt[k + i__ * xpt_dim1]; + xpt[k + i__ * xpt_dim1] -= .5 * xopt[i__]; + vlag[i__] = bmat[k + i__ * bmat_dim1]; + w[i__] = sum * xpt[k + i__ * xpt_dim1] + tempq * xopt[i__]; + ip = npt + i__; + i__3 = i__; + for (j = 1; j <= i__3; ++j) + bmat[ip + j * bmat_dim1] = bmat[ip + j * bmat_dim1] + + vlag[i__] * w[j] + w[i__] * vlag[j]; + } + } + /* Then the revisions of BMAT that depend on ZMAT are + * calculated. */ + i__3 = nptm; + for (k = 1; k <= i__3; ++k) { + sumz = 0; + i__2 = npt; + for (i__ = 1; i__ <= i__2; ++i__) { + sumz += zmat[i__ + k * zmat_dim1]; + w[i__] = w[npt + i__] * zmat[i__ + k * zmat_dim1]; + } + i__2 = n; + for (j = 1; j <= i__2; ++j) { + sum = tempq * sumz * xopt[j]; + i__1 = npt; + for (i__ = 1; i__ <= i__1; ++i__) + sum += w[i__] * xpt[i__ + j * xpt_dim1]; + vlag[j] = sum; + if (k < idz) sum = -sum; + i__1 = npt; + for (i__ = 1; i__ <= i__1; ++i__) + bmat[i__ + j * bmat_dim1] += sum * zmat[i__ + k * zmat_dim1]; + } + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + ip = i__ + npt; + temp = vlag[i__]; + if (k < idz) temp = -temp; + i__2 = i__; + for (j = 1; j <= i__2; ++j) + bmat[ip + j * bmat_dim1] += temp * vlag[j]; + } + } + /* The following instructions complete the shift of XBASE, + * including the changes to the parameters of the quadratic + * model. */ + ih = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) { + w[j] = 0; + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + w[j] += pq[k] * xpt[k + j * xpt_dim1]; + xpt[k + j * xpt_dim1] -= .5 * xopt[j]; + } + i__1 = j; + for (i__ = 1; i__ <= i__1; ++i__) { + ++ih; + if (i__ < j) gq[j] += hq[ih] * xopt[i__]; + gq[i__] += hq[ih] * xopt[j]; + hq[ih] = hq[ih] + w[i__] * xopt[j] + xopt[i__] * w[j]; + bmat[npt + i__ + j * bmat_dim1] = bmat[npt + j + i__ * + bmat_dim1]; + } + } + i__1 = n; + for (j = 1; j <= i__1; ++j) { + xbase[j] += xopt[j]; + xopt[j] = 0; + } + xoptsq = 0; + } + /* Pick the model step if KNEW is positive. A different choice of D + * may be made later, if the choice of D by BIGLAG causes + * substantial cancellation in DENOM. */ + if (knew > 0) { + biglag_(n, npt, &xopt[1], &xpt[xpt_offset], &bmat[bmat_offset], &zmat[zmat_offset], &idz, + ndim, &knew, &dstep, &d__[1], &alpha, &vlag[1], &vlag[npt + 1], &w[1], &w[np], &w[np + n], func); + } + /* Calculate VLAG and BETA for the current choice of D. The first + * NPT components of W_check will be held in W. */ + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + suma = 0; + sumb = 0; + sum = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) { + suma += xpt[k + j * xpt_dim1] * d__[j]; + sumb += xpt[k + j * xpt_dim1] * xopt[j]; + sum += bmat[k + j * bmat_dim1] * d__[j]; + } + w[k] = suma * (.5 * suma + sumb); + vlag[k] = sum; + } + beta = 0; + i__1 = nptm; + for (k = 1; k <= i__1; ++k) { + sum = 0; + i__2 = npt; + for (i__ = 1; i__ <= i__2; ++i__) + sum += zmat[i__ + k * zmat_dim1] * w[i__]; + if (k < idz) { + beta += sum * sum; + sum = -sum; + } else beta -= sum * sum; + i__2 = npt; + for (i__ = 1; i__ <= i__2; ++i__) + vlag[i__] += sum * zmat[i__ + k * zmat_dim1]; + } + bsum = 0; + dx = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) { + sum = 0; + i__1 = npt; + for (i__ = 1; i__ <= i__1; ++i__) + sum += w[i__] * bmat[i__ + j * bmat_dim1]; + bsum += sum * d__[j]; + jp = npt + j; + i__1 = n; + for (k = 1; k <= i__1; ++k) + sum += bmat[jp + k * bmat_dim1] * d__[k]; + vlag[jp] = sum; + bsum += sum * d__[j]; + dx += d__[j] * xopt[j]; + } + beta = dx * dx + dsq * (xoptsq + dx + dx + .5 * dsq) + beta - bsum; + vlag[kopt] += 1.0; + /* If KNEW is positive and if the cancellation in DENOM is + * unacceptable, then BIGDEN calculates an alternative model step, + * XNEW being used for working space. */ + if (knew > 0) { + /* Computing 2nd power */ + d__1 = vlag[knew]; + temp = 1.0 + alpha * beta / (d__1 * d__1); + if (fabs(temp) <= .8) { + bigden_(n, npt, &xopt[1], &xpt[xpt_offset], &bmat[bmat_offset], & + zmat[zmat_offset], &idz, ndim, &kopt, &knew, &d__[1], &w[ + 1], &vlag[1], &beta, &xnew[1], &w[*ndim + 1], &w[*ndim * + 6 + 1]); + } + } + /* Calculate the next value of the objective function. */ +L290: + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) { + xnew[i__] = xopt[i__] + d__[i__]; + x[i__] = xbase[i__] + xnew[i__]; + } + ++nf; +L310: + if (nf > nftest) { + --nf; + //fprintf(stderr, "++ Return from NEWUOA because CALFUN has been called MAXFUN times.\n"); + goto L530; + } + f = func(n, &x[1]); + if (nf <= npt) goto L70; + if (knew == -1) goto L530; + /* Use the quadratic model to predict the change in F due to the + * step D, and set DIFF to the error of this prediction. */ + vquad = ih = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) { + vquad += d__[j] * gq[j]; + i__1 = j; + for (i__ = 1; i__ <= i__1; ++i__) { + ++ih; + temp = d__[i__] * xnew[j] + d__[j] * xopt[i__]; + if (i__ == j) temp = .5 * temp; + vquad += temp * hq[ih]; + } + } + i__1 = npt; + for (k = 1; k <= i__1; ++k) vquad += pq[k] * w[k]; + diff = f - fopt - vquad; + diffc = diffb; + diffb = diffa; + diffa = fabs(diff); + if (dnorm > rho) nfsav = nf; + /* Update FOPT and XOPT if the new F is the least value of the + * objective function so far. The branch when KNEW is positive + * occurs if D is not a trust region step. */ + fsave = fopt; + if (f < fopt) { + fopt = f; + xoptsq = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + xopt[i__] = xnew[i__]; + /* Computing 2nd power */ + d__1 = xopt[i__]; + xoptsq += d__1 * d__1; + } + } + ksave = knew; + if (knew > 0) goto L410; + /* Pick the next value of DELTA after a trust region step. */ + if (vquad >= 0) { + fprintf(stderr, "++ Return from NEWUOA because a trust region step has failed to reduce Q.\n"); + goto L530; + } + ratio = (f - fsave) / vquad; + if (ratio <= 0.1) { + delta = .5 * dnorm; + } else if (ratio <= .7) { + /* Computing MAX */ + d__1 = .5 * delta; + delta = std::max(d__1, dnorm); + } else { + /* Computing MAX */ + d__1 = .5 * delta, d__2 = dnorm + dnorm; + delta = std::max(d__1, d__2); + } + if (delta <= rho * 1.5) delta = rho; + /* Set KNEW to the index of the next interpolation point to be + * deleted. */ + /* Computing MAX */ + d__2 = 0.1 * delta; + /* Computing 2nd power */ + d__1 = std::max(d__2, rho); + rhosq = d__1 * d__1; + ktemp = detrat = 0; + if (f >= fsave) { + ktemp = kopt; + detrat = 1.0; + } + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + hdiag = 0; + i__2 = nptm; + for (j = 1; j <= i__2; ++j) { + temp = 1.0; + if (j < idz) temp = -1.0; + /* Computing 2nd power */ + d__1 = zmat[k + j * zmat_dim1]; + hdiag += temp * (d__1 * d__1); + } + /* Computing 2nd power */ + d__2 = vlag[k]; + temp = (d__1 = beta * hdiag + d__2 * d__2, fabs(d__1)); + distsq = 0; + i__2 = n; + for (j = 1; j <= i__2; ++j) { + /* Computing 2nd power */ + d__1 = xpt[k + j * xpt_dim1] - xopt[j]; + distsq += d__1 * d__1; + } + if (distsq > rhosq) { + /* Computing 3rd power */ + d__1 = distsq / rhosq; + temp *= d__1 * (d__1 * d__1); + } + if (temp > detrat && k != ktemp) { + detrat = temp; + knew = k; + } + } + if (knew == 0) goto L460; + /* Update BMAT, ZMAT and IDZ, so that the KNEW-th interpolation + * point can be moved. Begin the updating of the quadratic model, + * starting with the explicit second derivative term. */ +L410: + update_(n, npt, &bmat[bmat_offset], &zmat[zmat_offset], &idz, ndim, &vlag[1], &beta, &knew, &w[1]); + fval[knew] = f; + ih = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + temp = pq[knew] * xpt[knew + i__ * xpt_dim1]; + i__2 = i__; + for (j = 1; j <= i__2; ++j) { + ++ih; + hq[ih] += temp * xpt[knew + j * xpt_dim1]; + } + } + pq[knew] = 0; + /* Update the other second derivative parameters, and then the + * gradient vector of the model. Also include the new interpolation + * point. */ + i__2 = nptm; + for (j = 1; j <= i__2; ++j) { + temp = diff * zmat[knew + j * zmat_dim1]; + if (j < idz) temp = -temp; + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + pq[k] += temp * zmat[k + j * zmat_dim1]; + } + } + gqsq = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + gq[i__] += diff * bmat[knew + i__ * bmat_dim1]; + /* Computing 2nd power */ + d__1 = gq[i__]; + gqsq += d__1 * d__1; + xpt[knew + i__ * xpt_dim1] = xnew[i__]; + } + /* If a trust region step makes a small change to the objective + * function, then calculate the gradient of the least Frobenius norm + * interpolant at XBASE, and store it in W, using VLAG for a vector + * of right hand sides. */ + if (ksave == 0 && delta == rho) { + if (fabs(ratio) > .01) { + itest = 0; + } else { + i__1 = npt; + for (k = 1; k <= i__1; ++k) + vlag[k] = fval[k] - fval[kopt]; + gisq = 0; + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) { + sum = 0; + i__2 = npt; + for (k = 1; k <= i__2; ++k) + sum += bmat[k + i__ * bmat_dim1] * vlag[k]; + gisq += sum * sum; + w[i__] = sum; + } + /* Test whether to replace the new quadratic model by the + * least Frobenius norm interpolant, making the replacement + * if the test is satisfied. */ + ++itest; + if (gqsq < gisq * 100.) itest = 0; + if (itest >= 3) { + i__1 = n; + for (i__ = 1; i__ <= i__1; ++i__) gq[i__] = w[i__]; + i__1 = nh; + for (ih = 1; ih <= i__1; ++ih) hq[ih] = 0; + i__1 = nptm; + for (j = 1; j <= i__1; ++j) { + w[j] = 0; + i__2 = npt; + for (k = 1; k <= i__2; ++k) + w[j] += vlag[k] * zmat[k + j * zmat_dim1]; + if (j < idz) w[j] = -w[j]; + } + i__1 = npt; + for (k = 1; k <= i__1; ++k) { + pq[k] = 0; + i__2 = nptm; + for (j = 1; j <= i__2; ++j) + pq[k] += zmat[k + j * zmat_dim1] * w[j]; + } + itest = 0; + } + } + } + if (f < fsave) kopt = knew; + /* If a trust region step has provided a sufficient decrease in F, + * then branch for another trust region calculation. The case + * KSAVE>0 occurs when the new function value was calculated by a + * model step. */ + if (f <= fsave + 0.1 * vquad) goto L100; + if (ksave > 0) goto L100; + /* Alternatively, find out if the interpolation points are close + * enough to the best point so far. */ + knew = 0; +L460: + distsq = delta * 4. * delta; + i__2 = npt; + for (k = 1; k <= i__2; ++k) { + sum = 0; + i__1 = n; + for (j = 1; j <= i__1; ++j) { + /* Computing 2nd power */ + d__1 = xpt[k + j * xpt_dim1] - xopt[j]; + sum += d__1 * d__1; + } + if (sum > distsq) { + knew = k; + distsq = sum; + } + } + /* If KNEW is positive, then set DSTEP, and branch back for the next + * iteration, which will generate a "model step". */ + if (knew > 0) { + /* Computing MAX and MIN*/ + d__2 = 0.1 * sqrt(distsq), d__3 = .5 * delta; + d__1 = std::min(d__2, d__3); + dstep = std::max(d__1, rho); + dsq = dstep * dstep; + goto L120; + } + if (ratio > 0) goto L100; + if (std::max(delta, dnorm) > rho) goto L100; + /* The calculations with the current value of RHO are complete. Pick + * the next values of RHO and DELTA. */ +L490: + if (rho > rhoend) { + delta = .5 * rho; + ratio = rho / rhoend; + if (ratio <= 16.) rho = rhoend; + else if (ratio <= 250.) rho = sqrt(ratio) * rhoend; + else rho = RHO_DECREASE * rho; + delta = std::max(delta, rho); + goto L90; + } + /* Return from the calculation, after another Newton-Raphson step, + * if it is too short to have been tried before. */ + if (knew == -1) goto L290; +L530: + if (fopt <= f) { + i__2 = n; + for (i__ = 1; i__ <= i__2; ++i__) + x[i__] = xbase[i__] + xopt[i__]; + f = fopt; + } + *ret_nf = nf; + return f; +} + +template +static TYPE newuoa_(int n, int npt, TYPE *x, TYPE rhobeg, TYPE rhoend, int *ret_nf, int maxfun, TYPE *w, Func &func) +{ + /* This subroutine seeks the least value of a function of many + * variables, by a trust region method that forms quadratic models + * by interpolation. There can be some freedom in the interpolation + * conditions, which is taken up by minimizing the Frobenius norm of + * the change to the second derivative of the quadratic model, + * beginning with a zero matrix. The arguments of the subroutine are + * as follows. */ + + /* N must be set to the number of variables and must be at least + * two. NPT is the number of interpolation conditions. Its value + * must be in the interval [N+2,(N+1)(N+2)/2]. Initial values of the + * variables must be set in X(1),X(2),...,X(N). They will be changed + * to the values that give the least calculated F. RHOBEG and RHOEND + * must be set to the initial and final values of a trust region + * radius, so both must be positive with RHOEND<=RHOBEG. Typically + * RHOBEG should be about one tenth of the greatest expected change + * to a variable, and RHOEND should indicate the accuracy that is + * required in the final values of the variables. MAXFUN must be set + * to an upper bound on the number of calls of CALFUN. The array W + * will be used for working space. Its length must be at least + * (NPT+13)*(NPT+N)+3*N*(N+3)/2. */ + + /* SUBROUTINE CALFUN (N,X,F) must be provided by the user. It must + * set F to the value of the objective function for the variables + * X(1),X(2),...,X(N). Partition the working space array, so that + * different parts of it can be treated separately by the subroutine + * that performs the main calculation. */ + + int id, np, iw, igq, ihq, ixb, ifv, ipq, ivl, ixn, + ixo, ixp, ndim, nptm, ibmat, izmat; + + /* Parameter adjustments */ + --w; --x; + /* Function Body */ + np = n + 1; + nptm = npt - np; + if (npt < n + 2 || npt > (n + 2) * np / 2) { + fprintf(stderr, "** Return from NEWUOA because NPT is not in the required interval.\n"); + return 1; + } + ndim = npt + n; + ixb = 1; + ixo = ixb + n; + ixn = ixo + n; + ixp = ixn + n; + ifv = ixp + n * npt; + igq = ifv + npt; + ihq = igq + n; + ipq = ihq + n * np / 2; + ibmat = ipq + npt; + izmat = ibmat + ndim * n; + id = izmat + npt * nptm; + ivl = id + n; + iw = ivl + ndim; + /* The above settings provide a partition of W for subroutine + * NEWUOB. The partition requires the first NPT*(NPT+N)+5*N*(N+3)/2 + * elements of W plus the space that is needed by the last array of + * NEWUOB. */ + return newuob_(n, npt, &x[1], rhobeg, rhoend, ret_nf, maxfun, &w[ixb], &w[ixo], &w[ixn], + &w[ixp], &w[ifv], &w[igq], &w[ihq], &w[ipq], &w[ibmat], &w[izmat], + &ndim, &w[id], &w[ivl], &w[iw], func); +} + +template +TYPE min_newuoa(int n, TYPE *x, Func &func, TYPE rb, TYPE tol, int max_iter) +{ + int npt = 2 * n + 1, rnf; + TYPE ret; + TYPE *w = (TYPE*)calloc((npt+13)*(npt+n) + 3*n*(n+3)/2 + 11, sizeof(TYPE)); + ret = newuoa_(n, 2*n+1, x, rb, tol, &rnf, max_iter, w, func); + free(w); + return ret; +} + +#endif From ec730298faad058ab513f03abeebb4180bb893a5 Mon Sep 17 00:00:00 2001 From: alemuntoni Date: Fri, 29 May 2020 11:55:51 +0200 Subject: [PATCH 02/13] TMP: first move align_pair --- vcg/complex/algorithms/align_pair.h | 515 ++++++++++++++++++++++++++++ 1 file changed, 515 insertions(+) create mode 100644 vcg/complex/algorithms/align_pair.h diff --git a/vcg/complex/algorithms/align_pair.h b/vcg/complex/algorithms/align_pair.h new file mode 100644 index 00000000..8fbc0d4b --- /dev/null +++ b/vcg/complex/algorithms/align_pair.h @@ -0,0 +1,515 @@ +/**************************************************************************** +* VCGLib o o * +* Visual and Computer Graphics Library o o * +* _ O _ * +* Copyright(C) 2004-2016 \/)\/ * +* Visual Computing Lab /\/| * +* ISTI - Italian National Research Council | * +* \ * +* All rights reserved. * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) * +* for more details. * +* * +****************************************************************************/ +#ifndef VCG_ALIGN_PAIR_H +#define VCG_ALIGN_PAIR_H + +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace vcg { +/************************************************************************* + AlignPair + +Classe per gestire un allineamento tra DUE sole mesh. + +**************************************************************************/ + +class AlignPair { +public: + + enum ErrorCode { + SUCCESS, + NO_COMMON_BBOX, + TOO_FEW_POINTS, + LSQ_DIVERGE, + TOO_MUCH_SHEAR, + TOO_MUCH_SCALE, + FORBIDDEN, + INVALID, + UNKNOWN_MODE }; + + + /*********************** Classi Accessorie ****************************/ + + class A2Vertex; + + class A2Face; + + class A2UsedTypes: + public vcg::UsedTypes < vcg::Use::AsVertexType, + vcg::Use::AsFaceType >{}; + + class A2Vertex : public vcg::Vertex {}; + class A2Face : public vcg::Face< A2UsedTypes,vcg::face::VertexRef, vcg::face::Normal3d,vcg::face::Mark,vcg::face::BitFlags> {}; + + class A2Mesh : public vcg::tri::TriMesh< std::vector, std::vector > + { + public: + //bool Import(const char *filename) { Matrix44d Tr; Tr.SetIdentity(); return Import(filename,Tr);} + //bool Import(const char *filename, Matrix44d &Tr); + + inline bool InitVert(const Matrix44d &Tr) { + Matrix44d Idn; Idn.SetIdentity(); + if (Tr != Idn) + tri::UpdatePosition::Matrix(*this, Tr); + tri::UpdateNormal::NormalizePerVertex(*this); + tri::UpdateBounding::Box(*this); + return true; + } + inline bool Init(const Matrix44d &Tr) { + Matrix44d Idn; Idn.SetIdentity(); + tri::Clean::RemoveUnreferencedVertex(*this); + if (Tr != Idn) + tri::UpdatePosition::Matrix(*this, Tr); + tri::UpdateNormal::PerVertexNormalizedPerFaceNormalized(*this); + tri::UpdateFlags::FaceBorderFromNone(*this); + tri::UpdateBounding::Box(*this); + + return true; + } + }; + + typedef A2Mesh::FaceContainer FaceContainer; + typedef A2Mesh::FaceType FaceType; + typedef GridStaticPtr A2Grid; + typedef GridStaticPtr A2GridVert; + + class Stat + { + public: + + class IterInfo + { + public: + IterInfo() + { + memset ( (void *) this, 0, sizeof(IterInfo)); + } + + double MinDistAbs; + int DistanceDiscarded; + int AngleDiscarded; + int BorderDiscarded; + int SampleTested; // quanti punti ho testato con la mindist + int SampleUsed; // quanti punti ho scelto per la computematrix + double pcl50; + double pclhi; + double AVG; + double RMS; + double StdDev; + int Time; // quando e' finita questa iterazione + + }; + + std::vector I; + + double LastPcl50() const + { + return I.back().pcl50; + } + + int LastSampleUsed() const { + return I.back().SampleUsed; + } + + int MovVertNum; + int FixVertNum; + int FixFaceNum; + + int TotTime() { + return I.back().Time-StartTime; + } + + int IterTime(unsigned int i) const + { + const int clock_per_ms = std::max(CLOCKS_PER_SEC / 1000,1); + assert(i\n"); + fprintf(fp, " Mindist 50ile Hi Avg RMS StdDev Time Tested Used Dist Bord Angl \n"); + for (unsigned int qi = 0; qi < I.size(); ++qi) + fprintf( + fp, " %8.5f %9.6f %8.5f %5.3f %8.5f %9.6f %4ims %5i %5i %4i %4i %4i \n", + I[qi].MinDistAbs, + I[qi].pcl50, I[qi].pclhi, + I[qi].AVG, I[qi].RMS, I[qi].StdDev, + IterTime(qi), + I[qi].SampleTested, I[qi].SampleUsed, I[qi].DistanceDiscarded, I[qi].BorderDiscarded, I[qi].AngleDiscarded); + fprintf(fp, "\n"); + } + + // Restituisce true se nelle ultime iterazioni non e' diminuito + // l'errore + inline bool Stable(int lastiter) + { + if (I.empty()) + return false; + int parag = int(I.size()) - lastiter; + + if (parag < 0) + parag = 0; + if (I.back().pcl50 < I[parag].pcl50) + return false; // se siamo diminuiti non e' stabile + + return true; + } + + }; + + + class Param + { + public: + enum MatchModeEnum {MMSimilarity, MMRigid}; + enum SampleModeEnum {SMRandom, SMNormalEqualized}; + + Param() + { + SampleNum = 2000; + MaxPointNum = 100000; + MinPointNum = 30; + + MaxIterNum = 75; + TrgDistAbs = 0.005f; + + MinDistAbs = 10; + MaxAngleRad = math::ToRad(45.0); + MaxShear = 0.5; + MaxScale = 0.5; // significa che lo scale deve essere compreso tra 1-MaxScale e 1+MaxScale + PassHiFilter = 0.75; + ReduceFactorPerc = 0.80; + MinMinDistPerc = 0.01; + EndStepNum = 5; + MatchMode = MMRigid; + SampleMode = SMNormalEqualized; + UGExpansionFactor=10; + MinFixVertNum=20000; + MinFixVertNumPerc=.25; + UseVertexOnly = false; + } + + int SampleNum; // numero di sample da prendere sulla mesh fix, utilizzando + // il SampleMode scelto tra cui poi sceglierne al piu' + // e almeno da usare per l'allineamento. + int MaxPointNum; // numero di coppie di punti da usare quando si calcola la matrice + // di allienamento e che poi si mettono da parte per il globale; + // di solito non usato + int MinPointNum; // minimo numero di coppie di punti ammissibile perche' sia considerato + // valido l'allineamento + double MinDistAbs; // distanza minima iniziale perche due punti vengano presi in + // considerazione. NON e' piu espressa in percentuale sul bbox della mesh nella ug. + // Ad ogni passo puo essere ridotta per + // accellerare usando ReduceFactor + double MaxAngleRad; // massimo angolo in radianti tra due normali perche' i due + // punti vengano presi in considerazione. + + int MaxIterNum; // massimo numero di iterazioni da fare in align + double TrgDistAbs; // distanza obiettivo entro la quale devono stare almeno la meta' + // dei campioni scelti; di solito non entra in gioco perche' ha un default molto basso + + int EndStepNum; // numero di iterazioni da considerare per decidere se icp ha converso. + + //double PassLoFilter; // Filtraggio utilizzato per decidere quali punti scegliere tra quello trovati abbastanza + double PassHiFilter; // vicini. Espresso in percentili. Di solito si scarta il quelli sopra il 75 e quelli sotto il 5 + double ReduceFactorPerc; // At each step we discard the points farther than a given threshold. The threshold is iterativeley reduced; + // StartMinDist= min(StartMinDist, 5.0*H.Percentile(ap.ReduceFactorPerc)) + + + double MinMinDistPerc; // Ratio between initial starting distance (MinDistAbs) and what can reach by the application of the ReduceFactor. + + int UGExpansionFactor; // Grandezza della UG per la mesh fix come rapporto del numero di facce della mesh fix + + // Nel caso si usi qualche struttura multiresolution + int MinFixVertNum; // Gli allineamenti si fanno mettendo nella ug come mesh fix una semplificata; + float MinFixVertNumPerc; // si usa il max tra MinFixVertNum e OrigMeshSize*MinFixVertNumPerc + bool UseVertexOnly; // if true all the Alignment pipeline ignores faces and works over point clouds. + + double MaxShear; + double MaxScale; + MatchModeEnum MatchMode; + SampleModeEnum SampleMode; + void Dump(FILE *fp,double BoxDiag); + + }; + + // Classe per memorizzare il risultato di un allineamento tra due mesh + // i punti si intendono nel sistema di riferimento iniziale delle due mesh. + // + // se le mesh hanno una trasformazione di base in ingresso, + // questa appare solo durante la A2Mesh::Import e poi e' per sempre dimenticata. + // Questi punti sono quindi nei sistemi di riferimento costruiti durante la Import + // la matrice Tr quella che + // + // Tr*Pmov[i]== Pfix + + + class Result + { + public: + int MovName; + int FixName; + + Matrix44d Tr; + std::vector Pfix; // vertici corrispondenti su fix (rossi) + std::vector Nfix; // normali corrispondenti su fix (rossi) + std::vector Pmov; // vertici scelti su mov (verdi) prima della trasformazione in ingresso (Original Point Target) + std::vector Nmov; // normali scelti su mov (verdi) + Histogramf H; + Stat as; + Param ap; + ErrorCode status; + bool IsValid() {return status==SUCCESS;} + double err; + float area; // the overlapping area, a percentage as computed in Occupancy Grid. + + bool operator < (const Result & rr) const {return (err< rr.err);} + bool operator <= (const Result & rr) const {return (err<=rr.err);} + bool operator > (const Result & rr) const {return (err> rr.err);} + bool operator >= (const Result & rr) const {return (err>=rr.err);} + bool operator == (const Result & rr) const {return (err==rr.err);} + bool operator != (const Result & rr) const {return (err!=rr.err);} + + std::pair ComputeAvgErr() const + { + double sum_before=0; + double sum_after=0; + for(unsigned int ii=0;ii *mov; + A2Mesh *fix; + + ErrorCode status; + AlignPair::Param ap; + + math::SubtractiveRingRNG myrnd; + + /**** End Data Members *********/ + + template < class MESH > + void ConvertMesh(MESH &M1, A2Mesh &M2) + { + tri::Append::MeshCopy(M2,M1); + } + + template < class VERTEX > + void ConvertVertex(const std::vector &vert1, std::vector &vert2, Box3d *Clip=0) + { + + vert2.clear(); + typename std::vector::const_iterator vi; + A2Vertex tv; + Box3 bb; + if(Clip){ + bb.Import(*Clip); + for(vi=vert1.begin();vi &vert, int SampleNum, AlignPair::Param::SampleModeEnum SampleMode); + bool SampleMovVertRandom(std::vector &vert, int SampleNum); + bool SampleMovVertNormalEqualized(std::vector &vert, int SampleNum); +/* +*Una volta trovati coppie di punti corrispondenti se ne sceglie +al piu' per calcolare la trasformazione che li fa coincidere. +La scelta viene fatta in base ai due parametri PassLo e PassHi; +*/ + bool ChoosePoints( + std::vector &Ps, // vertici corrispondenti su fix (rossi) + std::vector &Ns, // normali corrispondenti su fix (rossi) + std::vector &Pt, // vertici scelti su mov (verdi) + std::vector &OPt, // vertici scelti su mov (verdi) + //vector &Nt, // normali scelti su mov (verdi) + double PassHi, + Histogramf &H); + +/* +Minimo esempio di codice per l'uso della funzione di allineamento. + +AlignPair::A2Mesh Mov,Fix; // le due mesh da allineare +vector MovVert; // i punti sulla mov da usare per l'allineamento +Matrix44d In; In.SetIdentity(); // la trasformazione iniziale che applicata a mov la porterebbe su fix. + +AlignPair aa; // l'oggetto principale. +AlignPair::Param ap; +UGrid< AlignPair::A2Mesh::face_container > UG; + +Fix.LoadPly("FixMesh.ply"); // Standard ply loading +Mov.LoadPly("MovMesh.ply"); +Fix.Init( Ident, false); // Inizializzazione necessaria (normali per vert, +Mov.Init( Ident, false); // info per distanza punto faccia ecc) + +AlignPair::InitFix(&Fix, ap, UG); // la mesh fix viene messa nella ug. + +aa.ConvertVertex(Mov.vert,MovVert); // si campiona la mesh Mov per trovare un po' di vertici. +aa.SampleMovVert(MovVert, ap.SampleNum, ap.SampleMode); + +aa.mov=&MovVert; // si assegnano i membri principali dell'oggetto align pair +aa.fix=&Fix; +aa.ap = ap; + +aa.Align(In,UG,res); // si spera :) + // il risultato sta nella matrice res.Tr; + +res.as.Dump(stdout); +*/ + + bool Align(const Matrix44d &in, A2Grid &UG, A2GridVert &UGV, Result &res) + { + res.ap=ap; + + bool ret=Align(UG, UGV, in, res.Tr, res.Pfix, res.Nfix, res.Pmov, res.Nmov, res.H, res.as); + + res.err=res.as.LastPcl50(); + res.status=status; + return ret; + } + + double Abs2Perc(double val, Box3d bb) const {return val/bb.Diag();} + double Perc2Abs(double val, Box3d bb) const {return val*bb.Diag();} + +/************************************************************************************ +Versione Vera della Align a basso livello. + +Si assume che la mesh fix sia gia' stata messa nella ug u con le debite trasformazioni. +in + +************************************************************************************/ + + bool Align( + A2Grid &u, + A2GridVert &uv, + const Matrix44d &in, // trasformazione Iniziale che porta i punti di mov su fix + Matrix44d &out, // trasformazione calcolata + std::vector &Pfix, // vertici corrispondenti su src (rossi) + std::vector &Nfix, // normali corrispondenti su src (rossi) + std::vector &OPmov, // vertici scelti su trg (verdi) prima della trasformazione in ingresso (Original Point Target) + std::vector &ONmov, // normali scelti su trg (verdi) + Histogramf &H, + Stat &as); + + + bool InitMov( + std::vector< Point3d > &movvert, + std::vector< Point3d > &movnorm, + Box3d &movbox, + const Matrix44d &in ); + + static bool InitFixVert(A2Mesh *fm, + AlignPair::Param &pp, + A2GridVert &u, + int PreferredGridSize=0); + + static bool InitFix(A2Mesh *fm, + AlignPair::Param &pp, + A2Grid &u, + int PreferredGridSize=0); + +}; // end class + +} // end namespace vcg + +#endif From 74d417ac13de8bf473845851905eca92602230c8 Mon Sep 17 00:00:00 2001 From: alemuntoni Date: Fri, 29 May 2020 12:17:10 +0200 Subject: [PATCH 03/13] point_matching_scale moved to vcg --- vcg/complex/algorithms/point_matching_scale.h | 134 ++++++++++++++++++ 1 file changed, 134 insertions(+) create mode 100644 vcg/complex/algorithms/point_matching_scale.h diff --git a/vcg/complex/algorithms/point_matching_scale.h b/vcg/complex/algorithms/point_matching_scale.h new file mode 100644 index 00000000..d1dfb34c --- /dev/null +++ b/vcg/complex/algorithms/point_matching_scale.h @@ -0,0 +1,134 @@ +/**************************************************************************** +* VCGLib o o * +* Visual and Computer Graphics Library o o * +* _ O _ * +* Copyright(C) 2004-2016 \/)\/ * +* Visual Computing Lab /\/| * +* ISTI - Italian National Research Council | * +* \ * +* All rights reserved. * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) * +* for more details. * +* * +****************************************************************************/ + +#include +#include +#include +#include + +namespace vcg { + +template +struct RotoTranslation +{ + RotoTranslation(){} + Scalar _v[6]; + void ToMatrix(vcg::Matrix44 & m) + { + vcg::Matrix44 rot,tra; + rot.FromEulerAngles(_v[0],_v[1],_v[2]); + tra.SetTranslate(vcg::Point3(_v[3],_v[4],_v[5])); + m = tra * rot; + } +}; + +class PointMatchingScale { +private: + static std::vector *fix; + static std::vector *mov; + static vcg::Box3d b; + +public: + /** + * Compute a scaling transformation that bring PMov point as close as possible to Pfix + */ + static void ComputeScalingMatchMatrix( + vcg::Matrix44d &res, + std::vector &Pfix, + std::vector &Pmov) + { + fix = &Pfix; + mov = &Pmov; + b.SetNull(); + for(std::vector::iterator i = Pmov.begin(); i != Pmov.end(); ++i) + b.Add(*i); + + double scale = 1.0; + min_newuoa(1,&scale,errorScale); + + res.SetTranslate( b.Center()*(1.0-scale)); + res[0][0] = res[1][1] = res[2][2] = scale; + } + + /** + * Compute a rototranslation + scaling transformation that bring PMov point as close as possible to Pfix + */ + static void ComputeRotoTranslationScalingMatchMatrix( + vcg::Matrix44d &res, + std::vector &Pfix, + std::vector &Pmov) + { + fix = &Pfix; + mov = &Pmov; + b.SetNull(); + for(std::vector::iterator i = Pmov.begin(); i != Pmov.end(); ++i) + b.Add(*i); + + double x[7]={1.0,0.0,0.0,0.0,0.0,0.0,0.0}; + min_newuoa(7,&x[0],errorRotoTranslationScale); + + // rtm = rototranslation + RotoTranslation rt; + vcg::Matrix44d rtm; + memcpy(&rt._v[0],&x[1],6*sizeof(double)); + rt.ToMatrix(rtm); + + // res= scaling w.r.t. barycenter + res.SetTranslate( b.Center()*(1.0-x[0])); + res[0][0] = res[1][1] = res[2][2] = x[0]; + res = rtm*res; + } + + static double errorScale(int n, double *x) + { + assert(n==1); (void)n; + double dist = 0; + std::vector::iterator i = mov->begin(); + std::vector::iterator ifix = fix->begin(); + for(; i != mov->end(); ++i,++ifix) + dist += vcg::SquaredDistance(((*i)-b.Center())*(*x)+b.Center() , *ifix); + + return dist; + } + + static double errorRotoTranslationScale(int n, double *x) { + assert(n==7); (void)n; + double dist = 0; + std::vector::iterator i = mov->begin(); + std::vector::iterator ifix = fix->begin(); + + RotoTranslation rt; + vcg::Matrix44d m; + memcpy(&rt._v[0],&x[1],6*sizeof(double)); + rt.ToMatrix(m); + + for(; i != mov->end(); ++i,++ifix) { + dist += vcg::SquaredDistance( m*(((*i)-b.Center())*(x[0])+b.Center()),*ifix); + } + return dist; + } + +}; + +} + From 6cd9d7aa91ac7860031446a4e1f2ae0c46bd42ee Mon Sep 17 00:00:00 2001 From: alemuntoni Date: Fri, 29 May 2020 12:18:41 +0200 Subject: [PATCH 04/13] refactoring --- vcg/complex/algorithms/point_matching_scale.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/vcg/complex/algorithms/point_matching_scale.h b/vcg/complex/algorithms/point_matching_scale.h index d1dfb34c..7921110f 100644 --- a/vcg/complex/algorithms/point_matching_scale.h +++ b/vcg/complex/algorithms/point_matching_scale.h @@ -33,7 +33,7 @@ struct RotoTranslation { RotoTranslation(){} Scalar _v[6]; - void ToMatrix(vcg::Matrix44 & m) + void toMatrix(vcg::Matrix44 & m) { vcg::Matrix44 rot,tra; rot.FromEulerAngles(_v[0],_v[1],_v[2]); @@ -52,7 +52,7 @@ public: /** * Compute a scaling transformation that bring PMov point as close as possible to Pfix */ - static void ComputeScalingMatchMatrix( + static void computeScalingMatchMatrix( vcg::Matrix44d &res, std::vector &Pfix, std::vector &Pmov) @@ -73,7 +73,7 @@ public: /** * Compute a rototranslation + scaling transformation that bring PMov point as close as possible to Pfix */ - static void ComputeRotoTranslationScalingMatchMatrix( + static void computeRotoTranslationScalingMatchMatrix( vcg::Matrix44d &res, std::vector &Pfix, std::vector &Pmov) @@ -91,7 +91,7 @@ public: RotoTranslation rt; vcg::Matrix44d rtm; memcpy(&rt._v[0],&x[1],6*sizeof(double)); - rt.ToMatrix(rtm); + rt.toMatrix(rtm); // res= scaling w.r.t. barycenter res.SetTranslate( b.Center()*(1.0-x[0])); @@ -120,7 +120,7 @@ public: RotoTranslation rt; vcg::Matrix44d m; memcpy(&rt._v[0],&x[1],6*sizeof(double)); - rt.ToMatrix(m); + rt.toMatrix(m); for(; i != mov->end(); ++i,++ifix) { dist += vcg::SquaredDistance( m*(((*i)-b.Center())*(x[0])+b.Center()),*ifix); From a374e959ee6ffc8fc968b4c253fa3bf9c2b06130 Mon Sep 17 00:00:00 2001 From: alemuntoni Date: Fri, 29 May 2020 13:20:57 +0200 Subject: [PATCH 05/13] more implementation and some refactoring --- vcg/complex/algorithms/align_pair.h | 88 ++++++++++++++++++++--------- 1 file changed, 61 insertions(+), 27 deletions(-) diff --git a/vcg/complex/algorithms/align_pair.h b/vcg/complex/algorithms/align_pair.h index 8fbc0d4b..f7803a98 100644 --- a/vcg/complex/algorithms/align_pair.h +++ b/vcg/complex/algorithms/align_pair.h @@ -88,7 +88,7 @@ public: //bool Import(const char *filename) { Matrix44d Tr; Tr.SetIdentity(); return Import(filename,Tr);} //bool Import(const char *filename, Matrix44d &Tr); - inline bool InitVert(const Matrix44d &Tr) { + inline bool initVert(const Matrix44d &Tr) { Matrix44d Idn; Idn.SetIdentity(); if (Tr != Idn) tri::UpdatePosition::Matrix(*this, Tr); @@ -96,7 +96,7 @@ public: tri::UpdateBounding::Box(*this); return true; } - inline bool Init(const Matrix44d &Tr) { + inline bool init(const Matrix44d &Tr) { Matrix44d Idn; Idn.SetIdentity(); tri::Clean::RemoveUnreferencedVertex(*this); if (Tr != Idn) @@ -143,12 +143,12 @@ public: std::vector I; - double LastPcl50() const + double lastPcl50() const { return I.back().pcl50; } - int LastSampleUsed() const { + int lastSampleUsed() const { return I.back().SampleUsed; } @@ -156,11 +156,11 @@ public: int FixVertNum; int FixFaceNum; - int TotTime() { + int totTime() { return I.back().Time-StartTime; } - int IterTime(unsigned int i) const + int iterTime(unsigned int i) const { const int clock_per_ms = std::max(CLOCKS_PER_SEC / 1000,1); assert(i\n"); fprintf(fp, " Mindist 50ile Hi Avg RMS StdDev Time Tested Used Dist Bord Angl \n"); for (unsigned int qi = 0; qi < I.size(); ++qi) @@ -209,14 +209,14 @@ public: I[qi].MinDistAbs, I[qi].pcl50, I[qi].pclhi, I[qi].AVG, I[qi].RMS, I[qi].StdDev, - IterTime(qi), + iterTime(qi), I[qi].SampleTested, I[qi].SampleUsed, I[qi].DistanceDiscarded, I[qi].BorderDiscarded, I[qi].AngleDiscarded); fprintf(fp, "\n"); } // Restituisce true se nelle ultime iterazioni non e' diminuito // l'errore - inline bool Stable(int lastiter) + inline bool stable(int lastiter) { if (I.empty()) return false; @@ -304,7 +304,7 @@ public: double MaxScale; MatchModeEnum MatchMode; SampleModeEnum SampleMode; - void Dump(FILE *fp,double BoxDiag); + //void Dump(FILE *fp,double BoxDiag); }; @@ -334,7 +334,10 @@ public: Stat as; Param ap; ErrorCode status; - bool IsValid() {return status==SUCCESS;} + bool IsValid() + { + return status==SUCCESS; + } double err; float area; // the overlapping area, a percentage as computed in Occupancy Grid. @@ -345,12 +348,11 @@ public: bool operator == (const Result & rr) const {return (err==rr.err);} bool operator != (const Result & rr) const {return (err!=rr.err);} - std::pair ComputeAvgErr() const + std::pair computeAvgErr() const { double sum_before=0; double sum_after=0; - for(unsigned int ii=0;ii - void ConvertMesh(MESH &M1, A2Mesh &M2) + void convertMesh(MESH &M1, A2Mesh &M2) { tri::Append::MeshCopy(M2,M1); } template < class VERTEX > - void ConvertVertex(const std::vector &vert1, std::vector &vert2, Box3d *Clip=0) + void convertVertex(const std::vector &vert1, std::vector &vert2, Box3d *Clip=0) { - vert2.clear(); typename std::vector::const_iterator vi; A2Vertex tv; @@ -400,13 +432,15 @@ public: vert2.push_back(tv); } } - else - for(vi=vert1.begin();vi &vert, int SampleNum, AlignPair::Param::SampleModeEnum SampleMode); @@ -463,7 +497,7 @@ res.as.Dump(stdout); bool ret=Align(UG, UGV, in, res.Tr, res.Pfix, res.Nfix, res.Pmov, res.Nmov, res.H, res.as); - res.err=res.as.LastPcl50(); + res.err=res.as.lastPcl50(); res.status=status; return ret; } From 074a89c5888561c36e022efe9cd7333ffba4ca9b Mon Sep 17 00:00:00 2001 From: alemuntoni Date: Fri, 29 May 2020 14:48:29 +0200 Subject: [PATCH 06/13] more implementations imported from meshlab --- vcg/complex/algorithms/align_pair.h | 357 ++++++++++++++++-- vcg/complex/algorithms/point_matching_scale.h | 4 + 2 files changed, 325 insertions(+), 36 deletions(-) diff --git a/vcg/complex/algorithms/align_pair.h b/vcg/complex/algorithms/align_pair.h index f7803a98..bc5c7a6f 100644 --- a/vcg/complex/algorithms/align_pair.h +++ b/vcg/complex/algorithms/align_pair.h @@ -44,6 +44,7 @@ #include #include #include +#include namespace vcg { @@ -57,6 +58,12 @@ Classe per gestire un allineamento tra DUE sole mesh. class AlignPair { public: + AlignPair() + { + clear(); + myrnd.initialize(time(NULL)); + } + enum ErrorCode { SUCCESS, NO_COMMON_BBOX, @@ -334,7 +341,7 @@ public: Stat as; Param ap; ErrorCode status; - bool IsValid() + bool isValid() { return status==SUCCESS; } @@ -392,12 +399,6 @@ public: status=SUCCESS; } - AlignPair() - { - clear(); - myrnd.initialize(time(NULL)); - } - /******* Data Members *********/ std::vector *mov; @@ -443,22 +444,126 @@ public: } } - bool SampleMovVert(std::vector &vert, int SampleNum, AlignPair::Param::SampleModeEnum SampleMode); - bool SampleMovVertRandom(std::vector &vert, int SampleNum); - bool SampleMovVertNormalEqualized(std::vector &vert, int SampleNum); -/* -*Una volta trovati coppie di punti corrispondenti se ne sceglie -al piu' per calcolare la trasformazione che li fa coincidere. -La scelta viene fatta in base ai due parametri PassLo e PassHi; -*/ - bool ChoosePoints( - std::vector &Ps, // vertici corrispondenti su fix (rossi) - std::vector &Ns, // normali corrispondenti su fix (rossi) - std::vector &Pt, // vertici scelti su mov (verdi) - std::vector &OPt, // vertici scelti su mov (verdi) + inline bool sampleMovVert( + std::vector &vert, + int sampleNum, + AlignPair::Param::SampleModeEnum sampleMode) + { + switch (sampleMode) + { + case AlignPair::Param::SMRandom: + return SampleMovVertRandom(vert, sampleNum); + case AlignPair::Param::SMNormalEqualized: + return SampleMovVertNormalEqualized(vert, sampleNum); + default: + assert(0); + return false; + } + } + + inline bool SampleMovVertRandom(std::vector &vert, int sampleNum) + { + if (int(vert.size()) <= sampleNum) + return true; + for (int i = 0; i < sampleNum; ++i) { + int pos = myrnd.generate(vert.size()); + assert(pos >= 0 && pos < int(vert.size())); + std::swap(vert[i], vert[pos]); + } + vert.resize(sampleNum); + return true; + } + + bool SampleMovVertNormalEqualized(std::vector &vert, int sampleNum) + { + std::vector NV; + if (NV.size() == 0) { + GenNormal::Fibonacci(30, NV); + printf("Generated %i normals\n", int(NV.size())); + } + // Bucket vector dove, per ogni normale metto gli indici + // dei vertici ad essa corrispondenti + std::vector > BKT(NV.size()); + for (size_t i = 0; i < vert.size(); ++i) { + int ind = GenNormal::BestMatchingNormal(vert[i].N(), NV); + BKT[ind].push_back(int(i)); + } + + // vettore di contatori per sapere quanti punti ho gia' preso per ogni bucket + std::vector BKTpos(BKT.size(), 0); + + if (sampleNum >= int(vert.size())) + sampleNum = vert.size() - 1; + + for (int i = 0; i < sampleNum;) { + int ind = myrnd.generate(BKT.size()); // Scelgo un Bucket + int &CURpos = BKTpos[ind]; + std::vector &CUR = BKT[ind]; + + if (CURpos coppie di punti corrispondenti se ne sceglie + * al piu' per calcolare la trasformazione che li fa coincidere. + * La scelta viene fatta in base ai due parametri PassLo e PassHi; + */ + inline bool choosePoints( + std::vector &ps, // vertici corrispondenti su fix (rossi) + std::vector &ns, // normali corrispondenti su fix (rossi) + std::vector &pt, // vertici scelti su mov (verdi) + std::vector &opt, // vertici scelti su mov (verdi) //vector &Nt, // normali scelti su mov (verdi) - double PassHi, - Histogramf &H); + double passHi, + Histogramf &h) + { + const int N = ap.MaxPointNum; + double newmaxd = h.Percentile(float(passHi)); + int sz = int(ps.size()); + int fnd = 0; + int lastgood = sz - 1; + math::SubtractiveRingRNG myrnd; + while (fnd < N && fnd < lastgood) { + int index = fnd + myrnd.generate(lastgood - fnd); + double dd = Distance(ps[index], pt[index]); + if (dd <= newmaxd){ + std::swap(ps[index], ps[fnd]); + std::swap(ns[index], ns[fnd]); + std::swap(pt[index], pt[fnd]); + std::swap(opt[index], opt[fnd]); + ++fnd; + } + else { + std::swap(ps[index], ps[lastgood]); + std::swap(ns[index], ns[lastgood]); + std::swap(pt[index], pt[lastgood]); + std::swap(opt[index], opt[lastgood]); + lastgood--; + } + } + ps.resize(fnd); + ns.resize(fnd); + pt.resize(fnd); + opt.resize(fnd); + + if ((int)ps.size() < ap.MinPointNum){ + printf("Troppi pochi punti!\n"); + ps.clear(); + ns.clear(); + pt.clear(); + opt.clear(); + return false; + } + return true; + } /* Minimo esempio di codice per l'uso della funzione di allineamento. @@ -491,19 +596,26 @@ aa.Align(In,UG,res); // si spera :) res.as.Dump(stdout); */ - bool Align(const Matrix44d &in, A2Grid &UG, A2GridVert &UGV, Result &res) + bool align(const Matrix44d &in, A2Grid &UG, A2GridVert &UGV, Result &res) { res.ap=ap; - bool ret=Align(UG, UGV, in, res.Tr, res.Pfix, res.Nfix, res.Pmov, res.Nmov, res.H, res.as); + bool ret=align(UG, UGV, in, res.Tr, res.Pfix, res.Nfix, res.Pmov, res.Nmov, res.H, res.as); res.err=res.as.lastPcl50(); res.status=status; return ret; } - double Abs2Perc(double val, Box3d bb) const {return val/bb.Diag();} - double Perc2Abs(double val, Box3d bb) const {return val*bb.Diag();} + double abs2Perc(double val, Box3d bb) const + { + return val/bb.Diag(); + } + + double perc2Abs(double val, Box3d bb) const + { + return val*bb.Diag(); + } /************************************************************************************ Versione Vera della Align a basso livello. @@ -513,17 +625,190 @@ in ************************************************************************************/ - bool Align( + inline bool align( A2Grid &u, - A2GridVert &uv, - const Matrix44d &in, // trasformazione Iniziale che porta i punti di mov su fix - Matrix44d &out, // trasformazione calcolata - std::vector &Pfix, // vertici corrispondenti su src (rossi) - std::vector &Nfix, // normali corrispondenti su src (rossi) - std::vector &OPmov, // vertici scelti su trg (verdi) prima della trasformazione in ingresso (Original Point Target) - std::vector &ONmov, // normali scelti su trg (verdi) - Histogramf &H, - Stat &as); + A2GridVert &uv, + const Matrix44d &in, // trasformazione Iniziale che porta i punti di mov su fix + Matrix44d &out, // trasformazione calcolata + std::vector &pfix, // vertici corrispondenti su src (rossi) + std::vector &nfix, // normali corrispondenti su src (rossi) + std::vector &opmov, // vertici scelti su trg (verdi) prima della trasformazione in ingresso (Original Point Target) + std::vector &onmov, // normali scelti su trg (verdi) + Histogramf &h, + Stat &as) + { + std::vector beyondCntVec; // vettore per marcare i movvert che sicuramente non si devono usare + // ogni volta che un vertice si trova a distanza oltre max dist viene incrementato il suo contatore; + // i movvert che sono stati scartati piu' di MaxCntDist volte non si guardano piu'; + const int maxBeyondCnt = 3; + std::vector< Point3d > movvert; + std::vector< Point3d > movnorm; + std::vector Pmov; // vertici scelti dopo la trasf iniziale + status = SUCCESS; + int tt0 = clock(); + + out = in; + + int i; + + double cosAngleThr = cos(ap.MaxAngleRad); + double startMinDist = ap.MinDistAbs; + int tt1 = clock(); + int ttsearch = 0; + int ttleast = 0; + int nc = 0; + as.clear(); + as.StartTime = clock(); + + beyondCntVec.resize(mov->size(), 0); + + /**************** BEGIN ICP LOOP ****************/ + do { + Stat::IterInfo ii; + Box3d movbox; + InitMov(movvert, movnorm, movbox, out); + h.SetRange(0.0f, float(startMinDist), 512, 2.5f); + pfix.clear(); + nfix.clear(); + Pmov.clear(); + opmov.clear(); + onmov.clear(); + int tts0 = clock(); + ii.MinDistAbs = startMinDist; + int LocSampleNum = std::min(ap.SampleNum, int(movvert.size())); + Box3d fixbox; + if (u.Empty()) + fixbox = uv.bbox; + else + fixbox = u.bbox; + for (i = 0; i < LocSampleNum; ++i) { + if (beyondCntVec[i] < maxBeyondCnt) { + if (fixbox.IsIn(movvert[i])) { + double error = startMinDist; + Point3d closestPoint, closestNormal; + double maxd = startMinDist; + ii.SampleTested++; + if (u.Empty()) {// using the point cloud grid{ + A2Mesh::VertexPointer vp = tri::GetClosestVertex(*fix, uv, movvert[i], maxd, error); + if (error >= startMinDist) { + ii.DistanceDiscarded++; ++beyondCntVec[i]; continue; + } + if (movnorm[i].dot(vp->N()) < cosAngleThr) { + ii.AngleDiscarded++; continue; + } + closestPoint = vp->P(); + closestNormal = vp->N(); + } + else {// using the standard faces and grid + A2Mesh::FacePointer f = vcg::tri::GetClosestFaceBase(*fix, u, movvert[i], maxd, error, closestPoint); + if (error >= startMinDist) { + ii.DistanceDiscarded++; ++beyondCntVec[i]; continue; + } + if (movnorm[i].dot(f->N()) < cosAngleThr) { + ii.AngleDiscarded++; continue; + } + Point3d ip; + InterpolationParameters(*f, f->N(), closestPoint, ip); + const double IP_EPS = 0.00001; + // If ip[i] == 0 it means that we are on the edge opposite to i + if ((fabs(ip[0]) <= IP_EPS && f->IsB(1)) || (fabs(ip[1]) <= IP_EPS && f->IsB(2)) || (fabs(ip[2]) <= IP_EPS && f->IsB(0))){ + ii.BorderDiscarded++; continue; + } + closestNormal = f->N(); + } + // The sample was accepted. Store it. + Pmov.push_back(movvert[i]); + opmov.push_back((*mov)[i].P()); + onmov.push_back((*mov)[i].N()); + nfix.push_back(closestNormal); + pfix.push_back(closestPoint); + h.Add(float(error)); + ii.SampleUsed++; + } + else { + beyondCntVec[i] = maxBeyondCnt + 1; + } + } + } // End for each pmov + int tts1 = clock(); + //printf("Found %d pairs\n",(int)Pfix.size()); + if (!choosePoints(pfix, nfix, Pmov, opmov, ap.PassHiFilter, h)) { + if (int(pfix.size()) < ap.MinPointNum){ + status = TOO_FEW_POINTS; + ii.Time = clock(); + as.I.push_back(ii); + return false; + } + } + Matrix44d newout; + switch (ap.MatchMode){ + case AlignPair::Param::MMSimilarity: + vcg::PointMatchingScale::computeRotoTranslationScalingMatchMatrix(newout, pfix, Pmov); + break; + case AlignPair::Param::MMRigid: + ComputeRigidMatchMatrix(pfix, Pmov, newout); + break; + default: + status = UNKNOWN_MODE; + ii.Time = clock(); + as.I.push_back(ii); + return false; + } + + //double sum_before=0; + //double sum_after=0; + //for(unsigned int iii=0;iii %f\n",sum_before/double(Pfix.size()),sum_after/double(Pfix.size()) ) ; + + // le passate successive utilizzano quindi come trasformazione iniziale questa appena trovata. + // Nei prossimi cicli si parte da questa matrice come iniziale. + out = newout * out; + + assert(Pfix.size() == Pmov.size()); + int tts2 = clock(); + ttsearch += tts1 - tts0; + ttleast += tts2 - tts1; + ii.pcl50 = h.Percentile(.5); + ii.pclhi = h.Percentile(float(ap.PassHiFilter)); + ii.AVG = h.Avg(); + ii.RMS = h.RMS(); + ii.StdDev = h.StandardDeviation(); + ii.Time = clock(); + as.I.push_back(ii); + nc++; + // The distance of the next points to be considered is lowered according to the parameter. + // We use 5 times the percentile of the found points. + if (ap.ReduceFactorPerc<1) + startMinDist = std::max(ap.MinDistAbs*ap.MinMinDistPerc, std::min(startMinDist, 5.0*h.Percentile(float(ap.ReduceFactorPerc)))); + } while ( + nc <= ap.MaxIterNum && + h.Percentile(.5) > ap.TrgDistAbs && + (ncap.MaxScale || math::Abs(1 - scv[1]) > ap.MaxScale || math::Abs(1 - scv[2]) > ap.MaxScale)) { + status = TOO_MUCH_SCALE; + return false; + } + if (shv[0] > ap.MaxShear || shv[1] > ap.MaxShear || shv[2] > ap.MaxShear) { + status = TOO_MUCH_SHEAR; + return false; + } + printf("Grid %i %i %i - fn %i\n", u.siz[0], u.siz[1], u.siz[2], fix->fn); + printf("Init %8.3f Loop %8.3f Search %8.3f least sqrt %8.3f\n", + float(tt1 - tt0) / CLOCKS_PER_SEC, float(tt2 - tt1) / CLOCKS_PER_SEC, + float(ttsearch) / CLOCKS_PER_SEC, float(ttleast) / CLOCKS_PER_SEC); + + return true; + } bool InitMov( diff --git a/vcg/complex/algorithms/point_matching_scale.h b/vcg/complex/algorithms/point_matching_scale.h index 7921110f..2acfb2a1 100644 --- a/vcg/complex/algorithms/point_matching_scale.h +++ b/vcg/complex/algorithms/point_matching_scale.h @@ -21,6 +21,9 @@ * * ****************************************************************************/ +#ifndef VCG_POINT_MATCHING_SCALE +#define VCG_POINT_MATCHING_SCALE + #include #include #include @@ -132,3 +135,4 @@ public: } +#endif From 64e352374a854f68379bff7ce84775774d64a80f Mon Sep 17 00:00:00 2001 From: alemuntoni Date: Fri, 29 May 2020 15:18:33 +0200 Subject: [PATCH 07/13] last implementations moved from meshlab --- vcg/complex/algorithms/align_pair.h | 105 +++++++++++++++++++++++----- 1 file changed, 89 insertions(+), 16 deletions(-) diff --git a/vcg/complex/algorithms/align_pair.h b/vcg/complex/algorithms/align_pair.h index bc5c7a6f..dceb5ffb 100644 --- a/vcg/complex/algorithms/align_pair.h +++ b/vcg/complex/algorithms/align_pair.h @@ -511,11 +511,14 @@ public: return true; } + /* - * Una volta trovati coppie di punti corrispondenti se ne sceglie - * al piu' per calcolare la trasformazione che li fa coincidere. - * La scelta viene fatta in base ai due parametri PassLo e PassHi; - */ + This function is used to choose remove outliers after each ICP iteration. + All the points with a distance over the given Percentile are discarded. + It uses two parameters + MaxPointNum an (unused) hard limit on the number of points that are chosen + MinPointNum the minimum number of points that have to be chosen to be usable + */ inline bool choosePoints( std::vector &ps, // vertici corrispondenti su fix (rossi) std::vector &ns, // normali corrispondenti su fix (rossi) @@ -625,6 +628,18 @@ in ************************************************************************************/ + /* + The Main ICP alignment Function: + It assumes that: + we have two meshes: + - Fix the mesh that does not move and stays in the spatial indexing structure. + - Mov the mesh we 'move' e.g. the one for which we search the transforamtion. + + requires normalize normals for vertices AND faces + Allinea due mesh; + Assume che: + la uniform grid sia gia' inizializzata con la mesh fix + */ inline bool align( A2Grid &u, A2GridVert &uv, @@ -666,7 +681,7 @@ in do { Stat::IterInfo ii; Box3d movbox; - InitMov(movvert, movnorm, movbox, out); + initMov(movvert, movnorm, movbox, out); h.SetRange(0.0f, float(startMinDist), 512, 2.5f); pfix.clear(); nfix.clear(); @@ -810,22 +825,80 @@ in return true; } - - bool InitMov( + /* + Funzione chiamata dalla Align ad ogni ciclo + Riempie i vettori e con i coordinate e normali presi dal vettore di vertici mov + della mesh da muovere trasformata secondo la matrice + Calcola anche il nuovo bounding box di tali vertici trasformati. + */ + inline bool initMov( std::vector< Point3d > &movvert, std::vector< Point3d > &movnorm, Box3d &movbox, - const Matrix44d &in ); + const Matrix44d &in ) + { + Point3d pp, nn; - static bool InitFixVert(A2Mesh *fm, - AlignPair::Param &pp, - A2GridVert &u, - int PreferredGridSize=0); + movvert.clear(); + movnorm.clear(); + movbox.SetNull(); - static bool InitFix(A2Mesh *fm, - AlignPair::Param &pp, - A2Grid &u, - int PreferredGridSize=0); + A2Mesh::VertexIterator vi; + for (vi = mov->begin(); vi != mov->end(); vi++) { + pp = in*(*vi).P(); + nn = in*Point3d((*vi).P() + (*vi).N()) - pp; + nn.Normalize(); + movvert.push_back(pp); + movnorm.push_back(nn); + movbox.Add(pp); + } + return true; + } + + static inline bool InitFixVert( + A2Mesh *fm, + AlignPair::Param &pp, + A2GridVert &u, + int preferredGridSize=0) + { + Box3d bb2 = fm->bbox; + double minDist = pp.MinDistAbs*1.1; + //the bbox of the grid should be enflated of the mindist used in the ICP search + bb2.Offset(Point3d(minDist, minDist, minDist)); + + u.SetBBox(bb2); + + //Inserisco la src nella griglia + if (preferredGridSize == 0) + preferredGridSize = int(fm->vert.size())*pp.UGExpansionFactor; + u.Set(fm->vert.begin(), fm->vert.end());//, PreferredGridSize); + printf("UG %i %i %i\n", u.siz[0], u.siz[1], u.siz[2]); + return true; + } + + static inline bool initFix( + A2Mesh *fm, + AlignPair::Param &pp, + A2Grid &u, + int preferredGridSize=0) + { + tri::InitFaceIMark(*fm); + + Box3d bb2 = fm->bbox; + // double MinDist= fm->bbox.Diag()*pp.MinDistPerc; + double minDist = pp.MinDistAbs*1.1; + //gonfio della distanza utente il BBox della seconda mesh + bb2.Offset(Point3d(minDist, minDist, minDist)); + + u.SetBBox(bb2); + + //Inserisco la src nella griglia + if (preferredGridSize == 0) + preferredGridSize = int(fm->face.size())*pp.UGExpansionFactor; + u.Set(fm->face.begin(), fm->face.end(), preferredGridSize); + printf("UG %i %i %i\n", u.siz[0], u.siz[1], u.siz[2]); + return true; + } }; // end class From 4d57dde1020038d780d7202d814ff093ee6caea1 Mon Sep 17 00:00:00 2001 From: alemuntoni Date: Fri, 29 May 2020 15:21:38 +0200 Subject: [PATCH 08/13] fix compile error assert --- vcg/complex/algorithms/align_pair.h | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/vcg/complex/algorithms/align_pair.h b/vcg/complex/algorithms/align_pair.h index dceb5ffb..42bd7df6 100644 --- a/vcg/complex/algorithms/align_pair.h +++ b/vcg/complex/algorithms/align_pair.h @@ -658,7 +658,7 @@ in const int maxBeyondCnt = 3; std::vector< Point3d > movvert; std::vector< Point3d > movnorm; - std::vector Pmov; // vertici scelti dopo la trasf iniziale + std::vector pmov; // vertici scelti dopo la trasf iniziale status = SUCCESS; int tt0 = clock(); @@ -685,7 +685,7 @@ in h.SetRange(0.0f, float(startMinDist), 512, 2.5f); pfix.clear(); nfix.clear(); - Pmov.clear(); + pmov.clear(); opmov.clear(); onmov.clear(); int tts0 = clock(); @@ -732,7 +732,7 @@ in closestNormal = f->N(); } // The sample was accepted. Store it. - Pmov.push_back(movvert[i]); + pmov.push_back(movvert[i]); opmov.push_back((*mov)[i].P()); onmov.push_back((*mov)[i].N()); nfix.push_back(closestNormal); @@ -746,8 +746,8 @@ in } } // End for each pmov int tts1 = clock(); - //printf("Found %d pairs\n",(int)Pfix.size()); - if (!choosePoints(pfix, nfix, Pmov, opmov, ap.PassHiFilter, h)) { + //printf("Found %d pairs\n",(int)pfix.size()); + if (!choosePoints(pfix, nfix, pmov, opmov, ap.PassHiFilter, h)) { if (int(pfix.size()) < ap.MinPointNum){ status = TOO_FEW_POINTS; ii.Time = clock(); @@ -758,10 +758,10 @@ in Matrix44d newout; switch (ap.MatchMode){ case AlignPair::Param::MMSimilarity: - vcg::PointMatchingScale::computeRotoTranslationScalingMatchMatrix(newout, pfix, Pmov); + vcg::PointMatchingScale::computeRotoTranslationScalingMatchMatrix(newout, pfix, pmov); break; case AlignPair::Param::MMRigid: - ComputeRigidMatchMatrix(pfix, Pmov, newout); + ComputeRigidMatchMatrix(pfix, pmov, newout); break; default: status = UNKNOWN_MODE; @@ -772,17 +772,17 @@ in //double sum_before=0; //double sum_after=0; - //for(unsigned int iii=0;iii %f\n",sum_before/double(Pfix.size()),sum_after/double(Pfix.size()) ) ; + //printf("Distance %f -> %f\n",sum_before/double(pfix.size()),sum_after/double(pfix.size()) ) ; // le passate successive utilizzano quindi come trasformazione iniziale questa appena trovata. // Nei prossimi cicli si parte da questa matrice come iniziale. out = newout * out; - assert(Pfix.size() == Pmov.size()); + assert(pfix.size() == pmov.size()); int tts2 = clock(); ttsearch += tts1 - tts0; ttleast += tts2 - tts1; From e5a15a2c4886b145535d2949e03c62ea5da73ff5 Mon Sep 17 00:00:00 2001 From: alemuntoni Date: Fri, 29 May 2020 17:26:25 +0200 Subject: [PATCH 09/13] solved compile error perfect_spatial_hashing --- vcg/space/index/perfect_spatial_hashing.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vcg/space/index/perfect_spatial_hashing.h b/vcg/space/index/perfect_spatial_hashing.h index e5dd576c..bbcbdef9 100644 --- a/vcg/space/index/perfect_spatial_hashing.h +++ b/vcg/space/index/perfect_spatial_hashing.h @@ -1009,7 +1009,7 @@ namespace vcg * \return */ inline bool operator[](const typename UniformGrid::CellCoordinate &at) { return m_Mask[at.X()][at.Y()][at.Z()]; } - inline bool& GetFlag(const int i, const int j, const int k)const { return m_Mask[i][j][k]; } + inline const bool& GetFlag(const int i, const int j, const int k)const { return m_Mask[i][j][k]; } inline void SetFlat(const int i, const int j, const int k) { m_Mask[i][j][k] = true; } From 5b6d6ec767e9197705a50249d96f7e0715df93ae Mon Sep 17 00:00:00 2001 From: alemuntoni Date: Sat, 30 May 2020 17:10:27 +0200 Subject: [PATCH 10/13] first non-working align_pair sample --- apps/sample/common.pri | 14 +- apps/sample/sample.pro | 109 +++++++------ apps/sample/trimesh_align_pair/my_mesh.h | 151 ++++++++++++++++++ .../trimesh_align_pair/trimesh_align_pair.cpp | 131 +++++++++++++++ .../trimesh_align_pair/trimesh_align_pair.pro | 10 ++ vcg/math/matrix44.h | 3 +- 6 files changed, 368 insertions(+), 50 deletions(-) create mode 100644 apps/sample/trimesh_align_pair/my_mesh.h create mode 100644 apps/sample/trimesh_align_pair/trimesh_align_pair.cpp create mode 100644 apps/sample/trimesh_align_pair/trimesh_align_pair.pro diff --git a/apps/sample/common.pri b/apps/sample/common.pri index dd976378..6cb16b15 100644 --- a/apps/sample/common.pri +++ b/apps/sample/common.pri @@ -1,11 +1,21 @@ -DEPENDPATH += . ../../.. -INCLUDEPATH += . ../../.. ../../../eigenlib +DEPENDPATH += \ + . \ + ../../.. + +INCLUDEPATH += \ + . \ + ../../.. \ + ../../../eigenlib + + CONFIG += console c++11 TEMPLATE = app + # Mac specific Config required to avoid to make application bundles CONFIG -= app_bundle QMAKE_CXXFLAGS += -std=c++11 + unix { CONFIG(release, debug|release) { DEFINES *= NDEBUG diff --git a/apps/sample/sample.pro b/apps/sample/sample.pro index b1c10547..38fcfcc5 100644 --- a/apps/sample/sample.pro +++ b/apps/sample/sample.pro @@ -1,48 +1,63 @@ +TEMPLATE = subdirs -TEMPLATE = subdirs -SUBDIRS = trimesh_allocate \ - trimesh_attribute \ - trimesh_attribute_saving \ - trimesh_ball_pivoting \ - trimesh_base \ - trimesh_closest \ - trimesh_clustering \ - trimesh_color \ - trimesh_copy \ - trimesh_create \ - trimesh_curvature \ - trimesh_cylinder_clipping \ - trimesh_disk_parametrization \ - trimesh_fitting \ - trimesh_geodesic \ - trimesh_harmonic \ - trimesh_hole \ - trimesh_implicit_smooth \ - trimesh_indexing \ - trimesh_inertia \ - trimesh_intersection_plane \ - trimesh_intersection_mesh \ - trimesh_isosurface \ - trimesh_join \ - trimesh_kdtree \ - trimesh_montecarlo_sampling \ - trimesh_normal \ - trimesh_optional \ - trimesh_pointmatching \ - trimesh_pointcloud_sampling \ - trimesh_ray \ - trimesh_refine \ - trimesh_remeshing \ - trimesh_sampling \ - trimesh_select \ - trimesh_smooth \ - trimesh_split_vertex \ - trimesh_texture \ - trimesh_texture_clean \ - trimesh_topology \ - trimesh_topological_cut \ - trimesh_voronoi \ - trimesh_voronoiatlas \ - trimesh_voronoiclustering \ - trimesh_voronoisampling \ - +SUBDIRS = \ + aabb_binary_tree \ + colorspace \ + #edgemesh_sampling \ + polygonmesh_base \ + polygonmesh_dual \ + polygonmesh_optimize \ + polygonmesh_polychord_collapse \ + #polygonmesh_quadsimpl \ + polygonmesh_smooth \ + polygonmesh_zonohedra \ + space_index_2d \ + #space_minimal \ + space_packer \ + space_rasterized_packer \ + trimesh_align_pair \ + trimesh_allocate \ + trimesh_attribute \ + trimesh_attribute_saving \ + trimesh_ball_pivoting \ + trimesh_base \ + trimesh_closest \ + trimesh_clustering \ + trimesh_color \ + trimesh_copy \ + trimesh_create \ + trimesh_curvature \ + trimesh_cylinder_clipping \ + trimesh_disk_parametrization \ + trimesh_fitting \ + trimesh_geodesic \ + trimesh_harmonic \ + trimesh_hole \ + trimesh_implicit_smooth \ + trimesh_indexing \ + trimesh_inertia \ + trimesh_intersection_plane \ + trimesh_intersection_mesh \ + trimesh_isosurface \ + trimesh_join \ + trimesh_kdtree \ + trimesh_montecarlo_sampling \ + trimesh_normal \ + trimesh_optional \ + trimesh_pointmatching \ + trimesh_pointcloud_sampling \ + trimesh_ray \ + trimesh_refine \ + trimesh_remeshing \ + trimesh_sampling \ + trimesh_select \ + trimesh_smooth \ + trimesh_split_vertex \ + trimesh_texture \ + trimesh_texture_clean \ + trimesh_topology \ + trimesh_topological_cut \ + trimesh_voronoi \ + trimesh_voronoiatlas \ + trimesh_voronoiclustering \ + trimesh_voronoisampling \ diff --git a/apps/sample/trimesh_align_pair/my_mesh.h b/apps/sample/trimesh_align_pair/my_mesh.h new file mode 100644 index 00000000..14a604ce --- /dev/null +++ b/apps/sample/trimesh_align_pair/my_mesh.h @@ -0,0 +1,151 @@ +/**************************************************************************** +* VCGLib o o * +* Visual and Computer Graphics Library o o * +* _ O _ * +* Copyright(C) 2004-2016 \/)\/ * +* Visual Computing Lab /\/| * +* ISTI - Italian National Research Council | * +* \ * +* All rights reserved. * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) * +* for more details. * +* * +****************************************************************************/ +#ifndef MY_MESH_H +#define MY_MESH_H + +#include + +typedef double Scalarm; +typedef vcg::Point2 Point2m; +typedef vcg::Point3 Point3m; +typedef vcg::Point4 Point4m; +typedef vcg::Plane3 Plane3m; +typedef vcg::Segment2 Segment2m; +typedef vcg::Segment3 Segment3m; +typedef vcg::Box3 Box3m; +typedef vcg::Matrix44 Matrix44m; +typedef vcg::Matrix33 Matrix33m; +typedef vcg::Shot Shotm; +typedef vcg::Similarity Similaritym; + +namespace vcg +{ + namespace vertex + { + template class Coord3m: public Coord, T> { + public: static void Name(std::vector & name){name.push_back(std::string("Coord3m"));T::Name(name);} + }; + + template class Normal3m: public Normal, T> { + public: static void Name(std::vector & name){name.push_back(std::string("Normal3m"));T::Name(name);} + }; + + template class CurvatureDirmOcf: public CurvatureDirOcf, T> { + public: static void Name(std::vector & name){name.push_back(std::string("CurvatureDirmOcf"));T::Name(name);} + }; + + template class RadiusmOcf: public RadiusOcf { + public: static void Name(std::vector & name){name.push_back(std::string("RadiusmOcf"));T::Name(name);} + }; + + }//end namespace vertex + namespace face + { + template class Normal3m: public NormalAbs, T> { + public: static void Name(std::vector & name){name.push_back(std::string("Normal3m"));T::Name(name);} + }; + + template class CurvatureDirmOcf: public CurvatureDirOcf, T> { + public: static void Name(std::vector & name){name.push_back(std::string("CurvatureDirdOcf"));T::Name(name);} + }; + + }//end namespace face +}//end namespace vcg + +// Forward declarations needed for creating the used types +class CVertexO; +class CEdgeO; +class CFaceO; + +// Declaration of the semantic of the used types +class CUsedTypesO: public vcg::UsedTypes < vcg::Use::AsVertexType, + vcg::Use::AsEdgeType, + vcg::Use::AsFaceType >{}; + + +// The Main Vertex Class +// Most of the attributes are optional and must be enabled before use. +// Each vertex needs 40 byte, on 32bit arch. and 44 byte on 64bit arch. + +class CVertexO : public vcg::Vertex< CUsedTypesO, + vcg::vertex::InfoOcf, /* 4b */ + vcg::vertex::Coord3m, /* 12b */ + vcg::vertex::BitFlags, /* 4b */ + vcg::vertex::Normal3m, /* 12b */ + vcg::vertex::Qualityf, /* 4b */ + vcg::vertex::Color4b, /* 4b */ + vcg::vertex::VFAdjOcf, /* 0b */ + vcg::vertex::MarkOcf, /* 0b */ + vcg::vertex::TexCoordfOcf, /* 0b */ + vcg::vertex::CurvaturefOcf, /* 0b */ + vcg::vertex::CurvatureDirmOcf, /* 0b */ + vcg::vertex::RadiusmOcf /* 0b */ +>{ +}; + + +// The Main Edge Class +class CEdgeO : public vcg::Edge{ +}; + +// Each face needs 32 byte, on 32bit arch. and 48 byte on 64bit arch. +class CFaceO : public vcg::Face< CUsedTypesO, + vcg::face::InfoOcf, /* 4b */ + vcg::face::VertexRef, /*12b */ + vcg::face::BitFlags, /* 4b */ + vcg::face::Normal3m, /*12b */ + vcg::face::QualityfOcf, /* 0b */ + vcg::face::MarkOcf, /* 0b */ + vcg::face::Color4bOcf, /* 0b */ + vcg::face::FFAdjOcf, /* 0b */ + vcg::face::VFAdjOcf, /* 0b */ + vcg::face::CurvatureDirmOcf, /* 0b */ + vcg::face::WedgeTexCoordfOcf /* 0b */ +> {}; + + +class MyMesh : public vcg::tri::TriMesh< vcg::vertex::vector_ocf, vcg::face::vector_ocf > +{ +public : + int sfn; //The number of selected faces. + int svn; //The number of selected vertices. + + int pvn; //the number of the polygonal vertices + int pfn; //the number of the polygonal faces + + Matrix44m Tr; // Usually it is the identity. It is applied in rendering and filters can or cannot use it. (most of the filter will ignore this) + + const Box3m &trBB() + { + static Box3m bb; + bb.SetNull(); + bb.Add(Tr,bbox); + return bb; + } +}; + +#endif // MY_MESH_H diff --git a/apps/sample/trimesh_align_pair/trimesh_align_pair.cpp b/apps/sample/trimesh_align_pair/trimesh_align_pair.cpp new file mode 100644 index 00000000..c9876c76 --- /dev/null +++ b/apps/sample/trimesh_align_pair/trimesh_align_pair.cpp @@ -0,0 +1,131 @@ +/**************************************************************************** +* VCGLib o o * +* Visual and Computer Graphics Library o o * +* _ O _ * +* Copyright(C) 2004-2016 \/)\/ * +* Visual Computing Lab /\/| * +* ISTI - Italian National Research Council | * +* \ * +* All rights reserved. * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) * +* for more details. * +* * +****************************************************************************/ +/*! \file trimesh_align_pair.cpp +\ingroup code_sample + +\brief the minimal example of using the lib + +This file contain a minimal example of the library + +*/ + +#include + +#include + +#include +#include + +#include "my_mesh.h" + +using namespace vcg; +using namespace std; + +std::vector* vcg::PointMatchingScale::fix; +std::vector* vcg::PointMatchingScale::mov; +vcg::Box3d vcg::PointMatchingScale::b; + +int main(int argc,char ** argv) +{ + if(argc<3) { + printf("Usage: trimesh_smooth \n"); + return 0; + } + + MyMesh m1, m2; + + //open first mesh + int err = tri::io::Importer::Open(m1,argv[1]); + if(err) { // all the importers return 0 in case of success + printf("Error in reading %s: '%s'\n", argv[1], tri::io::Importer::ErrorMsg(err)); + exit(-1); + } + + //open second mesh + err = tri::io::Importer::Open(m1,argv[2]); + if(err) { // all the importers return 0 in case of success + printf("Error in reading %s: '%s'\n", argv[2], tri::io::Importer::ErrorMsg(err)); + exit(-1); + } + + ////PARAMS + /////TODO + vcg::Matrix44d MovM; + vcg::AlignPair::Result result; + vcg::AlignPair::Param ap; + + //MovM + vcg::Matrix44d FixM=vcg::Matrix44d::Construct(m1.Tr); + MovM=vcg::Matrix44d::Construct(m2.Tr); + MovM = Inverse(FixM) * MovM; + + vcg::AlignPair::A2Mesh fix; + vcg::AlignPair aa; + + // 1) Convert fixed mesh and put it into the grid. + m1.face.EnableMark(); + aa.convertMesh(m1,fix); + + vcg::AlignPair::A2Grid UG; + vcg::AlignPair::A2GridVert VG; + + if(m1.fn==0 || ap.UseVertexOnly) { + fix.initVert(vcg::Matrix44d::Identity()); + vcg::AlignPair::InitFixVert(&fix,ap,VG); + } + else { + fix.init(vcg::Matrix44d::Identity()); + vcg::AlignPair::initFix(&fix, ap, UG); + } + + + // 2) Convert the second mesh and sample a points on it. + //MM(movId)->updateDataMask(MeshModel::MM_FACEMARK); + m2.face.EnableMark(); + std::vector tmpmv; + aa.convertVertex(m2.vert,tmpmv); + aa.sampleMovVert(tmpmv, ap.SampleNum, ap.SampleMode); + + aa.mov=&tmpmv; + aa.fix=&fix; + aa.ap = ap; + + vcg::Matrix44d In=MovM; + // Perform the ICP algorithm + aa.align(In,UG,VG,result); + + m2.Tr = result.Tr; + tri::UpdatePosition::Matrix(m2, m2.Tr, true); + tri::UpdateBounding::Box(m2); + m2.Tr.SetIdentity(); + + //result.FixName=fixId; + //result.MovName=movId; + //result.as.Dump(stdout); + + //saves the rotated mesh + tri::io::ExporterPLY::Save(m2 ,"out.ply"); + + return 0; +} + diff --git a/apps/sample/trimesh_align_pair/trimesh_align_pair.pro b/apps/sample/trimesh_align_pair/trimesh_align_pair.pro new file mode 100644 index 00000000..81c0969c --- /dev/null +++ b/apps/sample/trimesh_align_pair/trimesh_align_pair.pro @@ -0,0 +1,10 @@ +include(../common.pri) + +TARGET = trimesh_align_pair + +HEADERS += \ + my_mesh.h + +SOURCES += \ + trimesh_align_pair.cpp \ + ../../../wrap/ply/plylib.cpp diff --git a/vcg/math/matrix44.h b/vcg/math/matrix44.h index 2dca8aca..7ca46188 100644 --- a/vcg/math/matrix44.h +++ b/vcg/math/matrix44.h @@ -192,7 +192,8 @@ public: template static inline Matrix44 Construct( const Matrix44 & b ) { - Matrix44 tmp; tmp.FromMatrix(b); + Matrix44 tmp; + tmp.FromMatrix(b); return tmp; } From fcdf421f47a18281810880b2349f37a610b7ba65 Mon Sep 17 00:00:00 2001 From: alemuntoni Date: Mon, 1 Jun 2020 16:33:44 +0200 Subject: [PATCH 11/13] align_pair sample working --- apps/sample/common.pri | 2 +- apps/sample/trimesh_align_pair/my_mesh.h | 151 ------------------ .../trimesh_align_pair/trimesh_align_pair.cpp | 32 ++-- .../trimesh_align_pair/trimesh_align_pair.pro | 3 - vcg/complex/algorithms/align_pair.h | 37 +++-- 5 files changed, 41 insertions(+), 184 deletions(-) delete mode 100644 apps/sample/trimesh_align_pair/my_mesh.h diff --git a/apps/sample/common.pri b/apps/sample/common.pri index 6cb16b15..070301f5 100644 --- a/apps/sample/common.pri +++ b/apps/sample/common.pri @@ -8,7 +8,7 @@ INCLUDEPATH += \ ../../../eigenlib -CONFIG += console c++11 +CONFIG += c++11 TEMPLATE = app # Mac specific Config required to avoid to make application bundles diff --git a/apps/sample/trimesh_align_pair/my_mesh.h b/apps/sample/trimesh_align_pair/my_mesh.h deleted file mode 100644 index 14a604ce..00000000 --- a/apps/sample/trimesh_align_pair/my_mesh.h +++ /dev/null @@ -1,151 +0,0 @@ -/**************************************************************************** -* VCGLib o o * -* Visual and Computer Graphics Library o o * -* _ O _ * -* Copyright(C) 2004-2016 \/)\/ * -* Visual Computing Lab /\/| * -* ISTI - Italian National Research Council | * -* \ * -* All rights reserved. * -* * -* This program is free software; you can redistribute it and/or modify * -* it under the terms of the GNU General Public License as published by * -* the Free Software Foundation; either version 2 of the License, or * -* (at your option) any later version. * -* * -* This program is distributed in the hope that it will be useful, * -* but WITHOUT ANY WARRANTY; without even the implied warranty of * -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * -* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) * -* for more details. * -* * -****************************************************************************/ -#ifndef MY_MESH_H -#define MY_MESH_H - -#include - -typedef double Scalarm; -typedef vcg::Point2 Point2m; -typedef vcg::Point3 Point3m; -typedef vcg::Point4 Point4m; -typedef vcg::Plane3 Plane3m; -typedef vcg::Segment2 Segment2m; -typedef vcg::Segment3 Segment3m; -typedef vcg::Box3 Box3m; -typedef vcg::Matrix44 Matrix44m; -typedef vcg::Matrix33 Matrix33m; -typedef vcg::Shot Shotm; -typedef vcg::Similarity Similaritym; - -namespace vcg -{ - namespace vertex - { - template class Coord3m: public Coord, T> { - public: static void Name(std::vector & name){name.push_back(std::string("Coord3m"));T::Name(name);} - }; - - template class Normal3m: public Normal, T> { - public: static void Name(std::vector & name){name.push_back(std::string("Normal3m"));T::Name(name);} - }; - - template class CurvatureDirmOcf: public CurvatureDirOcf, T> { - public: static void Name(std::vector & name){name.push_back(std::string("CurvatureDirmOcf"));T::Name(name);} - }; - - template class RadiusmOcf: public RadiusOcf { - public: static void Name(std::vector & name){name.push_back(std::string("RadiusmOcf"));T::Name(name);} - }; - - }//end namespace vertex - namespace face - { - template class Normal3m: public NormalAbs, T> { - public: static void Name(std::vector & name){name.push_back(std::string("Normal3m"));T::Name(name);} - }; - - template class CurvatureDirmOcf: public CurvatureDirOcf, T> { - public: static void Name(std::vector & name){name.push_back(std::string("CurvatureDirdOcf"));T::Name(name);} - }; - - }//end namespace face -}//end namespace vcg - -// Forward declarations needed for creating the used types -class CVertexO; -class CEdgeO; -class CFaceO; - -// Declaration of the semantic of the used types -class CUsedTypesO: public vcg::UsedTypes < vcg::Use::AsVertexType, - vcg::Use::AsEdgeType, - vcg::Use::AsFaceType >{}; - - -// The Main Vertex Class -// Most of the attributes are optional and must be enabled before use. -// Each vertex needs 40 byte, on 32bit arch. and 44 byte on 64bit arch. - -class CVertexO : public vcg::Vertex< CUsedTypesO, - vcg::vertex::InfoOcf, /* 4b */ - vcg::vertex::Coord3m, /* 12b */ - vcg::vertex::BitFlags, /* 4b */ - vcg::vertex::Normal3m, /* 12b */ - vcg::vertex::Qualityf, /* 4b */ - vcg::vertex::Color4b, /* 4b */ - vcg::vertex::VFAdjOcf, /* 0b */ - vcg::vertex::MarkOcf, /* 0b */ - vcg::vertex::TexCoordfOcf, /* 0b */ - vcg::vertex::CurvaturefOcf, /* 0b */ - vcg::vertex::CurvatureDirmOcf, /* 0b */ - vcg::vertex::RadiusmOcf /* 0b */ ->{ -}; - - -// The Main Edge Class -class CEdgeO : public vcg::Edge{ -}; - -// Each face needs 32 byte, on 32bit arch. and 48 byte on 64bit arch. -class CFaceO : public vcg::Face< CUsedTypesO, - vcg::face::InfoOcf, /* 4b */ - vcg::face::VertexRef, /*12b */ - vcg::face::BitFlags, /* 4b */ - vcg::face::Normal3m, /*12b */ - vcg::face::QualityfOcf, /* 0b */ - vcg::face::MarkOcf, /* 0b */ - vcg::face::Color4bOcf, /* 0b */ - vcg::face::FFAdjOcf, /* 0b */ - vcg::face::VFAdjOcf, /* 0b */ - vcg::face::CurvatureDirmOcf, /* 0b */ - vcg::face::WedgeTexCoordfOcf /* 0b */ -> {}; - - -class MyMesh : public vcg::tri::TriMesh< vcg::vertex::vector_ocf, vcg::face::vector_ocf > -{ -public : - int sfn; //The number of selected faces. - int svn; //The number of selected vertices. - - int pvn; //the number of the polygonal vertices - int pfn; //the number of the polygonal faces - - Matrix44m Tr; // Usually it is the identity. It is applied in rendering and filters can or cannot use it. (most of the filter will ignore this) - - const Box3m &trBB() - { - static Box3m bb; - bb.SetNull(); - bb.Add(Tr,bbox); - return bb; - } -}; - -#endif // MY_MESH_H diff --git a/apps/sample/trimesh_align_pair/trimesh_align_pair.cpp b/apps/sample/trimesh_align_pair/trimesh_align_pair.cpp index c9876c76..3073797d 100644 --- a/apps/sample/trimesh_align_pair/trimesh_align_pair.cpp +++ b/apps/sample/trimesh_align_pair/trimesh_align_pair.cpp @@ -36,7 +36,14 @@ This file contain a minimal example of the library #include #include -#include "my_mesh.h" +class MyFace; +class MyVertex; + +struct MyUsedTypes : public vcg::UsedTypes< vcg::Use::AsVertexType, vcg::Use::AsFaceType>{}; + +class MyVertex : public vcg::Vertex< MyUsedTypes, vcg::vertex::Coord3d, vcg::vertex::Normal3d, vcg::vertex::Color4b, vcg::vertex::BitFlags >{}; +class MyFace : public vcg::Face < MyUsedTypes, vcg::face::VertexRef, vcg::face::Normal3d, vcg::face::FFAdj, vcg::face::Mark, vcg::face::BitFlags > {}; +class MyMesh : public vcg::tri::TriMesh< std::vector, std::vector > {}; using namespace vcg; using namespace std; @@ -62,28 +69,30 @@ int main(int argc,char ** argv) } //open second mesh - err = tri::io::Importer::Open(m1,argv[2]); + err = tri::io::Importer::Open(m2,argv[2]); if(err) { // all the importers return 0 in case of success printf("Error in reading %s: '%s'\n", argv[2], tri::io::Importer::ErrorMsg(err)); exit(-1); } + vcg::tri::UpdateNormal::PerVertexNormalizedPerFace(m1); + vcg::tri::UpdateNormal::PerVertexNormalizedPerFace(m2); + ////PARAMS /////TODO - vcg::Matrix44d MovM; vcg::AlignPair::Result result; vcg::AlignPair::Param ap; //MovM - vcg::Matrix44d FixM=vcg::Matrix44d::Construct(m1.Tr); - MovM=vcg::Matrix44d::Construct(m2.Tr); - MovM = Inverse(FixM) * MovM; + //vcg::Matrix44d FixM=vcg::Matrix44d::Construct(m1.Tr); + //MovM=vcg::Matrix44d::Construct(m2.Tr); + //MovM = Inverse(FixM) * MovM; vcg::AlignPair::A2Mesh fix; vcg::AlignPair aa; // 1) Convert fixed mesh and put it into the grid. - m1.face.EnableMark(); + //m1.face.EnableMark(); aa.convertMesh(m1,fix); vcg::AlignPair::A2Grid UG; @@ -101,7 +110,7 @@ int main(int argc,char ** argv) // 2) Convert the second mesh and sample a points on it. //MM(movId)->updateDataMask(MeshModel::MM_FACEMARK); - m2.face.EnableMark(); + //m2.face.EnableMark(); std::vector tmpmv; aa.convertVertex(m2.vert,tmpmv); aa.sampleMovVert(tmpmv, ap.SampleNum, ap.SampleMode); @@ -110,14 +119,13 @@ int main(int argc,char ** argv) aa.fix=&fix; aa.ap = ap; - vcg::Matrix44d In=MovM; + vcg::Matrix44d In; + In.SetIdentity(); // Perform the ICP algorithm aa.align(In,UG,VG,result); - m2.Tr = result.Tr; - tri::UpdatePosition::Matrix(m2, m2.Tr, true); + tri::UpdatePosition::Matrix(m2, result.Tr, true); tri::UpdateBounding::Box(m2); - m2.Tr.SetIdentity(); //result.FixName=fixId; //result.MovName=movId; diff --git a/apps/sample/trimesh_align_pair/trimesh_align_pair.pro b/apps/sample/trimesh_align_pair/trimesh_align_pair.pro index 81c0969c..0fba0c03 100644 --- a/apps/sample/trimesh_align_pair/trimesh_align_pair.pro +++ b/apps/sample/trimesh_align_pair/trimesh_align_pair.pro @@ -2,9 +2,6 @@ include(../common.pri) TARGET = trimesh_align_pair -HEADERS += \ - my_mesh.h - SOURCES += \ trimesh_align_pair.cpp \ ../../../wrap/ply/plylib.cpp diff --git a/vcg/complex/algorithms/align_pair.h b/vcg/complex/algorithms/align_pair.h index 42bd7df6..51746ab6 100644 --- a/vcg/complex/algorithms/align_pair.h +++ b/vcg/complex/algorithms/align_pair.h @@ -24,6 +24,7 @@ #define VCG_ALIGN_PAIR_H #include +#include #include #include #include @@ -643,22 +644,22 @@ in inline bool align( A2Grid &u, A2GridVert &uv, - const Matrix44d &in, // trasformazione Iniziale che porta i punti di mov su fix - Matrix44d &out, // trasformazione calcolata - std::vector &pfix, // vertici corrispondenti su src (rossi) - std::vector &nfix, // normali corrispondenti su src (rossi) - std::vector &opmov, // vertici scelti su trg (verdi) prima della trasformazione in ingresso (Original Point Target) - std::vector &onmov, // normali scelti su trg (verdi) + const Matrix44d &in, // starting transformation that matches mov points to fix mesh + Matrix44d &out, // computed transformation + std::vector &pfix, // (red) corresponding vertices on src + std::vector &nfix, // (red) corresponding normals on src + std::vector &opmov, // chosen vertices on trg (verdi) before the input transormation (Original Point Target) + std::vector &onmov, // chosen normals on trg (verdi) Histogramf &h, Stat &as) { - std::vector beyondCntVec; // vettore per marcare i movvert che sicuramente non si devono usare - // ogni volta che un vertice si trova a distanza oltre max dist viene incrementato il suo contatore; - // i movvert che sono stati scartati piu' di MaxCntDist volte non si guardano piu'; + std::vector beyondCntVec; // flag vector to set the movverts that we should not use + // every time that a vertex is at a distance beyound max dist, its counter is incremented; + // movverts that has been discarded more than MaxCntDist times will not be considered anymore const int maxBeyondCnt = 3; std::vector< Point3d > movvert; std::vector< Point3d > movnorm; - std::vector pmov; // vertici scelti dopo la trasf iniziale + std::vector pmov; // vertices chosen after the transformation status = SUCCESS; int tt0 = clock(); @@ -746,7 +747,7 @@ in } } // End for each pmov int tts1 = clock(); - //printf("Found %d pairs\n",(int)pfix.size()); + printf("Found %d pairs\n",(int)pfix.size()); if (!choosePoints(pfix, nfix, pmov, opmov, ap.PassHiFilter, h)) { if (int(pfix.size()) < ap.MinPointNum){ status = TOO_FEW_POINTS; @@ -778,8 +779,8 @@ in //} //printf("Distance %f -> %f\n",sum_before/double(pfix.size()),sum_after/double(pfix.size()) ) ; - // le passate successive utilizzano quindi come trasformazione iniziale questa appena trovata. - // Nei prossimi cicli si parte da questa matrice come iniziale. + // the following tuns will use as a initial transformation, the one that has been just found. + // in the next loops the starting matrix will be this one. out = newout * out; assert(pfix.size() == pmov.size()); @@ -798,6 +799,7 @@ in // We use 5 times the percentile of the found points. if (ap.ReduceFactorPerc<1) startMinDist = std::max(ap.MinDistAbs*ap.MinMinDistPerc, std::min(startMinDist, 5.0*h.Percentile(float(ap.ReduceFactorPerc)))); + //as.dump(stderr); } while ( nc <= ap.MaxIterNum && h.Percentile(.5) > ap.TrgDistAbs && @@ -826,10 +828,11 @@ in } /* - Funzione chiamata dalla Align ad ogni ciclo - Riempie i vettori e con i coordinate e normali presi dal vettore di vertici mov - della mesh da muovere trasformata secondo la matrice - Calcola anche il nuovo bounding box di tali vertici trasformati. + * Function called by Align at every cycle. + * It fills the and vectors with the coordinates and normals + * taken from the the vertex vector "mov" of the mesh to move according to the + * matrix . + * It computes also the new bounding box of the transformed vertices */ inline bool initMov( std::vector< Point3d > &movvert, From da737ebad7f1396bf5ec380b9ef20551dcee950c Mon Sep 17 00:00:00 2001 From: alemuntoni Date: Mon, 1 Jun 2020 16:42:53 +0200 Subject: [PATCH 12/13] cleanups and comments --- .../trimesh_align_pair/trimesh_align_pair.cpp | 52 +++++++++++-------- 1 file changed, 31 insertions(+), 21 deletions(-) diff --git a/apps/sample/trimesh_align_pair/trimesh_align_pair.cpp b/apps/sample/trimesh_align_pair/trimesh_align_pair.cpp index 3073797d..b7903151 100644 --- a/apps/sample/trimesh_align_pair/trimesh_align_pair.cpp +++ b/apps/sample/trimesh_align_pair/trimesh_align_pair.cpp @@ -23,9 +23,14 @@ /*! \file trimesh_align_pair.cpp \ingroup code_sample -\brief the minimal example of using the lib +\brief the minimal example for aligning two meshes -This file contain a minimal example of the library +This file contain a minimal example for aligning two meshs. + +Example call: +./trimesh_align_pair mesh1.ply mesh2.ply output.ply + +output.ply will contain mesh2.ply rotated in order to be aligned to mesh1.ply */ @@ -39,11 +44,22 @@ This file contain a minimal example of the library class MyFace; class MyVertex; -struct MyUsedTypes : public vcg::UsedTypes< vcg::Use::AsVertexType, vcg::Use::AsFaceType>{}; +struct MyUsedTypes : + public vcg::UsedTypes< vcg::Use::AsVertexType, + vcg::Use::AsFaceType> +{}; -class MyVertex : public vcg::Vertex< MyUsedTypes, vcg::vertex::Coord3d, vcg::vertex::Normal3d, vcg::vertex::Color4b, vcg::vertex::BitFlags >{}; -class MyFace : public vcg::Face < MyUsedTypes, vcg::face::VertexRef, vcg::face::Normal3d, vcg::face::FFAdj, vcg::face::Mark, vcg::face::BitFlags > {}; -class MyMesh : public vcg::tri::TriMesh< std::vector, std::vector > {}; +class MyVertex : + public vcg::Vertex< MyUsedTypes, vcg::vertex::Coord3d, vcg::vertex::Normal3d, vcg::vertex::Color4b, vcg::vertex::BitFlags> +{}; + +class MyFace : + public vcg::Face < MyUsedTypes, vcg::face::VertexRef, vcg::face::Normal3d, vcg::face::FFAdj, vcg::face::Mark, vcg::face::BitFlags > +{}; + +class MyMesh : + public vcg::tri::TriMesh< std::vector, std::vector > +{}; using namespace vcg; using namespace std; @@ -58,6 +74,10 @@ int main(int argc,char ** argv) printf("Usage: trimesh_smooth \n"); return 0; } + std::string outputname = "output.ply"; + if (argc == 4){ + outputname = std::string(argv[3]); + } MyMesh m1, m2; @@ -75,24 +95,17 @@ int main(int argc,char ** argv) exit(-1); } + //update normals vcg::tri::UpdateNormal::PerVertexNormalizedPerFace(m1); vcg::tri::UpdateNormal::PerVertexNormalizedPerFace(m2); ////PARAMS - /////TODO vcg::AlignPair::Result result; vcg::AlignPair::Param ap; - - //MovM - //vcg::Matrix44d FixM=vcg::Matrix44d::Construct(m1.Tr); - //MovM=vcg::Matrix44d::Construct(m2.Tr); - //MovM = Inverse(FixM) * MovM; - vcg::AlignPair::A2Mesh fix; vcg::AlignPair aa; // 1) Convert fixed mesh and put it into the grid. - //m1.face.EnableMark(); aa.convertMesh(m1,fix); vcg::AlignPair::A2Grid UG; @@ -109,8 +122,6 @@ int main(int argc,char ** argv) // 2) Convert the second mesh and sample a points on it. - //MM(movId)->updateDataMask(MeshModel::MM_FACEMARK); - //m2.face.EnableMark(); std::vector tmpmv; aa.convertVertex(m2.vert,tmpmv); aa.sampleMovVert(tmpmv, ap.SampleNum, ap.SampleMode); @@ -119,20 +130,19 @@ int main(int argc,char ** argv) aa.fix=&fix; aa.ap = ap; + //use identity as first matrix vcg::Matrix44d In; In.SetIdentity(); + // Perform the ICP algorithm aa.align(In,UG,VG,result); + //rotate m2 using the resulting transformation tri::UpdatePosition::Matrix(m2, result.Tr, true); tri::UpdateBounding::Box(m2); - //result.FixName=fixId; - //result.MovName=movId; - //result.as.Dump(stdout); - //saves the rotated mesh - tri::io::ExporterPLY::Save(m2 ,"out.ply"); + tri::io::ExporterPLY::Save(m2 ,outputname.c_str()); return 0; } From 26aad158354b0a3b1fb06d6ca5b09ade6404c0b2 Mon Sep 17 00:00:00 2001 From: alemuntoni Date: Mon, 1 Jun 2020 17:03:44 +0200 Subject: [PATCH 13/13] comment non compiling sample on windows --- apps/sample/sample.pro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/sample/sample.pro b/apps/sample/sample.pro index 38fcfcc5..1036b4d2 100644 --- a/apps/sample/sample.pro +++ b/apps/sample/sample.pro @@ -10,7 +10,7 @@ SUBDIRS = \ polygonmesh_polychord_collapse \ #polygonmesh_quadsimpl \ polygonmesh_smooth \ - polygonmesh_zonohedra \ + #polygonmesh_zonohedra \ space_index_2d \ #space_minimal \ space_packer \