//calculate determinant of gaudin matrix for given Bethe rapidity set
double detgaudinnorm (double kEpshiftinv_gelesen[N+6])
{
double k[N];//Bethe rapidities
double determinante;
double c = kEpshiftinv_gelesen[0];//interaction strenght
for(int ll=0; ll<N; ll++)
k[ll]=kEpshiftinv_gelesen[ll+1];
//allocate and calculate gaudin matrix:
gsl_matrix * m = gsl_matrix_alloc (N, N);
for (int ii=0; ii<N; ii++)
for (int jj=0; jj<N; jj++)
gsl_matrix_set (m, ii, jj, matrixelementenorm(k, c, ii, jj));
//calculate determinant via LU decomposition
int sign_permutation;
gsl_permutation * p = gsl_permutation_alloc (N);
gsl_linalg_LU_decomp (m, p, &sign_permutation);
determinante = gsl_linalg_LU_det (m, sign_permutation);
gsl_permutation_free (p);
gsl_matrix_free (m);
//end LU decomposition and det calc
return determinante;
}
/** *************************************************************************************************************************/
double compute_mlik_pois_marg_brent(double finitestepsize, void *params)
{
struct fnparams *gparams = ((struct fnparams *) params);
gsl_vector *myBeta=gparams->betastatic;
int n=gparams->nDim;
int m=gparams->mDim;
gsl_permutation *perm=gparams->perm;
gsl_matrix *hessgvalues=gparams->mattmp2;
gsl_matrix *hessgvalues3pt=gparams->mattmp3;
double gvalue=gparams->gvalue;
int status,sss;
double mydet;
double logscore,logscore3pt;
double error_val=0.0;
/** ***/
/*double finitestepsize=gsl_vector_get(finitestepsize_vec,0);*/
/** ***/
/*Rprintf("got h=%e n=%d m=%d gvalue=%e\n",finitestepsize,n,m,gvalue);*/
/*for(i=0;i<myBeta->size;i++){Rprintf("beta= %f ",gsl_vector_get(myBeta,i));}Rprintf("\n");*/
rv_hessg_pois_outer_marg(myBeta,gparams, hessgvalues,finitestepsize,hessgvalues3pt);/** start with LARGEST STEPSIZE **/
/*for(i=0;i<hessgvalues3pt->size1;i++){for(j=0;j<hessgvalues3pt->size2;j++){Rprintf("%e ",gsl_matrix_get(hessgvalues3pt,i,j));}Rprintf("\n");}*/
status=gsl_linalg_LU_decomp(hessgvalues,perm,&sss);
mydet=gsl_linalg_LU_lndet(hessgvalues);/** compute determinant but this might be a nan - overflow? gsl_linalg_LU_lndet*/
logscore= -n*gvalue-0.5*mydet+(m/2.0)*log((2.0*M_PI)/n);/** this is the final value */
status=gsl_linalg_LU_decomp(hessgvalues3pt,perm,&sss);
mydet=gsl_linalg_LU_lndet(hessgvalues3pt);/** compute determinant but this might be a nan - overflow? gsl_linalg_LU_lndet*/
logscore3pt= -n*gvalue-0.5*mydet+(m/2.0)*log((2.0*M_PI)/n);/** this is the final value */
error_val=fabs(logscore-logscore3pt);
/*Rprintf("error_val=%e\n",error_val);*/
/*gparams->logscore=logscore;
gparams->logscore3pt=logscore3pt;*/
/*Rprintf("logscore=%e logscore3pt=%e\n",logscore,logscore3pt);*/
if(gsl_isnan(error_val) || gsl_isinf(error_val)){return(DBL_MAX);/*error("Non-finite value in mlik error estimation");*/}
return(error_val);
}
void matrix_inv(gsl_matrix *input, gsl_matrix *output){
int s;
gsl_permutation * p = gsl_permutation_alloc (input->size1);
gsl_linalg_LU_decomp (input, p, &s);
gsl_linalg_LU_invert (input, p, output);
gsl_permutation_free(p);
}
CAMLprim value ml_gsl_linalg_LU_decomp(value A, value P)
{
int sign;
GSL_PERMUT_OF_BIGARRAY(P);
_DECLARE_MATRIX(A);
_CONVERT_MATRIX(A);
gsl_linalg_LU_decomp(&m_A, &perm_P, &sign);
return Val_int(sign);
}
void StrainForce::setup() {
size_t size = nodes.size();
if (size < 2)
return;
matA = gsl_matrix_alloc(size-1, size-1);
for (int i = 0; i < 3; ++i) {
vecX[i] = gsl_vector_alloc(size-1);
vecB[i] = gsl_vector_alloc(size-1);
}
permutation = gsl_permutation_alloc(size-1);
gsl_matrix_set_all(matA, 1.0);
if (auto sys = system.lock()) {
auto avgpos = glm::dvec3(0.0);
double minDistance = DBL_MAX;
for (size_t i = 0; i < nodes.size(); ++i) {
auto node = sys->getNode(nodes[i]);
avgpos += node->getPosition();
}
avgpos /= size;
for (size_t i = 0; i < size; ++i) {
auto node = sys->getNode(nodes[i]);
double distance = glm::length(node->getPosition() - avgpos);
if (distance < minDistance) {
minDistance = distance;
mainId = nodes[i];
}
}
size_t k = 0;
otherIds = new size_t[size-1];
for (size_t i = 0; i < size; ++i) {
if (nodes[i] == mainId)
continue;
otherIds[k++] = nodes[i];
}
auto mainNode = sys->getNode(mainId);
for (size_t i = 0; i < size-1; ++i) {
auto node = sys->getNode(otherIds[i]);
gsl_matrix_set(matA, i, i, gsl_matrix_get(matA, i, i) + mainNode->getMass() / node->getMass());
}
int signum;
gsl_linalg_LU_decomp(matA, permutation, &signum);
signum *= 1;
}
}
int tGSLSolve::solveLU(void)
{
int s;
gsl_permutation * p = gsl_permutation_alloc (N);
gsl_linalg_LU_decomp(MATRIX, p, &s);
for (int i=0;i<RHS.count() && i<x.count(); i++){
gsl_linalg_LU_solve (MATRIX, p, RHS.at(i), x.at(i));
}
return s;
}
void mcmclib_matrix_inverse(gsl_matrix* A) {
gsl_permutation* p = gsl_permutation_alloc(A->size1);
gsl_matrix* A1 = gsl_matrix_alloc(A->size1, A->size1);
int tmp=0;
gsl_matrix_memcpy(A1, A);
gsl_linalg_LU_decomp(A1, p, &tmp);
gsl_linalg_LU_invert(A1, p, A);
gsl_matrix_free(A1);
gsl_permutation_free(p);
}
void QgsLeastSquares::helmert( std::vector<QgsPoint> mapCoords,
std::vector<QgsPoint> pixelCoords,
QgsPoint& origin, double& pixelSize,
double& rotation )
{
int n = mapCoords.size();
if ( n < 2 )
{
throw std::domain_error( QObject::tr( "Fit to a Helmert transform requires at least 2 points." ).toLocal8Bit().constData() );
}
double A = 0, B = 0, C = 0, D = 0, E = 0, F = 0, G = 0, H = 0, I = 0, J = 0;
for ( int i = 0; i < n; ++i )
{
A += pixelCoords[i].x();
B += pixelCoords[i].y();
C += mapCoords[i].x();
D += mapCoords[i].y();
E += mapCoords[i].x() * pixelCoords[i].x();
F += mapCoords[i].y() * pixelCoords[i].y();
G += std::pow( pixelCoords[i].x(), 2 );
H += std::pow( pixelCoords[i].y(), 2 );
I += mapCoords[i].x() * pixelCoords[i].y();
J += pixelCoords[i].x() * mapCoords[i].y();
}
/* The least squares fit for the parameters { a, b, x0, y0 } is the solution
to the matrix equation Mx = b, where M and b is given below. I *think*
that this is correct but I derived it myself late at night. Look at
helmert.jpg if you suspect bugs. */
double MData[] = { A, -B, n, 0,
B, A, 0, n,
G + H, 0, A, B,
0, G + H, -B, A
};
double bData[] = { C, D, E + F, J - I };
// we want to solve the equation M*x = b, where x = [a b x0 y0]
gsl_matrix_view M = gsl_matrix_view_array( MData, 4, 4 );
gsl_vector_view b = gsl_vector_view_array( bData, 4 );
gsl_vector* x = gsl_vector_alloc( 4 );
gsl_permutation* p = gsl_permutation_alloc( 4 );
int s;
gsl_linalg_LU_decomp( &M.matrix, p, &s );
gsl_linalg_LU_solve( &M.matrix, p, &b.vector, x );
gsl_permutation_free( p );
origin.setX( gsl_vector_get( x, 2 ) );
origin.setY( gsl_vector_get( x, 3 ) );
pixelSize = std::sqrt( std::pow( gsl_vector_get( x, 0 ), 2 ) +
std::pow( gsl_vector_get( x, 1 ), 2 ) );
rotation = std::atan2( gsl_vector_get( x, 1 ), gsl_vector_get( x, 0 ) );
}
void xmi_inverse_matrix(double x[3], double y[3], double z[3], double **inverseF) {
gsl_matrix *m = gsl_matrix_alloc(3,3);
gsl_matrix *inverse = gsl_matrix_alloc(3,3);
gsl_permutation *p = gsl_permutation_calloc(3);
int signum;
double *rv;
int i,j,k;
gsl_matrix_set(m,0,0, x[0]);
gsl_matrix_set(m,1,0, x[1]);
gsl_matrix_set(m,2,0, x[2]);
gsl_matrix_set(m,0,1, y[0]);
gsl_matrix_set(m,1,1, y[1]);
gsl_matrix_set(m,2,1, y[2]);
gsl_matrix_set(m,0,2, z[0]);
gsl_matrix_set(m,1,2, z[1]);
gsl_matrix_set(m,2,2, z[2]);
#if DEBUG == 2
fprintf(stdout,"input matrix\n");
gsl_matrix_fprintf(stdout,m , "%g");
#endif
//invert the sucker
gsl_linalg_LU_decomp(m,p,&signum);
gsl_linalg_LU_invert(m,p,inverse);
#if DEBUG == 2
fprintf(stdout,"inverted matrix\n");
gsl_matrix_fprintf(stdout,inverse , "%g");
#endif
gsl_matrix_transpose(inverse);
#if DEBUG == 2
fprintf(stdout,"transposed inverted matrix\n");
gsl_matrix_fprintf(stdout,inverse , "%g");
#endif
rv = (double *) malloc(9*sizeof(double));
k = 0;
for (i = 0 ; i < 3 ; i++)
for (j = 0 ; j < 3 ; j++)
rv[k++] = gsl_matrix_get(inverse, i,j);
gsl_matrix_free(m);
gsl_matrix_free(inverse);
gsl_permutation_free(p);
*inverseF = rv;
}
static int
modnewton1_init (void *vstate, const gsl_matrix * A,
const double h, const gsl_matrix * dfdy,
const gsl_odeiv2_system * sys)
{
/* Initializes the method by forming the iteration matrix IhAJ
and generating its LU-decomposition
*/
modnewton1_state_t *state = (modnewton1_state_t *) vstate;
gsl_matrix *const IhAJ = state->IhAJ;
gsl_permutation *const p = state->p;
const size_t dim = sys->dimension;
const size_t stage = A->size1;
state->eeta_prev = GSL_DBL_MAX;
/* Generate IhAJ */
{
size_t i, j, k, m;
for (i = 0; i < dim; i++)
for (j = 0; j < dim; j++)
for (k = 0; k < stage; k++)
for (m = 0; m < stage; m++)
{
size_t x = dim * k + i;
size_t y = dim * m + j;
if (x != y)
gsl_matrix_set (IhAJ, x, y,
-h * gsl_matrix_get (A, k, m) *
gsl_matrix_get (dfdy, i, j));
else
gsl_matrix_set (IhAJ, x, y,
1.0 - h * gsl_matrix_get (A, k, m) *
gsl_matrix_get (dfdy, i, j));
}
}
/* decompose */
{
int signum;
int s = gsl_linalg_LU_decomp (IhAJ, p, &signum);
if (s != GSL_SUCCESS)
return s;
}
return GSL_SUCCESS;
}
int update_circuit(Circuit* c)
{
gsl_matrix* A;
gsl_vector* b;
to_matrix(c,&A,&b);
if(A==NULL){
return -1;
}
gsl_permutation * p=gsl_permutation_alloc(b->size);
gsl_vector* x =gsl_vector_alloc(b->size);
if(p==NULL||x==NULL){
printf("unable to allocate memory (update_circuit()), freeing and halting\n");
gsl_matrix_free(A);
gsl_vector_free(b);
if(p!=NULL){
gsl_permutation_free(p);
}
if(x!=NULL){
gsl_vector_free(x);
}
return -1;
}
int i,j;
for(i=0;i<A->size1;i++){
printf("[ ");
for(j=0;j<A->size2;j++)
{
printf("%6.3g ",gsl_matrix_get(A,i,j));
}
printf("] [ %6.3g ]\n",gsl_vector_get(b,i));
}
int s;
gsl_linalg_LU_decomp(A,p,&s);
double det=gsl_linalg_LU_det(A,s);
printf("\ndeterminant is %g\n",det);
if(det==0.0||(det<0x1p-16&&det>-0x1p-16)){
printf("ERROR, NON-TRIVIAL SOLUTION\nFREEING MEMORY AND HALTING COMPUTATION\n...");
gsl_vector_free(x);
gsl_vector_free(b);
gsl_matrix_free(A);
gsl_permutation_free(p);
printf("DONE\n");
return -1;
}
gsl_linalg_LU_solve(A,p,b,x);
printf("\nthe solution is:\n");
print_vector(x);
to_circuit(x,c);
gsl_vector_free(x);
gsl_matrix_free(A);
gsl_vector_free(b);
gsl_permutation_free(p);
return 0;
}
void mygsl_linalg_invert(const gsl_matrix * A, gsl_matrix * A_inv)
{
gsl_matrix * tmp = gsl_matrix_alloc(A->size1, A->size2);
gsl_matrix_memcpy(tmp, A);
gsl_permutation * perm = gsl_permutation_alloc(A->size1);
int signum;
gsl_linalg_LU_decomp(tmp, perm, &signum);
gsl_linalg_LU_invert(tmp, perm, A_inv);
gsl_matrix_free(tmp);
gsl_permutation_free(perm);
}
/*! \brief Discrete Cepstrum Transform
*
* method for computing cepstrum aenalysis from a discrete
* set of partial peaks (frequency and amplitude)
*
* This implementation is owed to the help of Jordi Janer (thanks!) from the MTG,
* along with the following paper:
* "Regularization Techniques for Discrete Cepstrum Estimation"
* Olivier Cappe and Eric Moulines, IEEE Signal Processing Letters, Vol. 3
* No.4, April 1996
*
* \todo add anchor point add at frequency = 0 with the same magnitude as the first
* peak in pMag. This does not change the size of the cepstrum, only helps to smoothen it
* at the very beginning.
*
* \param sizeCepstrum order+1 of the discrete cepstrum
* \param pCepstrum pointer to output array of cepstrum coefficients
* \param sizeFreq number of partials peaks (the size of pFreq should be the same as pMag
* \param pFreq pointer to partial peak frequencies (hertz)
* \param pMag pointer to partial peak magnitudes (linear)
* \param fLambda regularization factor
* \param iMaxFreq maximum frequency of cepstrum
*/
void sms_dCepstrum( int sizeCepstrum, sfloat *pCepstrum, int sizeFreq, sfloat *pFreq, sfloat *pMag,
sfloat fLambda, int iMaxFreq)
{
int i, k;
sfloat factor;
sfloat fNorm = PI / (float)iMaxFreq; /* value to normalize frequencies to 0:0.5 */
//static sizeCepstrumStatic
static CepstrumMatrices m;
//printf("nPoints: %d, nCoeff: %d \n", m.nPoints, m.nCoeff);
if(m.nPoints != sizeCepstrum || m.nCoeff != sizeFreq)
AllocateDCepstrum(sizeFreq, sizeCepstrum, &m);
int s; /* signum: "(-1)^n, where n is the number of interchanges in the permutation." */
/* compute matrix M (eq. 4)*/
for (i=0; i<sizeFreq; i++)
{
gsl_matrix_set (m.pM, i, 0, 1.); // first colum is all 1
for (k=1; k <sizeCepstrum; k++)
gsl_matrix_set (m.pM, i, k , 2.*sms_sine(PI_2 + fNorm * k * pFreq[i]) );
}
/* compute transpose of M */
gsl_matrix_transpose_memcpy (m.pMt, m.pM);
/* compute R diagonal matrix (for eq. 7)*/
factor = COEF * (fLambda / (1.-fLambda)); /* \todo why is this divided like this again? */
for (k=0; k<sizeCepstrum; k++)
gsl_matrix_set(m.pR, k, k, factor * powf((sfloat) k,2.));
/* MtM = Mt * M, later will add R */
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1., m.pMt, m.pM, 0.0, m.pMtMR);
/* add R to make MtMR */
gsl_matrix_add (m.pMtMR, m.pR);
/* set pMag in X and multiply with Mt to get pMtXk */
for(k = 0; k <sizeFreq; k++)
gsl_vector_set(m.pXk, k, log(pMag[k]));
gsl_blas_dgemv (CblasNoTrans, 1., m.pMt, m.pXk, 0., m.pMtXk);
/* solve x (the cepstrum) in Ax = b, where A=MtMR and b=pMtXk */
/* ==== the Cholesky Decomposition way ==== */
/* MtM is 'symmetric and positive definite?' */
//gsl_linalg_cholesky_decomp (m.pMtMR);
//gsl_linalg_cholesky_solve (m.pMtMR, m.pMtXk, m.pC);
/* ==== the LU decomposition way ==== */
gsl_linalg_LU_decomp (m.pMtMR, m.pPerm, &s);
gsl_linalg_LU_solve (m.pMtMR, m.pPerm, m.pMtXk, m.pC);
/* copy pC to pCepstrum */
for(i = 0; i < sizeCepstrum; i++)
pCepstrum[i] = gsl_vector_get (m.pC, i);
}
int invertirmatriz (gsl_matrix * m, gsl_matrix * inversa, int n)
{
int s;
gsl_permutation * perm = gsl_permutation_alloc (n);
gsl_linalg_LU_decomp (m, perm, &s);
gsl_linalg_LU_invert (m, perm, inversa);
return 0;
}
/*
void invert(valarray<double>& Mi,const valarray<double>& M){
int Dim = sqrt(double(M.size()));
vector<int> indx(Dim);
valarray<double> Mt = M;
valarray<double> vv(Dim);
//// LU decomposition
for(int i=0; i<Dim; ++i){
double big = 0.0;
for(int j=0; j<Dim; ++j) big = max(big,fabs(Mt[i*Dim+j]));
vv[i] = 1.0/big;
}
for(int j=0; j<Dim; ++j){
for(int i=0; i<j; ++i){
double sum = Mt[i*Dim+j];
for(int k=0; k<i; ++k) sum -= Mt[i*Dim+k]*Mt[k*Dim+j];
Mt[i*Dim+j] = sum;
}
double big=0.0;
int imax;
for(int i=j; i<Dim; ++i){
imax = j;
double sum = Mt[i*Dim+j];
for(int k=0; k<j; ++k) sum -= Mt[i*Dim+k]*Mt[k*Dim+j];
Mt[i*Dim+j] = sum;
if(double dum= vv[i]*fabs(sum) >= big){
big = dum;
imax = i;
}
}
if(j!=imax){
for(int k=0; k<Dim; ++k) swap(Mt[imax*Dim+k],Mt[j*Dim+k]);
vv[imax] = vv[j];
}
indx[j]= imax;
if(j !=Dim-1)
for(int i=j+1; i<Dim; ++i) Mt[i*Dim+j] /= Mt[j*Dim+j];
}
///////
for(int k=0; k<Dim; ++k){
for(int l=0; l<Dim; ++l)
CCIO::cout<<" LU["<<k<<","<<l<<"]="<<Mt[k*Dim+l];
CCIO::cout<<"\n";
}
//// forward/backward subtractions
for(int j=0; j<Dim; ++j){
vector<double> col(Dim,0.0);
col[j] = 1.0;
int ii = 0;
for(int i=0; i<Dim; ++i){
double sum = col[indx[i]];
col[indx[i]] = col[i];
if(ii)
for(int k=ii; k<i; ++k) sum -= Mt[i*Dim+k]*col[k];
else if(sum) ii = i;
col[i] = sum;
}
for(int i=Dim-1; i>=0; --i){
double sum = col[i];
for(int k=i+1; k<Dim; ++k) sum -= Mt[i*Dim+k]*col[k];
col[i] = sum/Mt[i*Dim+i];
}
for(int i=0; i<Dim; ++i) Mi[i*Dim+j] = col[i];
}
}
*/
void invert(valarray<double>& Mi,const valarray<double>& M){
int Dim = sqrt(double(M.size()));
valarray<double> Mt(M);
gsl_matrix_view m = gsl_matrix_view_array(&(Mt[0]),Dim,Dim);
gsl_matrix_view mi = gsl_matrix_view_array(&(Mi[0]),Dim,Dim);
gsl_permutation* p = gsl_permutation_alloc(Dim);
int sgn;
gsl_linalg_LU_decomp(&m.matrix,p,&sgn);
gsl_linalg_LU_invert(&m.matrix,p,&mi.matrix);
}
void MVEE::calculateOptimalValue()
{
gsl_matrix* LU=gsl_matrix_alloc(M->size1, M->size2);
gsl_matrix_memcpy(LU, M);
gsl_permutation* p = gsl_permutation_alloc(M->size1);
int signum = 0;
gsl_linalg_LU_decomp (LU, p, &signum);
optimalValue = log(gsl_linalg_LU_det(LU,signum));
gsl_matrix_free(LU);
gsl_permutation_free(p);
}
double matrix_determ(gsl_matrix *X)
{
int s;
int n = X->size1;
int m = X->size2;
gsl_matrix *a_copy = gsl_matrix_alloc(m, n);
gsl_matrix_memcpy(a_copy, X );
gsl_permutation *P = gsl_permutation_alloc(n);
gsl_linalg_LU_decomp(a_copy, P, &s);
double my_det = gsl_linalg_LU_det (a_copy, s);
return(my_det);
}
/* gsl_matrix_det function
returns the determinant of gsl_matrix mat
*/
double gsl_matrix_det(const gsl_matrix *mat) {
gsl_permutation *p = gsl_permutation_alloc(mat->size1);
gsl_matrix *LU = gsl_matrix_alloc(mat->size1, mat->size2);
int sign = 0;
double det = 0.0;
gsl_matrix_memcpy(LU, mat);
gsl_linalg_LU_decomp(LU, p, &sign);
det = gsl_linalg_LU_det(LU, sign);
gsl_matrix_free(LU);
gsl_permutation_free(p);
return det;
}
static void __attribute__((unused)) solve_linear_system
(gsl_matrix* a, gsl_vector* x, const gsl_vector* b)
{
gsl_permutation* const p = gsl_permutation_alloc(a->size1);
int s;
gsl_linalg_LU_decomp(a, p, &s);
gsl_linalg_LU_solve(a, p, b, x);
gsl_permutation_free(p);
}
gsl_matrix inv_matrix(gsl_matrix *X, gsl_matrix *inv)
{
int s;
int n = X->size1;
int m = X->size2;
gsl_matrix *a_copy = gsl_matrix_alloc(m, n);
gsl_matrix_memcpy( a_copy, X );
gsl_permutation *P = gsl_permutation_alloc(n);
gsl_linalg_LU_decomp(a_copy, P, &s);
gsl_linalg_LU_invert(a_copy, P, inv);
return(*inv);
}
/// Invert this matrix
void GSLMatrix::invert() {
if (size1() != size2()) {
throw std::runtime_error("Matrix inverse: the matrix must be square.");
}
size_t n = size1();
int s;
GSLMatrix LU(*this);
gsl_permutation *p = gsl_permutation_alloc(n);
gsl_linalg_LU_decomp(LU.gsl(), p, &s);
gsl_linalg_LU_invert(LU.gsl(), p, this->gsl());
gsl_permutation_free(p);
}
/* gsl_matrix_inverse function
inverts a gsl_matrix
*/
gsl_matrix *gsl_matrix_inverse(const gsl_matrix *src) {
int sign;
gsl_matrix *LU = gsl_matrix_alloc(src->size1, src->size2);
gsl_matrix *inv = gsl_matrix_calloc(src->size1, src->size2);
gsl_permutation *p = gsl_permutation_alloc(src->size1);
gsl_matrix_memcpy(LU, src);
gsl_linalg_LU_decomp(LU, p, &sign);
gsl_linalg_LU_invert(LU, p, inv);
gsl_permutation_free(p);
gsl_matrix_free(LU);
return inv;
}
void fred2_solver(double a,
double b,
gsl_vector* t,
gsl_vector* f,
gsl_vector* w)
{
gsl_integration_glfixed_table* glgrid = gsl_integration_glfixed_table_alloc(MAX);
gsl_permutation* p = gsl_permutation_alloc(MAX);
gsl_matrix* lhs = gsl_matrix_alloc(MAX, MAX);
gsl_matrix* ktilde = gsl_matrix_alloc(MAX, MAX);
gsl_matrix* ak = gsl_matrix_alloc(MAX, MAX);
gsl_vector* g = gsl_vector_alloc(MAX);
int i, j, error, s;
double ptsi, wghtsi;
// set Gauss-Legendre integration points and weights
for (i = 0; i < MAX; i++)
{
error = gsl_integration_glfixed_point(a, b, i, &ptsi, &wghtsi, glgrid);
gsl_vector_set(t, i, ptsi);
gsl_vector_set(w, i, wghtsi);
}
kernel(ak, t, t);
aux_g(g, t);
// fill in unit matrix first
gsl_matrix_set_identity(lhs);
for (i = 0; i < MAX; i++)
{
for (j = 0; j < MAX; j++)
{
gsl_matrix_set(ktilde, i, j, gsl_matrix_get(ak, i, j) * gsl_vector_get(w, j));
}
}
// set up LHS matrix
error = gsl_matrix_sub(lhs, ktilde);
gsl_linalg_LU_decomp(lhs, p, &s);
gsl_linalg_LU_solve(lhs, p, g, f);
gsl_integration_glfixed_table_free(glgrid);
gsl_permutation_free(p);
gsl_matrix_free(lhs);
gsl_matrix_free(ktilde);
gsl_vector_free(g);
gsl_matrix_free(ak);
return;
}
double gaussian_pdf_log(const gaussian_t* dist,
const gsl_vector* x) {
double r = 0.0;
double logdet = 0.0;
if (gaussian_isdiagonal(dist)) {
size_t i;
double dx, dd;
for (i = 0; i < dist->dim; i++) {
dx = gsl_vector_get(x, i) - gsl_vector_get(dist->mean, i);
dd = gsl_vector_get(dist->diag, i);
r += dx * dx / dd;
logdet += DEBUG_LOG(dd);
}
}
else {
int signum;
gsl_vector* w1 = gsl_vector_alloc(dist->dim);
gsl_vector* w2 = gsl_vector_alloc(dist->dim);
gsl_vector_memcpy(w1, x);
gsl_vector_sub(w1, dist->mean);
gsl_matrix* v = gsl_matrix_alloc(dist->dim, dist->dim);
gsl_matrix_memcpy(v, dist->cov);
gsl_permutation* p = gsl_permutation_alloc(dist->dim);
gsl_linalg_LU_decomp(v, p, &signum);
gsl_linalg_LU_solve(v, p, w1, w2);
gsl_blas_ddot(w1, w2, &r);
logdet = gsl_linalg_LU_lndet(v);
assert(gsl_linalg_LU_sgndet(v, signum) == 1.0);
gsl_vector_free(w1);
gsl_vector_free(w2);
gsl_matrix_free(v);
gsl_permutation_free(p);
}
/* Use log to avoid underflow !
here
r = (x - mean)^T * cov^-1 * (x - mean)
logdet = log(det(cov))
then
logpdf = -.5 * (k * log(2*pi) + logdet + r);
*/
r = r + dist->dim * DEBUG_LOG(2 * M_PI) + logdet;
r = -0.5 * r;
assert(!isnan(r));
return r;
}
void QgsLeastSquares::affine( std::vector<QgsPoint> mapCoords,
std::vector<QgsPoint> pixelCoords )
{
int n = mapCoords.size();
if ( n < 4 )
{
throw std::domain_error( QObject::tr( "Fit to an affine transform requires at least 4 points." ).toLocal8Bit().constData() );
}
double A = 0, B = 0, C = 0, D = 0, E = 0, F = 0,
G = 0, H = 0, I = 0, J = 0, K = 0;
for ( int i = 0; i < n; ++i )
{
A += pixelCoords[i].x();
B += pixelCoords[i].y();
C += mapCoords[i].x();
D += mapCoords[i].y();
E += std::pow( pixelCoords[i].x(), 2 );
F += std::pow( pixelCoords[i].y(), 2 );
G += pixelCoords[i].x() * pixelCoords[i].y();
H += pixelCoords[i].x() * mapCoords[i].x();
I += pixelCoords[i].y() * mapCoords[i].y();
J += pixelCoords[i].x() * mapCoords[i].y();
K += mapCoords[i].x() * pixelCoords[i].y();
}
/* The least squares fit for the parameters { a, b, c, d, x0, y0 } is the
solution to the matrix equation Mx = b, where M and b is given below.
I *think* that this is correct but I derived it myself late at night.
Look at affine.jpg if you suspect bugs. */
double MData[] = { A, B, 0, 0, n, 0,
0, 0, A, B, 0, n,
E, G, 0, 0, A, 0,
G, F, 0, 0, B, 0,
0, 0, E, G, 0, A,
0, 0, G, F, 0, B
};
double bData[] = { C, D, H, K, J, I };
// we want to solve the equation M*x = b, where x = [a b c d x0 y0]
gsl_matrix_view M = gsl_matrix_view_array( MData, 6, 6 );
gsl_vector_view b = gsl_vector_view_array( bData, 6 );
gsl_vector* x = gsl_vector_alloc( 6 );
gsl_permutation* p = gsl_permutation_alloc( 6 );
int s;
gsl_linalg_LU_decomp( &M.matrix, p, &s );
gsl_linalg_LU_solve( &M.matrix, p, &b.vector, x );
gsl_permutation_free( p );
}
/// Calculate the determinant
double GSLMatrix::det() {
if (size1() != size2()) {
throw std::runtime_error("Matrix inverse: the matrix must be square.");
}
size_t n = size1();
int s;
GSLMatrix LU(*this);
gsl_permutation *p = gsl_permutation_alloc(n);
gsl_linalg_LU_decomp(LU.gsl(), p, &s);
double res = gsl_linalg_LU_det(LU.gsl(), s);
gsl_permutation_free(p);
return res;
}
double mygsl_linalg_det(const gsl_matrix * A)
{
double det = NaN;
gsl_matrix * tmp = gsl_matrix_alloc(A->size1, A->size2);
gsl_matrix_memcpy(tmp, A);
gsl_permutation * perm = gsl_permutation_alloc(A->size1);
int signum;
gsl_linalg_LU_decomp(tmp, perm, &signum);
gsl_linalg_LU_lndet(tmp);
gsl_matrix_free(tmp);
gsl_permutation_free(perm);
return det;
}
double ran_wishart_pdf(const gsl_matrix *X, const double nu, const gsl_matrix *V)
{
const int k = X->size1;
double detX, detV, den, temp;
int s, i;
gsl_matrix *work_k_k = gsl_matrix_alloc(k, k);
gsl_matrix *Vinv = gsl_matrix_alloc(k, k);
gsl_permutation *p = gsl_permutation_alloc(k);
gsl_matrix_memcpy(work_k_k, X);
gsl_linalg_LU_decomp(work_k_k, p, &s);
detX = gsl_linalg_LU_det(work_k_k, s);
gsl_matrix_memcpy(work_k_k, V);
gsl_linalg_LU_decomp(work_k_k, p, &s);
detV = gsl_linalg_LU_det(work_k_k, s);
gsl_linalg_LU_invert(work_k_k, p, Vinv);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Vinv, X, 0.0, work_k_k);
den = 0;
for (i=0; i<k; i++)
{
den -= 0.5 * gsl_matrix_get(work_k_k, i, i);
}
//den = exp(den);
temp = 0.5*(nu-k-1)*log(detX) - 0.5*nu*k*log(2) -0.5*nu*log(detV);
temp -= sf_mv_lngamma(nu/2, k);
den += temp;
den = exp(den);
gsl_matrix_free(work_k_k);
gsl_matrix_free(Vinv);
gsl_permutation_free(p);
return den;
}
double
Matrix::detMatrix(gsl_matrix* ludecomp, gsl_permutation* p)
{
ludecomp->size1=row;
ludecomp->size2=col;
p->size=row;
gsl_matrix_memcpy(ludecomp, matrix);
int signum=0;
gsl_linalg_LU_decomp(ludecomp, p, &signum);
double det=gsl_linalg_LU_det(ludecomp, signum);
return det;
}
double ldmvnorm(const int n, const gsl_vector *x, const gsl_vector *mean, const gsl_matrix *var){
/* log-multivariate normal density function */
/*
* n dimension of the random vetor
* mean vector of means of size n
* var variance matrix of dimension n x n
*/
int s;
double ax,ay;
gsl_vector *ym, *xm;
gsl_matrix *work = gsl_matrix_alloc(n,n),
*winv = gsl_matrix_alloc(n,n);
#ifdef PRINTSTIFF
/* Print Stiffness indicator S=max(eigen)/min(eigen)*/
gsl_vector *eval = gsl_vector_alloc (n);
gsl_matrix *evec = gsl_matrix_alloc (n, n);
gsl_matrix_memcpy(work,var);
gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc(n);
gsl_eigen_symmv (work, eval, evec, w);
gsl_eigen_symmv_free (w);
gsl_eigen_symmv_sort (eval, evec,
GSL_EIGEN_SORT_ABS_ASC);
printf("%f ",
gsl_vector_get(eval,n-1) / gsl_vector_get(eval,0));
gsl_vector_free(eval);
gsl_matrix_free(evec);
#endif
gsl_permutation *p = gsl_permutation_alloc(n);
gsl_matrix_memcpy( work, var );
gsl_linalg_LU_decomp( work, p, &s );
gsl_linalg_LU_invert( work, p, winv );
ax = gsl_linalg_LU_det( work, s );
gsl_matrix_free( work );
gsl_permutation_free( p );
xm = gsl_vector_alloc(n);
gsl_vector_memcpy( xm, x);
gsl_vector_sub( xm, mean );
ym = gsl_vector_alloc(n);
gsl_blas_dsymv(CblasUpper,1.0,winv,xm,0.0,ym);
gsl_matrix_free( winv );
gsl_blas_ddot( xm, ym, &ay);
gsl_vector_free(xm);
gsl_vector_free(ym);
/*
* ay = exp(-0.5*ay)/sqrt( pow((2*M_PI),n)*ax );
*/
ay = -0.5*( ay + n*log(2*M_PI) + log(ax) );
return ay;
}
