int IdasInterface::lsolve(IDAMem IDA_mem, N_Vector b, N_Vector weight, N_Vector xz, N_Vector xzdot, N_Vector rr) { try { auto m = to_mem(IDA_mem->ida_lmem); auto& s = m->self; // Current time double t = IDA_mem->ida_tn; // Multiple of df_dydot to be added to the matrix double cj = IDA_mem->ida_cj; // Accuracy double delta = 0.0; // Call the preconditioner solve function (which solves the linear system) if (psolve(t, xz, xzdot, rr, b, b, cj, delta, static_cast<void*>(m), 0)) return 1; // Scale the correction to account for change in cj if (s.cj_scaling_) { double cjratio = IDA_mem->ida_cjratio; if (cjratio != 1.0) N_VScale(2.0/(1.0 + cjratio), b, b); } return 0; } catch(int flag) { // recoverable error return flag; } catch(exception& e) { // non-recoverable error userOut<true, PL_WARN>() << "lsolve failed: " << e.what() << endl; return -1; } }
int CVSpilsPSolve(void *cvode_mem, N_Vector r, N_Vector z, int lr) { CVodeMem cv_mem; CVSpilsMem cvspils_mem; int retval; cv_mem = (CVodeMem) cvode_mem; cvspils_mem = (CVSpilsMem)lmem; /* This call is counted in nps within the CVSp***Solve routine */ retval = psolve(tn, ycur, fcur, r, z, gamma, delta, lr, P_data, ytemp); return(retval); }
static int CVSpgmrPSolve(void *cvode_mem, N_Vector r, N_Vector z, int lr) { CVodeMem cv_mem; CVSpgmrMem cvspgmr_mem; int ier; cv_mem = (CVodeMem) cvode_mem; cvspgmr_mem = (CVSpgmrMem)lmem; ier = psolve(tn, ycur, fcur, r, z, gamma, delta, lr, P_data, ytemp); /* This call is counted in nps within the CVSpgmrSolve routine */ return(ier); }
int IDASpilsPSolve(void *ida_mem, N_Vector r, N_Vector z, int lr) { IDAMem IDA_mem; IDASpilsMem idaspils_mem; int retval; IDA_mem = (IDAMem) ida_mem; idaspils_mem = (IDASpilsMem) lmem; retval = psolve(tn, ycur, ypcur, rcur, r, z, cj, epslin, pdata, ytemp); /* This call is counted in nps within the IDASp**Solve routine */ return(retval); }
int KINSpilsPSolve(void *kinsol_mem, N_Vector r, N_Vector z, int lrdummy) { KINMem kin_mem; KINSpilsMem kinspils_mem; int ret; kin_mem = (KINMem) kinsol_mem; kinspils_mem = (KINSpilsMem) lmem; /* copy the rhs into z before the psolve call */ /* Note: z returns with the solution */ N_VScale(ONE, r, z); /* this call is counted in nps within the KINSpilsSolve routine */ ret = psolve(uu, uscale, fval, fscale, z, P_data, vtemp1); return(ret); }
bool mrpt::vision::pnp::rpnp::compute_pose(Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_) { // selecting an edge $P_{ i1 }P_{ i2 }$ by n random sampling int i1 = 0, i2 = 1; double lmin = Q(0, i1)*Q(0, i2) + Q(1, i1)*Q(1, i2) + Q(2, i1)*Q(2, i2); Eigen::MatrixXi rij (n,2); R_=Eigen::MatrixXd::Identity(3,3); t_=Eigen::Vector3d::Zero(); for (int i = 0; i < n; i++) for (int j = 0; j < 2; j++) rij(i, j) = rand() % n; for (int ii = 0; ii < n; ii++) { int i = rij(ii, 0), j = rij(ii,1); if (i == j) continue; double l = Q(0, i)*Q(0, j) + Q(1, i)*Q(1, j) + Q(2, i)*Q(2, j); if (l < lmin) { i1 = i; i2 = j; lmin = l; } } // calculating the rotation matrix of $O_aX_aY_aZ_a$. Eigen::Vector3d p1, p2, p0, x, y, z, dum_vec; p1 = P.col(i1); p2 = P.col(i2); p0 = (p1 + p2) / 2; x = p2 - p0; x /= x.norm(); if (abs(x(1)) < abs(x(2)) ) { dum_vec << 0, 1, 0; z = x.cross(dum_vec); z /= z.norm(); y = z.cross(x); y /= y.norm(); } else { dum_vec << 0, 0, 1; y = dum_vec.cross(x); y /= y.norm(); z = x.cross(y); x /= x.norm(); } Eigen::Matrix3d R0; R0.col(0) = x; R0.col(1) =y; R0.col(2) = z; for (int i = 0; i < n; i++) P.col(i) = R0.transpose() * (P.col(i) - p0); // Dividing the n - point set into(n - 2) 3 - point subsets // and setting up the P3P equations Eigen::Vector3d v1 = Q.col(i1), v2 = Q.col(i2); double cg1 = v1.dot(v2); double sg1 = sqrt(1 - cg1*cg1); double D1 = (P.col(i1) - P.col(i2)).norm(); Eigen::MatrixXd D4(n - 2, 5); int j = 0; Eigen::Vector3d vi; Eigen::VectorXd rowvec(5); for (int i = 0; i < n; i++) { if (i == i1 || i == i2) continue; vi = Q.col(i); double cg2 = v1.dot(vi); double cg3 = v2.dot(vi); double sg2 = sqrt(1 - cg2*cg2); double D2 = (P.col(i1) - P.col(i)).norm(); double D3 = (P.col(i) - P.col(i2)).norm(); // get the coefficients of the P3P equation from each subset. rowvec = getp3p(cg1, cg2, cg3, sg1, sg2, D1, D2, D3); D4.row(j) = rowvec; j += 1; if(j>n-3) break; } Eigen::VectorXd D7(8), dumvec(8), dumvec1(5); D7.setZero(); for (int i = 0; i < n-2; i++) { dumvec1 = D4.row(i); dumvec= getpoly7(dumvec1); D7 += dumvec; } Eigen::PolynomialSolver<double, 7> psolve(D7.reverse()); Eigen::VectorXcd comp_roots = psolve.roots().transpose(); Eigen::VectorXd real_comp, imag_comp; real_comp = comp_roots.real(); imag_comp = comp_roots.imag(); Eigen::VectorXd::Index max_index; double max_real= real_comp.cwiseAbs().maxCoeff(&max_index); std::vector<double> act_roots_; int cnt=0; for (int i=0; i<imag_comp.size(); i++ ) { if(abs(imag_comp(i))/max_real<0.001) { act_roots_.push_back(real_comp(i)); cnt++; } } double* ptr = &act_roots_[0]; Eigen::Map<Eigen::VectorXd> act_roots(ptr, cnt); if (cnt==0) { return false; } Eigen::VectorXd act_roots1(cnt); act_roots1 << act_roots.segment(0,cnt); std::vector<Eigen::Matrix3d> R_cum(cnt); std::vector<Eigen::Vector3d> t_cum(cnt); std::vector<double> err_cum(cnt); for(int i=0; i<cnt; i++) { double root = act_roots(i); // Compute the rotation matrix double d2 = cg1 + root; Eigen::Vector3d unitx, unity, unitz; unitx << 1,0,0; unity << 0,1,0; unitz << 0,0,1; x = v2*d2 -v1; x/=x.norm(); if (abs(unity.dot(x)) < abs(unitz.dot(x))) { z = x.cross(unity);z/=z.norm(); y=z.cross(x); y/y.norm(); } else { y=unitz.cross(x); y/=y.norm(); z = x.cross(y); z/=z.norm(); } R.col(0)=x; R.col(1)=y; R.col(2)=z; //calculating c, s, tx, ty, tz Eigen::MatrixXd D(2 * n, 6); D.setZero(); R0 = R.transpose(); Eigen::VectorXd r(Eigen::Map<Eigen::VectorXd>(R0.data(), R0.cols()*R0.rows())); for (int j = 0; j<n; j++) { double ui = img_pts(j, 0), vi = img_pts(j, 1), xi = P(0, j), yi = P(1, j), zi = P(2, j); D.row(2 * j) << -r(1)*yi + ui*(r(7)*yi + r(8)*zi) - r(2)*zi, -r(2)*yi + ui*(r(8)*yi - r(7)*zi) + r(1)*zi, -1, 0, ui, ui*r(6)*xi - r(0)*xi; D.row(2 * j + 1) << -r(4)*yi + vi*(r(7)*yi + r(8)*zi) - r(5)*zi, -r(5)*yi + vi*(r(8)*yi - r(7)*zi) + r(4)*zi, 0, -1, vi, vi*r(6)*xi - r(3)*xi; } Eigen::MatrixXd DTD = D.transpose()*D; Eigen::EigenSolver<Eigen::MatrixXd> es(DTD); Eigen::VectorXd Diag = es.pseudoEigenvalueMatrix().diagonal(); Eigen::MatrixXd V_mat = es.pseudoEigenvectors(); Eigen::MatrixXd::Index min_index; Diag.minCoeff(&min_index); Eigen::VectorXd V = V_mat.col(min_index); V /= V(5); double c = V(0), s = V(1); t << V(2), V(3), V(4); //calculating the camera pose by 3d alignment Eigen::VectorXd xi, yi, zi; xi = P.row(0); yi = P.row(1); zi = P.row(2); Eigen::MatrixXd XXcs(3, n), XXc(3,n); XXc.setZero(); XXcs.row(0) = r(0)*xi + (r(1)*c + r(2)*s)*yi + (-r(1)*s + r(2)*c)*zi + t(0)*Eigen::VectorXd::Ones(n); XXcs.row(1) = r(3)*xi + (r(4)*c + r(5)*s)*yi + (-r(4)*s + r(5)*c)*zi + t(1)*Eigen::VectorXd::Ones(n); XXcs.row(2) = r(6)*xi + (r(7)*c + r(8)*s)*yi + (-r(7)*s + r(8)*c)*zi + t(2)*Eigen::VectorXd::Ones(n); for (int ii = 0; ii < n; ii++) XXc.col(ii) = Q.col(ii)*XXcs.col(ii).norm(); Eigen::Matrix3d R2; Eigen::Vector3d t2; Eigen::MatrixXd XXw = obj_pts.transpose(); calcampose(XXc, XXw, R2, t2); R_cum[i] = R2; t_cum[i] = t2; for (int k = 0; k < n; k++) XXc.col(k) = R2 * XXw.col(k) + t2; Eigen::MatrixXd xxc(2, n); xxc.row(0) = XXc.row(0).array() / XXc.row(2).array(); xxc.row(1) = XXc.row(1).array() / XXc.row(2).array(); double res = ((xxc.row(0) - img_pts.col(0).transpose()).norm() + (xxc.row(1) - img_pts.col(1).transpose()).norm()) / 2; err_cum[i] = res; } int pos_cum = std::min_element(err_cum.begin(), err_cum.end()) - err_cum.begin(); R_ = R_cum[pos_cum]; t_ = t_cum[pos_cum]; return true; }
int fgmr(int n, void (*matvec) (double, double[], double, double[]), void (*psolve) (int, double[], double[]), double *rhs, double *sol, double tol, int im, int *itmax, FILE * fits) { /*---------------------------------------------------------------------- | *** Preconditioned FGMRES *** +----------------------------------------------------------------------- | This is a simple version of the ARMS preconditioned FGMRES algorithm. +----------------------------------------------------------------------- | Y. S. Dec. 2000. -- Apr. 2008 +----------------------------------------------------------------------- | on entry: |---------- | | rhs = real vector of length n containing the right hand side. | sol = real vector of length n containing an initial guess to the | solution on input. | tol = tolerance for stopping iteration | im = Krylov subspace dimension | (itmax) = max number of iterations allowed. | fits = NULL: no output | != NULL: file handle to output " resid vs time and its" | | on return: |---------- | fgmr int = 0 --> successful return. | int = 1 --> convergence not achieved in itmax iterations. | sol = contains an approximate solution (upon successful return). | itmax = has changed. It now contains the number of steps required | to converge -- +----------------------------------------------------------------------- | internal work arrays: |---------- | vv = work array of length [im+1][n] (used to store the Arnoldi | basis) | hh = work array of length [im][im+1] (Householder matrix) | z = work array of length [im][n] to store preconditioned vectors +----------------------------------------------------------------------- | subroutines called : | matvec - matrix-vector multiplication operation | psolve - (right) preconditionning operation | psolve can be a NULL pointer (GMRES without preconditioner) +---------------------------------------------------------------------*/ int maxits = *itmax; int i, i1, ii, j, k, k1, its, retval, i_1 = 1; double **hh, *c, *s, *rs, t, t0; double beta, eps1 = 0.0, gam, **vv, **z; its = 0; vv = (double **)SUPERLU_MALLOC((im + 1) * sizeof(double *)); for (i = 0; i <= im; i++) vv[i] = doubleMalloc(n); z = (double **)SUPERLU_MALLOC(im * sizeof(double *)); hh = (double **)SUPERLU_MALLOC(im * sizeof(double *)); for (i = 0; i < im; i++) { hh[i] = doubleMalloc(i + 2); z[i] = doubleMalloc(n); } c = doubleMalloc(im); s = doubleMalloc(im); rs = doubleMalloc(im + 1); /*---- outer loop starts here ----*/ do { /*---- compute initial residual vector ----*/ matvec(1.0, sol, 0.0, vv[0]); for (j = 0; j < n; j++) vv[0][j] = rhs[j] - vv[0][j]; /* vv[0]= initial residual */ beta = dnrm2_(&n, vv[0], &i_1); /*---- print info if fits != null ----*/ if (fits != NULL && its == 0) fprintf(fits, "%8d %10.2e\n", its, beta); /*if ( beta < tol * dnrm2_(&n, rhs, &i_1) )*/ if ( !(beta >= tol * dnrm2_(&n, rhs, &i_1)) ) break; t = 1.0 / beta; /*---- normalize: vv[0] = vv[0] / beta ----*/ for (j = 0; j < n; j++) vv[0][j] = vv[0][j] * t; if (its == 0) eps1 = tol * beta; /*---- initialize 1-st term of rhs of hessenberg system ----*/ rs[0] = beta; for (i = 0; i < im; i++) { its++; i1 = i + 1; /*------------------------------------------------------------ | (Right) Preconditioning Operation z_{j} = M^{-1} v_{j} +-----------------------------------------------------------*/ if (psolve) psolve(n, z[i], vv[i]); else dcopy_(&n, vv[i], &i_1, z[i], &i_1); /*---- matvec operation w = A z_{j} = A M^{-1} v_{j} ----*/ matvec(1.0, z[i], 0.0, vv[i1]); /*------------------------------------------------------------ | modified gram - schmidt... | h_{i,j} = (w,v_{i}) | w = w - h_{i,j} v_{i} +------------------------------------------------------------*/ t0 = dnrm2_(&n, vv[i1], &i_1); for (j = 0; j <= i; j++) { double negt; t = ddot_(&n, vv[j], &i_1, vv[i1], &i_1); hh[i][j] = t; negt = -t; daxpy_(&n, &negt, vv[j], &i_1, vv[i1], &i_1); } /*---- h_{j+1,j} = ||w||_{2} ----*/ t = dnrm2_(&n, vv[i1], &i_1); while (t < 0.5 * t0) { t0 = t; for (j = 0; j <= i; j++) { double negt; t = ddot_(&n, vv[j], &i_1, vv[i1], &i_1); hh[i][j] += t; negt = -t; daxpy_(&n, &negt, vv[j], &i_1, vv[i1], &i_1); } t = dnrm2_(&n, vv[i1], &i_1); } hh[i][i1] = t; if (t != 0.0) { /*---- v_{j+1} = w / h_{j+1,j} ----*/ t = 1.0 / t; for (k = 0; k < n; k++) vv[i1][k] = vv[i1][k] * t; } /*--------------------------------------------------- | done with modified gram schimdt and arnoldi step | now update factorization of hh +--------------------------------------------------*/ /*-------------------------------------------------------- | perform previous transformations on i-th column of h +-------------------------------------------------------*/ for (k = 1; k <= i; k++) { k1 = k - 1; t = hh[i][k1]; hh[i][k1] = c[k1] * t + s[k1] * hh[i][k]; hh[i][k] = -s[k1] * t + c[k1] * hh[i][k]; } gam = sqrt(pow(hh[i][i], 2) + pow(hh[i][i1], 2)); /*--------------------------------------------------- | if gamma is zero then any small value will do | affect only residual estimate +--------------------------------------------------*/ /* if (gam == 0.0) gam = epsmac; */ /*---- get next plane rotation ---*/ if (gam > 0.0) { c[i] = hh[i][i] / gam; s[i] = hh[i][i1] / gam; } else { c[i] = 1.0; s[i] = 0.0; } rs[i1] = -s[i] * rs[i]; rs[i] = c[i] * rs[i]; /*---------------------------------------------------- | determine residual norm and test for convergence +---------------------------------------------------*/ hh[i][i] = c[i] * hh[i][i] + s[i] * hh[i][i1]; beta = fabs(rs[i1]); if (fits != NULL) fprintf(fits, "%8d %10.2e\n", its, beta); if (beta <= eps1 || its >= maxits) break; } if (i == im) i--; /*---- now compute solution. 1st, solve upper triangular system ----*/ rs[i] = rs[i] / hh[i][i]; for (ii = 1; ii <= i; ii++) { k = i - ii; k1 = k + 1; t = rs[k]; for (j = k1; j <= i; j++) t = t - hh[j][k] * rs[j]; rs[k] = t / hh[k][k]; } /*---- linear combination of v[i]'s to get sol. ----*/ for (j = 0; j <= i; j++) { t = rs[j]; for (k = 0; k < n; k++) sol[k] += t * z[j][k]; } /* calculate the residual and output */ matvec(1.0, sol, 0.0, vv[0]); for (j = 0; j < n; j++) vv[0][j] = rhs[j] - vv[0][j]; /* vv[0]= initial residual */ /*---- print info if fits != null ----*/ beta = dnrm2_(&n, vv[0], &i_1); /*---- restart outer loop if needed ----*/ /*if (beta >= eps1 / tol)*/ if ( !(beta < eps1 / tol) ) { its = maxits + 10; break; } if (beta <= eps1) break; } while(its < maxits); retval = (its >= maxits); for (i = 0; i <= im; i++) SUPERLU_FREE(vv[i]); SUPERLU_FREE(vv); for (i = 0; i < im; i++) { SUPERLU_FREE(hh[i]); SUPERLU_FREE(z[i]); } SUPERLU_FREE(hh); SUPERLU_FREE(z); SUPERLU_FREE(c); SUPERLU_FREE(s); SUPERLU_FREE(rs); *itmax = its; return retval; } /*----end of fgmr ----*/