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 ----*/
