template <typename fptype> static inline int
lapack_LU(fptype* a, size_t a_step, int m, fptype* b, size_t b_step, int n, int* info)
{
int lda = a_step / sizeof(fptype), sign = 0;
int* piv = new int[m];
transpose_square_inplace(a, lda, m);
if(b)
{
if(n == 1 && b_step == sizeof(fptype))
{
if(typeid(fptype) == typeid(float))
sgesv_(&m, &n, (float*)a, &lda, piv, (float*)b, &m, info);
else if(typeid(fptype) == typeid(double))
dgesv_(&m, &n, (double*)a, &lda, piv, (double*)b, &m, info);
}
else
{
int ldb = b_step / sizeof(fptype);
fptype* tmpB = new fptype[m*n];
transpose(b, ldb, tmpB, m, m, n);
if(typeid(fptype) == typeid(float))
sgesv_(&m, &n, (float*)a, &lda, piv, (float*)tmpB, &m, info);
else if(typeid(fptype) == typeid(double))
dgesv_(&m, &n, (double*)a, &lda, piv, (double*)tmpB, &m, info);
transpose(tmpB, m, b, ldb, n, m);
delete[] tmpB;
}
}
else
{
if(typeid(fptype) == typeid(float))
sgetrf_(&m, &m, (float*)a, &lda, piv, info);
else if(typeid(fptype) == typeid(double))
dgetrf_(&m, &m, (double*)a, &lda, piv, info);
}
if(*info == 0)
{
for(int i = 0; i < m; i++)
sign ^= piv[i] != i + 1;
*info = sign ? -1 : 1;
}
else
*info = 0; //in opencv LU function zero means error
delete[] piv;
return CV_HAL_ERROR_OK;
}
main()
{
int i, j , c1, c2, pivot[size], ok;
float A[size][size], b[size], AT[size*size]; /* single precision!!! */
A[0][0]=3.1; A[0][1]=1.3; A[0][2]=-5.7; /* matrix A */
A[1][0]=1.0; A[1][1]=-6.9; A[1][2]=5.8;
A[2][0]=3.4; A[2][1]=7.2; A[2][2]=-8.8;
b[0]=-1.3; /* if you define b as a matrix then you */
b[1]=-0.1; /* can solve multiple equations with */
b[2]=1.8; /* the same A but different b */
for (i=0; i<size; i++) /* to call a Fortran routine from C we */
{ /* have to transform the matrix */
for(j=0; j<size; j++) AT[j+size*i]=A[j][i];
}
c1=size; /* and put all numbers we want to pass */
c2=1; /* to the routine in variables */
/* find solution using LAPACK routine SGESV, all the arguments have to */
/* be pointers and you have to add an underscore to the routine name */
sgesv_(&c1, &c2, AT, &c1, pivot, b, &c1, &ok);
/*
parameters in the order as they appear in the function call
order of matrix A, number of right hand sides (b), matrix A,
leading dimension of A, array that records pivoting,
result vector b on entry, x on exit, leading dimension of b
return value */
for (j=0; j<size; j++) printf("%e\n", b[j]); /* print vector x */
}
void solve_(int B, int C, float*& D, int E, int* F, float* G, int H, int I){
float* tD = new float[B * B];
for(int i = 0; i < B; i++){
for(int j = 0; j < B; j++){
tD[j * B + i] = D[j * B + i];
}
}
sgesv_(&B, &C, tD, &E, F, G, &H, &I);
delete [] tD;
}
void THLapack_(gesv)(int n, int nrhs, real *a, int lda, int *ipiv, real *b, int ldb, int* info)
{
#ifdef USE_LAPACK
#if defined(TH_REAL_IS_DOUBLE)
dgesv_(&n, &nrhs, a, &lda, ipiv, b, &ldb, info);
#else
sgesv_(&n, &nrhs, a, &lda, ipiv, b, &ldb, info);
#endif
#else
THError("gesv : Lapack library not found in compile time\n");
#endif
return;
}
inline void
xgesv_call( xgesv_params< float >& p )
{
sgesv_(
&p.n,
&p.nrhs,
p.a,
&p.lda,
p.ipiv,
p.b,
&p.ldb,
&p.info
);
}
void THLapack_(gesv)(int n, int nrhs, real *a, int lda, int *ipiv, real *b, int ldb, int* info)
{
#ifdef USE_LAPACK
#if defined(TH_REAL_IS_DOUBLE)
extern void dgesv_(int *n, int *nrhs, double *a, int *lda, int *ipiv, double *b, int *ldb, int *info);
dgesv_(&n, &nrhs, a, &lda, ipiv, b, &ldb, info);
#else
extern void sgesv_(int *n, int *nrhs, float *a, int *lda, int *ipiv, float *b, int *ldb, int *info);
sgesv_(&n, &nrhs, a, &lda, ipiv, b, &ldb, info);
#endif
#else
THError("gesv : Lapack library not found in compile time\n");
#endif
return;
}
void CRGetDifferenceRodriguesRotPos(float delta_param[6], float jacobian[8][6], float hessian[6][6], float error[8], float lambda) {
float hessian_dash[6][6];
// _CRTestShowMatrix6x6(hessian);
CRGetMatrixFromHessianAndLambda(hessian_dash, hessian, lambda);
CRMatrixMultiTransposeMat8x6Vec8(delta_param, jacobian, error);
for (int i = 0; i < 6; i++) {
delta_param[i] = -delta_param[i];
}
int rank = 6;
int nrhs = 1;
int pivot[6];
int info = 0;
sgesv_((__CLPK_integer*)&rank, (__CLPK_integer*)&nrhs, (__CLPK_real*)hessian_dash, (__CLPK_integer*)&rank, (__CLPK_integer*)pivot,(__CLPK_real*)delta_param, (__CLPK_integer*)&rank, (__CLPK_integer*)&info);
}
/* Subroutine */ int sdrvge_(logical *dotype, integer *nn, integer *nval,
integer *nrhs, real *thresh, logical *tsterr, integer *nmax, real *a,
real *afac, real *asav, real *b, real *bsav, real *x, real *xact,
real *s, real *work, real *rwork, integer *iwork, integer *nout)
{
/* Initialized data */
static integer iseedy[4] = { 1988,1989,1990,1991 };
static char transs[1*3] = "N" "T" "C";
static char facts[1*3] = "F" "N" "E";
static char equeds[1*4] = "N" "R" "C" "B";
/* Format strings */
static char fmt_9999[] = "(1x,a6,\002, N =\002,i5,\002, type \002,i2,"
"\002, test(\002,i2,\002) =\002,g12.5)";
static char fmt_9997[] = "(1x,a6,\002, FACT='\002,a1,\002', TRANS='\002,"
"a1,\002', N=\002,i5,\002, EQUED='\002,a1,\002', type \002,i2,"
"\002, test(\002,i1,\002)=\002,g12.5)";
static char fmt_9998[] = "(1x,a6,\002, FACT='\002,a1,\002', TRANS='\002,"
"a1,\002', N=\002,i5,\002, type \002,i2,\002, test(\002,i1,\002)"
"=\002,g12.5)";
/* System generated locals */
address a__1[2];
integer i__1, i__2, i__3, i__4, i__5[2];
real r__1;
char ch__1[2];
/* Builtin functions */
/* Subroutine */ int s_copy(char *, char *, ftnlen, ftnlen);
integer s_wsfe(cilist *), do_fio(integer *, char *, ftnlen), e_wsfe(void);
/* Subroutine */ int s_cat(char *, char **, integer *, integer *, ftnlen);
/* Local variables */
integer i__, k, n, k1, nb, in, kl, ku, nt, lda;
char fact[1];
integer ioff, mode;
real amax;
char path[3];
integer imat, info;
char dist[1], type__[1];
integer nrun, ifact, nfail, iseed[4], nfact;
extern logical lsame_(char *, char *);
char equed[1];
integer nbmin;
real rcond, roldc;
extern /* Subroutine */ int sget01_(integer *, integer *, real *, integer
*, real *, integer *, integer *, real *, real *);
integer nimat;
real roldi;
extern doublereal sget06_(real *, real *);
extern /* Subroutine */ int sget02_(char *, integer *, integer *, integer
*, real *, integer *, real *, integer *, real *, integer *, real *
, real *);
real anorm;
integer itran;
extern /* Subroutine */ int sget04_(integer *, integer *, real *, integer
*, real *, integer *, real *, real *);
logical equil;
real roldo;
extern /* Subroutine */ int sget07_(char *, integer *, integer *, real *,
integer *, real *, integer *, real *, integer *, real *, integer *
, real *, real *, real *);
char trans[1];
integer izero, nerrs;
extern /* Subroutine */ int sgesv_(integer *, integer *, real *, integer *
, integer *, real *, integer *, integer *);
integer lwork;
logical zerot;
char xtype[1];
extern /* Subroutine */ int slatb4_(char *, integer *, integer *, integer
*, char *, integer *, integer *, real *, integer *, real *, char *
), aladhd_(integer *, char *),
alaerh_(char *, char *, integer *, integer *, char *, integer *,
integer *, integer *, integer *, integer *, integer *, integer *,
integer *, integer *);
logical prefac;
real colcnd;
extern doublereal slamch_(char *);
real rcondc;
extern doublereal slange_(char *, integer *, integer *, real *, integer *,
real *);
logical nofact;
integer iequed;
extern /* Subroutine */ int slaqge_(integer *, integer *, real *, integer
*, real *, real *, real *, real *, real *, char *);
real rcondi;
extern /* Subroutine */ int alasvm_(char *, integer *, integer *, integer
*, integer *);
real cndnum, anormi, rcondo, ainvnm;
extern /* Subroutine */ int sgeequ_(integer *, integer *, real *, integer
*, real *, real *, real *, real *, real *, integer *);
logical trfcon;
real anormo, rowcnd;
extern /* Subroutine */ int sgetrf_(integer *, integer *, real *, integer
*, integer *, integer *), sgetri_(integer *, real *, integer *,
integer *, real *, integer *, integer *), slacpy_(char *, integer
*, integer *, real *, integer *, real *, integer *),
slarhs_(char *, char *, char *, char *, integer *, integer *,
integer *, integer *, integer *, real *, integer *, real *,
integer *, real *, integer *, integer *, integer *);
extern doublereal slantr_(char *, char *, char *, integer *, integer *,
real *, integer *, real *);
extern /* Subroutine */ int slaset_(char *, integer *, integer *, real *,
real *, real *, integer *), slatms_(integer *, integer *,
char *, integer *, char *, real *, integer *, real *, real *,
integer *, integer *, char *, real *, integer *, real *, integer *
), xlaenv_(integer *, integer *);
real result[7];
extern /* Subroutine */ int sgesvx_(char *, char *, integer *, integer *,
real *, integer *, real *, integer *, integer *, char *, real *,
real *, real *, integer *, real *, integer *, real *, real *,
real *, real *, integer *, integer *);
real rpvgrw;
extern /* Subroutine */ int serrvx_(char *, integer *);
/* Fortran I/O blocks */
static cilist io___55 = { 0, 0, 0, fmt_9999, 0 };
static cilist io___61 = { 0, 0, 0, fmt_9997, 0 };
static cilist io___62 = { 0, 0, 0, fmt_9998, 0 };
static cilist io___63 = { 0, 0, 0, fmt_9997, 0 };
static cilist io___64 = { 0, 0, 0, fmt_9998, 0 };
static cilist io___65 = { 0, 0, 0, fmt_9997, 0 };
static cilist io___66 = { 0, 0, 0, fmt_9998, 0 };
static cilist io___67 = { 0, 0, 0, fmt_9997, 0 };
static cilist io___68 = { 0, 0, 0, fmt_9998, 0 };
/* -- LAPACK test routine (version 3.1) -- */
/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
/* November 2006 */
/* .. Scalar Arguments .. */
/* .. */
/* .. Array Arguments .. */
/* .. */
/* Purpose */
/* ======= */
/* SDRVGE tests the driver routines SGESV and -SVX. */
/* Arguments */
/* ========= */
/* DOTYPE (input) LOGICAL array, dimension (NTYPES) */
/* The matrix types to be used for testing. Matrices of type j */
/* (for 1 <= j <= NTYPES) are used for testing if DOTYPE(j) = */
/* .TRUE.; if DOTYPE(j) = .FALSE., then type j is not used. */
/* NN (input) INTEGER */
/* The number of values of N contained in the vector NVAL. */
/* NVAL (input) INTEGER array, dimension (NN) */
/* The values of the matrix column dimension N. */
/* NRHS (input) INTEGER */
/* The number of right hand side vectors to be generated for */
/* each linear system. */
/* THRESH (input) REAL */
/* The threshold value for the test ratios. A result is */
/* included in the output file if RESULT >= THRESH. To have */
/* every test ratio printed, use THRESH = 0. */
/* TSTERR (input) LOGICAL */
/* Flag that indicates whether error exits are to be tested. */
/* NMAX (input) INTEGER */
/* The maximum value permitted for N, used in dimensioning the */
/* work arrays. */
/* A (workspace) REAL array, dimension (NMAX*NMAX) */
/* AFAC (workspace) REAL array, dimension (NMAX*NMAX) */
/* ASAV (workspace) REAL array, dimension (NMAX*NMAX) */
/* B (workspace) REAL array, dimension (NMAX*NRHS) */
/* BSAV (workspace) REAL array, dimension (NMAX*NRHS) */
/* X (workspace) REAL array, dimension (NMAX*NRHS) */
/* XACT (workspace) REAL array, dimension (NMAX*NRHS) */
/* S (workspace) REAL array, dimension (2*NMAX) */
/* WORK (workspace) REAL array, dimension */
/* (NMAX*max(3,NRHS)) */
/* RWORK (workspace) REAL array, dimension (2*NRHS+NMAX) */
/* IWORK (workspace) INTEGER array, dimension (2*NMAX) */
/* NOUT (input) INTEGER */
/* The unit number for output. */
/* ===================================================================== */
/* .. Parameters .. */
/* .. */
/* .. Local Scalars .. */
/* .. */
/* .. Local Arrays .. */
/* .. */
/* .. External Functions .. */
/* .. */
/* .. External Subroutines .. */
/* .. */
/* .. Intrinsic Functions .. */
/* .. */
/* .. Scalars in Common .. */
/* .. */
/* .. Common blocks .. */
/* .. */
/* .. Data statements .. */
/* Parameter adjustments */
--iwork;
--rwork;
--work;
--s;
--xact;
--x;
--bsav;
--b;
--asav;
--afac;
--a;
--nval;
--dotype;
/* Function Body */
/* .. */
/* .. Executable Statements .. */
/* Initialize constants and the random number seed. */
s_copy(path, "Single precision", (ftnlen)1, (ftnlen)16);
s_copy(path + 1, "GE", (ftnlen)2, (ftnlen)2);
nrun = 0;
nfail = 0;
nerrs = 0;
for (i__ = 1; i__ <= 4; ++i__) {
iseed[i__ - 1] = iseedy[i__ - 1];
/* L10: */
}
/* Test the error exits */
if (*tsterr) {
serrvx_(path, nout);
}
infoc_1.infot = 0;
/* Set the block size and minimum block size for testing. */
nb = 1;
nbmin = 2;
xlaenv_(&c__1, &nb);
xlaenv_(&c__2, &nbmin);
/* Do for each value of N in NVAL */
i__1 = *nn;
for (in = 1; in <= i__1; ++in) {
n = nval[in];
lda = max(n,1);
*(unsigned char *)xtype = 'N';
nimat = 11;
if (n <= 0) {
nimat = 1;
}
i__2 = nimat;
for (imat = 1; imat <= i__2; ++imat) {
/* Do the tests only if DOTYPE( IMAT ) is true. */
if (! dotype[imat]) {
goto L80;
}
/* Skip types 5, 6, or 7 if the matrix size is too small. */
zerot = imat >= 5 && imat <= 7;
if (zerot && n < imat - 4) {
goto L80;
}
/* Set up parameters with SLATB4 and generate a test matrix */
/* with SLATMS. */
slatb4_(path, &imat, &n, &n, type__, &kl, &ku, &anorm, &mode, &
cndnum, dist);
rcondc = 1.f / cndnum;
s_copy(srnamc_1.srnamt, "SLATMS", (ftnlen)6, (ftnlen)6);
slatms_(&n, &n, dist, iseed, type__, &rwork[1], &mode, &cndnum, &
anorm, &kl, &ku, "No packing", &a[1], &lda, &work[1], &
info);
/* Check error code from SLATMS. */
if (info != 0) {
alaerh_(path, "SLATMS", &info, &c__0, " ", &n, &n, &c_n1, &
c_n1, &c_n1, &imat, &nfail, &nerrs, nout);
goto L80;
}
/* For types 5-7, zero one or more columns of the matrix to */
/* test that INFO is returned correctly. */
if (zerot) {
if (imat == 5) {
izero = 1;
} else if (imat == 6) {
izero = n;
} else {
izero = n / 2 + 1;
}
ioff = (izero - 1) * lda;
if (imat < 7) {
i__3 = n;
for (i__ = 1; i__ <= i__3; ++i__) {
a[ioff + i__] = 0.f;
/* L20: */
}
} else {
i__3 = n - izero + 1;
slaset_("Full", &n, &i__3, &c_b20, &c_b20, &a[ioff + 1], &
lda);
}
} else {
izero = 0;
}
/* Save a copy of the matrix A in ASAV. */
slacpy_("Full", &n, &n, &a[1], &lda, &asav[1], &lda);
for (iequed = 1; iequed <= 4; ++iequed) {
*(unsigned char *)equed = *(unsigned char *)&equeds[iequed -
1];
if (iequed == 1) {
nfact = 3;
} else {
nfact = 1;
}
i__3 = nfact;
for (ifact = 1; ifact <= i__3; ++ifact) {
*(unsigned char *)fact = *(unsigned char *)&facts[ifact -
1];
prefac = lsame_(fact, "F");
nofact = lsame_(fact, "N");
equil = lsame_(fact, "E");
if (zerot) {
if (prefac) {
goto L60;
}
rcondo = 0.f;
rcondi = 0.f;
} else if (! nofact) {
/* Compute the condition number for comparison with */
/* the value returned by SGESVX (FACT = 'N' reuses */
/* the condition number from the previous iteration */
/* with FACT = 'F'). */
slacpy_("Full", &n, &n, &asav[1], &lda, &afac[1], &
lda);
if (equil || iequed > 1) {
/* Compute row and column scale factors to */
/* equilibrate the matrix A. */
sgeequ_(&n, &n, &afac[1], &lda, &s[1], &s[n + 1],
&rowcnd, &colcnd, &amax, &info);
if (info == 0 && n > 0) {
if (lsame_(equed, "R"))
{
rowcnd = 0.f;
colcnd = 1.f;
} else if (lsame_(equed, "C")) {
rowcnd = 1.f;
colcnd = 0.f;
} else if (lsame_(equed, "B")) {
rowcnd = 0.f;
colcnd = 0.f;
}
/* Equilibrate the matrix. */
slaqge_(&n, &n, &afac[1], &lda, &s[1], &s[n +
1], &rowcnd, &colcnd, &amax, equed);
}
}
/* Save the condition number of the non-equilibrated */
/* system for use in SGET04. */
if (equil) {
roldo = rcondo;
roldi = rcondi;
}
/* Compute the 1-norm and infinity-norm of A. */
anormo = slange_("1", &n, &n, &afac[1], &lda, &rwork[
1]);
anormi = slange_("I", &n, &n, &afac[1], &lda, &rwork[
1]);
/* Factor the matrix A. */
sgetrf_(&n, &n, &afac[1], &lda, &iwork[1], &info);
/* Form the inverse of A. */
slacpy_("Full", &n, &n, &afac[1], &lda, &a[1], &lda);
lwork = *nmax * max(3,*nrhs);
sgetri_(&n, &a[1], &lda, &iwork[1], &work[1], &lwork,
&info);
/* Compute the 1-norm condition number of A. */
ainvnm = slange_("1", &n, &n, &a[1], &lda, &rwork[1]);
if (anormo <= 0.f || ainvnm <= 0.f) {
rcondo = 1.f;
} else {
rcondo = 1.f / anormo / ainvnm;
}
/* Compute the infinity-norm condition number of A. */
ainvnm = slange_("I", &n, &n, &a[1], &lda, &rwork[1]);
if (anormi <= 0.f || ainvnm <= 0.f) {
rcondi = 1.f;
} else {
rcondi = 1.f / anormi / ainvnm;
}
}
for (itran = 1; itran <= 3; ++itran) {
/* Do for each value of TRANS. */
*(unsigned char *)trans = *(unsigned char *)&transs[
itran - 1];
if (itran == 1) {
rcondc = rcondo;
} else {
rcondc = rcondi;
}
/* Restore the matrix A. */
slacpy_("Full", &n, &n, &asav[1], &lda, &a[1], &lda);
/* Form an exact solution and set the right hand side. */
s_copy(srnamc_1.srnamt, "SLARHS", (ftnlen)6, (ftnlen)
6);
slarhs_(path, xtype, "Full", trans, &n, &n, &kl, &ku,
nrhs, &a[1], &lda, &xact[1], &lda, &b[1], &
lda, iseed, &info);
*(unsigned char *)xtype = 'C';
slacpy_("Full", &n, nrhs, &b[1], &lda, &bsav[1], &lda);
if (nofact && itran == 1) {
/* --- Test SGESV --- */
/* Compute the LU factorization of the matrix and */
/* solve the system. */
slacpy_("Full", &n, &n, &a[1], &lda, &afac[1], &
lda);
slacpy_("Full", &n, nrhs, &b[1], &lda, &x[1], &
lda);
s_copy(srnamc_1.srnamt, "SGESV ", (ftnlen)6, (
ftnlen)6);
sgesv_(&n, nrhs, &afac[1], &lda, &iwork[1], &x[1],
&lda, &info);
/* Check error code from SGESV . */
if (info != izero) {
alaerh_(path, "SGESV ", &info, &izero, " ", &
n, &n, &c_n1, &c_n1, nrhs, &imat, &
nfail, &nerrs, nout);
}
/* Reconstruct matrix from factors and compute */
/* residual. */
sget01_(&n, &n, &a[1], &lda, &afac[1], &lda, &
iwork[1], &rwork[1], result);
nt = 1;
if (izero == 0) {
/* Compute residual of the computed solution. */
slacpy_("Full", &n, nrhs, &b[1], &lda, &work[
1], &lda);
sget02_("No transpose", &n, &n, nrhs, &a[1], &
lda, &x[1], &lda, &work[1], &lda, &
rwork[1], &result[1]);
/* Check solution from generated exact solution. */
sget04_(&n, nrhs, &x[1], &lda, &xact[1], &lda,
&rcondc, &result[2]);
nt = 3;
}
/* Print information about the tests that did not */
/* pass the threshold. */
i__4 = nt;
for (k = 1; k <= i__4; ++k) {
if (result[k - 1] >= *thresh) {
if (nfail == 0 && nerrs == 0) {
aladhd_(nout, path);
}
io___55.ciunit = *nout;
s_wsfe(&io___55);
do_fio(&c__1, "SGESV ", (ftnlen)6);
do_fio(&c__1, (char *)&n, (ftnlen)sizeof(
integer));
do_fio(&c__1, (char *)&imat, (ftnlen)
sizeof(integer));
do_fio(&c__1, (char *)&k, (ftnlen)sizeof(
integer));
do_fio(&c__1, (char *)&result[k - 1], (
ftnlen)sizeof(real));
e_wsfe();
++nfail;
}
/* L30: */
}
nrun += nt;
}
/* --- Test SGESVX --- */
if (! prefac) {
slaset_("Full", &n, &n, &c_b20, &c_b20, &afac[1],
&lda);
}
slaset_("Full", &n, nrhs, &c_b20, &c_b20, &x[1], &lda);
if (iequed > 1 && n > 0) {
/* Equilibrate the matrix if FACT = 'F' and */
/* EQUED = 'R', 'C', or 'B'. */
slaqge_(&n, &n, &a[1], &lda, &s[1], &s[n + 1], &
rowcnd, &colcnd, &amax, equed);
}
/* Solve the system and compute the condition number */
/* and error bounds using SGESVX. */
s_copy(srnamc_1.srnamt, "SGESVX", (ftnlen)6, (ftnlen)
6);
sgesvx_(fact, trans, &n, nrhs, &a[1], &lda, &afac[1],
&lda, &iwork[1], equed, &s[1], &s[n + 1], &b[
1], &lda, &x[1], &lda, &rcond, &rwork[1], &
rwork[*nrhs + 1], &work[1], &iwork[n + 1], &
info);
/* Check the error code from SGESVX. */
if (info != izero) {
/* Writing concatenation */
i__5[0] = 1, a__1[0] = fact;
i__5[1] = 1, a__1[1] = trans;
s_cat(ch__1, a__1, i__5, &c__2, (ftnlen)2);
alaerh_(path, "SGESVX", &info, &izero, ch__1, &n,
&n, &c_n1, &c_n1, nrhs, &imat, &nfail, &
nerrs, nout);
}
/* Compare WORK(1) from SGESVX with the computed */
/* reciprocal pivot growth factor RPVGRW */
if (info != 0) {
rpvgrw = slantr_("M", "U", "N", &info, &info, &
afac[1], &lda, &work[1]);
if (rpvgrw == 0.f) {
rpvgrw = 1.f;
} else {
rpvgrw = slange_("M", &n, &info, &a[1], &lda,
&work[1]) / rpvgrw;
}
} else {
rpvgrw = slantr_("M", "U", "N", &n, &n, &afac[1],
&lda, &work[1]);
if (rpvgrw == 0.f) {
rpvgrw = 1.f;
} else {
rpvgrw = slange_("M", &n, &n, &a[1], &lda, &
work[1]) / rpvgrw;
}
}
result[6] = (r__1 = rpvgrw - work[1], dabs(r__1)) /
dmax(work[1],rpvgrw) / slamch_("E")
;
if (! prefac) {
/* Reconstruct matrix from factors and compute */
/* residual. */
sget01_(&n, &n, &a[1], &lda, &afac[1], &lda, &
iwork[1], &rwork[(*nrhs << 1) + 1],
result);
k1 = 1;
} else {
k1 = 2;
}
if (info == 0) {
trfcon = FALSE_;
/* Compute residual of the computed solution. */
slacpy_("Full", &n, nrhs, &bsav[1], &lda, &work[1]
, &lda);
sget02_(trans, &n, &n, nrhs, &asav[1], &lda, &x[1]
, &lda, &work[1], &lda, &rwork[(*nrhs <<
1) + 1], &result[1]);
/* Check solution from generated exact solution. */
if (nofact || prefac && lsame_(equed, "N")) {
sget04_(&n, nrhs, &x[1], &lda, &xact[1], &lda,
&rcondc, &result[2]);
} else {
if (itran == 1) {
roldc = roldo;
} else {
roldc = roldi;
}
sget04_(&n, nrhs, &x[1], &lda, &xact[1], &lda,
&roldc, &result[2]);
}
/* Check the error bounds from iterative */
/* refinement. */
sget07_(trans, &n, nrhs, &asav[1], &lda, &b[1], &
lda, &x[1], &lda, &xact[1], &lda, &rwork[
1], &rwork[*nrhs + 1], &result[3]);
} else {
trfcon = TRUE_;
}
/* Compare RCOND from SGESVX with the computed value */
/* in RCONDC. */
result[5] = sget06_(&rcond, &rcondc);
/* Print information about the tests that did not pass */
/* the threshold. */
if (! trfcon) {
for (k = k1; k <= 7; ++k) {
if (result[k - 1] >= *thresh) {
if (nfail == 0 && nerrs == 0) {
aladhd_(nout, path);
}
if (prefac) {
io___61.ciunit = *nout;
s_wsfe(&io___61);
do_fio(&c__1, "SGESVX", (ftnlen)6);
do_fio(&c__1, fact, (ftnlen)1);
do_fio(&c__1, trans, (ftnlen)1);
do_fio(&c__1, (char *)&n, (ftnlen)
sizeof(integer));
do_fio(&c__1, equed, (ftnlen)1);
do_fio(&c__1, (char *)&imat, (ftnlen)
sizeof(integer));
do_fio(&c__1, (char *)&k, (ftnlen)
sizeof(integer));
do_fio(&c__1, (char *)&result[k - 1],
(ftnlen)sizeof(real));
e_wsfe();
} else {
io___62.ciunit = *nout;
s_wsfe(&io___62);
do_fio(&c__1, "SGESVX", (ftnlen)6);
do_fio(&c__1, fact, (ftnlen)1);
do_fio(&c__1, trans, (ftnlen)1);
do_fio(&c__1, (char *)&n, (ftnlen)
sizeof(integer));
do_fio(&c__1, (char *)&imat, (ftnlen)
sizeof(integer));
do_fio(&c__1, (char *)&k, (ftnlen)
sizeof(integer));
do_fio(&c__1, (char *)&result[k - 1],
(ftnlen)sizeof(real));
e_wsfe();
}
++nfail;
}
/* L40: */
}
nrun = nrun + 7 - k1;
} else {
if (result[0] >= *thresh && ! prefac) {
if (nfail == 0 && nerrs == 0) {
aladhd_(nout, path);
}
if (prefac) {
io___63.ciunit = *nout;
s_wsfe(&io___63);
do_fio(&c__1, "SGESVX", (ftnlen)6);
do_fio(&c__1, fact, (ftnlen)1);
do_fio(&c__1, trans, (ftnlen)1);
do_fio(&c__1, (char *)&n, (ftnlen)sizeof(
integer));
do_fio(&c__1, equed, (ftnlen)1);
do_fio(&c__1, (char *)&imat, (ftnlen)
sizeof(integer));
do_fio(&c__1, (char *)&c__1, (ftnlen)
sizeof(integer));
do_fio(&c__1, (char *)&result[0], (ftnlen)
sizeof(real));
e_wsfe();
} else {
io___64.ciunit = *nout;
s_wsfe(&io___64);
do_fio(&c__1, "SGESVX", (ftnlen)6);
do_fio(&c__1, fact, (ftnlen)1);
do_fio(&c__1, trans, (ftnlen)1);
do_fio(&c__1, (char *)&n, (ftnlen)sizeof(
integer));
do_fio(&c__1, (char *)&imat, (ftnlen)
sizeof(integer));
do_fio(&c__1, (char *)&c__1, (ftnlen)
sizeof(integer));
do_fio(&c__1, (char *)&result[0], (ftnlen)
sizeof(real));
e_wsfe();
}
++nfail;
++nrun;
}
if (result[5] >= *thresh) {
if (nfail == 0 && nerrs == 0) {
aladhd_(nout, path);
}
if (prefac) {
io___65.ciunit = *nout;
s_wsfe(&io___65);
do_fio(&c__1, "SGESVX", (ftnlen)6);
do_fio(&c__1, fact, (ftnlen)1);
do_fio(&c__1, trans, (ftnlen)1);
do_fio(&c__1, (char *)&n, (ftnlen)sizeof(
integer));
do_fio(&c__1, equed, (ftnlen)1);
do_fio(&c__1, (char *)&imat, (ftnlen)
sizeof(integer));
do_fio(&c__1, (char *)&c__6, (ftnlen)
sizeof(integer));
do_fio(&c__1, (char *)&result[5], (ftnlen)
sizeof(real));
e_wsfe();
} else {
io___66.ciunit = *nout;
s_wsfe(&io___66);
do_fio(&c__1, "SGESVX", (ftnlen)6);
do_fio(&c__1, fact, (ftnlen)1);
do_fio(&c__1, trans, (ftnlen)1);
do_fio(&c__1, (char *)&n, (ftnlen)sizeof(
integer));
do_fio(&c__1, (char *)&imat, (ftnlen)
sizeof(integer));
do_fio(&c__1, (char *)&c__6, (ftnlen)
sizeof(integer));
do_fio(&c__1, (char *)&result[5], (ftnlen)
sizeof(real));
e_wsfe();
}
++nfail;
++nrun;
}
if (result[6] >= *thresh) {
if (nfail == 0 && nerrs == 0) {
aladhd_(nout, path);
}
if (prefac) {
io___67.ciunit = *nout;
s_wsfe(&io___67);
do_fio(&c__1, "SGESVX", (ftnlen)6);
do_fio(&c__1, fact, (ftnlen)1);
do_fio(&c__1, trans, (ftnlen)1);
do_fio(&c__1, (char *)&n, (ftnlen)sizeof(
integer));
do_fio(&c__1, equed, (ftnlen)1);
do_fio(&c__1, (char *)&imat, (ftnlen)
sizeof(integer));
do_fio(&c__1, (char *)&c__7, (ftnlen)
sizeof(integer));
do_fio(&c__1, (char *)&result[6], (ftnlen)
sizeof(real));
e_wsfe();
} else {
io___68.ciunit = *nout;
s_wsfe(&io___68);
do_fio(&c__1, "SGESVX", (ftnlen)6);
do_fio(&c__1, fact, (ftnlen)1);
do_fio(&c__1, trans, (ftnlen)1);
do_fio(&c__1, (char *)&n, (ftnlen)sizeof(
integer));
do_fio(&c__1, (char *)&imat, (ftnlen)
sizeof(integer));
do_fio(&c__1, (char *)&c__7, (ftnlen)
sizeof(integer));
do_fio(&c__1, (char *)&result[6], (ftnlen)
sizeof(real));
e_wsfe();
}
++nfail;
++nrun;
}
}
/* L50: */
}
L60:
;
}
/* L70: */
}
L80:
;
}
/* L90: */
}
/* Print a summary of the results. */
alasvm_(path, nout, &nfail, &nrun, &nerrs);
return 0;
/* End of SDRVGE */
} /* sdrvge_ */
template<> void lapackGesv<float>(
int n, int nrhs, float* a, int lda, int* ipiv,
float* b, int ldb, int* info) {
sgesv_(&n, &nrhs, a, &lda, ipiv, b, &ldb, info);
}
int _CRGetHomographyMatrix(float *matrix, CRCorner *p, int inputImgWidth, int inputImgHeight) {
float x1 = (p[0].x - inputImgWidth/2) / CRGetXFocalLength();
float y1 = (p[0].y - inputImgHeight/2) / CRGetYFocalLength();
float x2 = (p[1].x - inputImgWidth/2) / CRGetXFocalLength();
float y2 = (p[1].y - inputImgHeight/2) / CRGetYFocalLength();
float x3 = (p[2].x - inputImgWidth/2) / CRGetXFocalLength();
float y3 = (p[2].y - inputImgHeight/2) / CRGetYFocalLength();
float x4 = (p[3].x - inputImgWidth/2) / CRGetXFocalLength();
float y4 = (p[3].y - inputImgHeight/2) / CRGetYFocalLength();
#ifdef DEBUG_CODE_CALCULATION
printf("------------------------------------------->Corner points\n");
printf(" %d,%d,%d,%d\n", p[0]->x,p[1]->x,p[2]->x,p[3]->x);
printf(" %d,%d,%d,%d\n", p[0]->y,p[1]->y,p[2]->y,p[3]->y);
printf("------------------------------------------->Corner points(normalized)\n");
printf(" %f,%f,%f,%f\n", x1, x2, x3, x4);
printf(" %f,%f,%f,%f\n", y1, y2, y3, y4);
#endif
float a[64];
float *b = matrix;
float code = 0.5;
a[0] = -code; a[ 8] = code; a[16] = 1; a[24] = 0; a[32] = 0; a[40] = 0; a[48] = -x1 * (-code); a[56] = -x1 * ( code);
a[1] = 0; a[ 9] = 0; a[17] = 0; a[25] = -code; a[33] = code; a[41] = 1; a[49] = -y1 * (-code); a[57] = -y1 * ( code);
a[2] = code; a[10] = code; a[18] = 1; a[26] = 0; a[34] = 0; a[42] = 0; a[50] = -x2 * ( code); a[58] = -x2 * ( code);
a[3] = 0; a[11] = 0; a[19] = 0; a[27] = code; a[35] = code; a[43] = 1; a[51] = -y2 * ( code); a[59] = -y2 * ( code);
a[4] = code; a[12] = -code; a[20] = 1; a[28] = 0; a[36] = 0; a[44] = 0; a[52] = -x3 * ( code); a[60] = -x3 * (-code);
a[5] = 0; a[13] = 0; a[21] = 0; a[29] = code; a[37] = -code; a[45] = 1; a[53] = -y3 * ( code); a[61] = -y3 * (-code);
a[6] = -code; a[14] = -code; a[22] = 1; a[30] = 0; a[38] = 0; a[46] = 0; a[54] = -x4 * (-code); a[62] = -x4 * (-code);
a[7] = 0; a[15] = 0; a[23] = 0; a[31] = -code; a[39] = -code; a[47] = 1; a[55] = -y4 * (-code); a[63] = -y4 * (-code);
#ifdef DEBUG_CODE_CALCULATION
printf("------------------------------------------->Data matrix\n");
for (int i = 0; i < 8; i++)
printf(" %3.5f,%3.5f, %3.5f, %3.5f,%3.5f,%3.5f, %3.5f, %3.5f\n", a[i], a[i+8], a[i+16], a[i+24], a[i+32], a[i+40], a[i+48], a[i+56]);
#endif
b[0] = x1;
b[1] = y1;
b[2] = x2;
b[3] = y2;
b[4] = x3;
b[5] = y3;
b[6] = x4;
b[7] = y4;
int rank = 8;
int nrhs = 1;
int pivot[8];
int info = 0;
sgesv_((__CLPK_integer*)&rank, (__CLPK_integer*)&nrhs, (__CLPK_real*)a, (__CLPK_integer*)&rank, (__CLPK_integer*)pivot,(__CLPK_real*)b, (__CLPK_integer*)&rank, (__CLPK_integer*)&info);
return (info == 0); // check result of sgesv_
}
