// determinant computed using LU-decomposition // runtime: O(n^3) static matrix_entry eff_det(matrix a) { // make a copy of a so that the entries are not affected by lu_decomp matrix *a_cpy = copy_matrix(a); int row_exchanges = 0; lu_decomp(a_cpy, &row_exchanges); // a is now upper triangular with same determinant up to sign matrix_entry result = alt(row_exchanges); // takes care of sign for (int i=0; i<a_cpy->m; i++) { result *= a_cpy->entries[i][i]; } free_matrix(a_cpy); return result; }
bool qbd_compute_pi0(const MatrixXd & R, const MatrixXd & B0, const MatrixXd & A0, RowVectorXd & pi0, const qbd_parms & parms) throw (Exc) { if(R.rows()!=R.cols()) EXC_PRINT("R had to be square"); if (!check_sizes(A0,R) || !check_sizes(A0,B0)) EXC_PRINT("A0, A1, A2 matrixes have to be square and equal size"); if ((R.minCoeff()<0)&&parms.verbose) cerr<<"QBD_COMPUTE_PI0: Warning: R has negative coeeficients"<<endl; SelfAdjointEigenSolver<MatrixXd> eigensolver(R); if (eigensolver.info() != Success) { if (parms.verbose) cerr<<"QBD_COMPUTE_PI0: cannot compute eigenvalues of R"<<endl; return false; } if ((ArrayXd(eigensolver.eigenvalues()).abs().maxCoeff()>1)&&parms.verbose) cerr<<"QBD_COMPUTE_PI0: Warning: R has spectral radius greater than 1"<<endl; int n = R.rows(); MatrixXd Id = MatrixXd::Identity(n,n); VectorXd u(n); u.setOnes(); MatrixXd M(n,n+1); M.block(0,0,n,n)= B0+R*A0-Id; M.block(0,n,n,1)= (Id-R).inverse()*u; FullPivLU<MatrixXd> lu_decomp(M); if(lu_decomp.rank()<n) { if (parms.verbose) cerr<<"QBD_COMPUTE_PI0: No unique solution"<<endl; return false; } RowVectorXd work(n+1); work.setZero(); work(n)=1; MatrixXd W1; pseudoInverse<MatrixXd>(M,W1); pi0 = work*W1; if ((pi0.minCoeff()<0)&&parms.verbose) cerr<<"QBD_COMPUTE_PI0: Warning: x0 has negative elements"<<endl; return true; }
LocalMatrix chemReductionGIA::orthcomp( LocalMatrix & inMat ) { // initialize it so that they have the same dimension LocalMatrix outMat; Eigen::FullPivLU<LocalMatrix> lu_decomp(inMat.transpose()); outMat = lu_decomp.kernel(); // notice that if the kernel returns a matrix with dimension zero, // then the returned matrix will be a column-vector filled with zeros // therefore we do a safety check here, and set the number of columns to zero if ( outMat.cols() == 1 && outMat.col(0).norm() == 0.0 ) outMat = LocalMatrix::Zero(inMat.rows(), 0); return outMat; }
// runtime: O(n^3) matrix *invert_matrix(matrix a) { assert(a.m == a.n); // only defined for square matrices // make a copy of a so as not to modify it matrix *u = copy_matrix(a); // perform lu decomposition matrix **pl = lu_decomp(u, NULL); matrix *u_inv = invert_upper_tri_matrix(*u); matrix *l_inv = invert_lower_tri_matrix(*pl[1]); matrix *lu_inv = matrix_mult(*u_inv, *l_inv); matrix *result = matrix_mult(*lu_inv, *pl[0]); free_matrix(u); free_matrix(pl[0]); free_matrix(pl[1]); free_matrix(u_inv); free_matrix(l_inv); free_matrix(lu_inv); free(pl); return result; }
/** * @function testPPP */ void Tensor3D::testPPP() { Eigen::MatrixXd stacked = Eigen::MatrixXd::Zero( 9*mPPP.size(), 27); for( int i = 0; i < mPPP.size(); ++i ) { Eigen::MatrixXd block; block = getEq_PPP( mPPP[i] ); // Check rank Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(block); int rank = lu_decomp.rank(); printf("Rank of block %d is %d \n", i, rank); stacked.block( 9*i,0, 9,27 ) = block; // Stack Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp2(stacked); int rank2 = lu_decomp2.rank(); printf("Rank of blocks stacked so far (until %d) is %d \n", i, rank2 ); } }
bool Line::GeometryLineInter(Vector3f x1, Vector3f x2, Vector3f y1, Vector3f y2) { int i = 0; Matrix3f k; if ((x1 - y1).norm() < MINFLOATDIF) { return false; } if ((x1 - y2).norm() < MINFLOATDIF) { return false; } if ((x2 - y1).norm() < MINFLOATDIF) { return false; } if ((x2 - y2).norm() < MINFLOATDIF) { return false; } for (i = 0; i < 3; i++) { k(i, 0) = x1(i) - x2(i); k(i, 1) = -(y1(i) - y2(i)); k(i, 2) = x2(i) - y2(i); } FullPivLU<Matrix3f> lu_decomp(k); auto rank = lu_decomp.rank(); if (rank == 3) { return false; } else { return true; } }
void DIIS::update_diis_coefficients(){ /** General case - holds true for both N_diis<N_diis_max and for N_diis==N_diis_max (after preliminary corrections ) Starting at this point we compute the extrapolation coefficients: Solving Ax = b, where b - are the errors, x - are the changes of the parameter space, A contain the extrapolation coefficients */ int N_diis_curr; int rank; int i,j; int debug_flag = 0; double diis_damp = 0.0; // see [Hamilton,Pulay, JCP 84, 5728 (1986) ] - scale diagonal element of B matrix by (1+diis_damp) to // avoid numerical problems associated with large diis coefficents MatrixXd* A; // will be of size N_diis+2 VectorXd* b; //(N_diis+2); VectorXd* x; //(N_diis+2); if(debug_flag){ cout<<"*diis_err[N_diis]= "<<*diis_err[N_diis]<<endl; } // Actual size of DIIS matrices at given call // We save this number, because N_diis can reduce in the following run, to give full-rank matrices N_diis_curr = N_diis; // Initial guess A = new MatrixXd(N_diis+2,N_diis+2); for(i=0;i<=N_diis;i++){ for(j=0;j<=N_diis;j++){ (*A)(i,j) = ((*diis_err[i]).T() * (*diis_err[j])).tr(); if(i==j){ (*A)(i,j) *= (1.0+diis_damp); } } (*A)(i,N_diis+1) = -1.0; (*A)(N_diis+1,i) = -1.0; } (*A)(N_diis+1,N_diis+1) = 0.0; // Determine the rank of the DIIS matrix A FullPivLU<MatrixXd> lu_decomp(*A); rank = lu_decomp.rank(); if(rank==(N_diis+2)){ if(debug_flag){ cout<<"Matrix is well-conditioned\n"<<*A<<endl; } } else{ if(debug_flag){ cout<<"Matrix is ill-conditioned\n"<<*A<<endl; } int min_indx = 0; // Reduce DIIS matrix by removing older iterates untill it is well-conditioned while(rank!=(N_diis+2)){ N_diis--; min_indx++; // this will hide first min_indx diis_err matrices from consideration MatrixXd tempA(N_diis+2,N_diis+2); for(i=0;i<=N_diis;i++){ for(j=0;j<=N_diis;j++){ tempA(i,j) = ((*diis_err[min_indx+i]).T() * (*diis_err[min_indx+j])).tr(); if(i==j){ tempA(i,j) *= (1.0+diis_damp); } } tempA(i,N_diis+1) = -1.0; tempA(N_diis+1,i) = -1.0; } tempA(N_diis+1,N_diis+1) = 0.0; FullPivLU<MatrixXd> tmp_lu_decomp(tempA); rank = tmp_lu_decomp.rank(); if(debug_flag){ cout<<"Reduction iteration "<<min_indx<<endl; cout<<"Reduced matrix = \n"<<tempA<<endl; cout<<"Reduced matrix rank = "<<rank<<endl; } if(rank==N_diis+2){ A = new MatrixXd(N_diis+2,N_diis+2); *A = tempA; } }// while }// else rank!=Ndiis+2 b = new VectorXd(N_diis+2); for(i=0;i<=N_diis;i++){ (*b)(i) = 0.0; } (*b)(N_diis+1) = -1.0; x = new VectorXd(N_diis+2); // Solve linear algebra to get coefficients *x = A->lu().solve(*b); for(i=0;i<=N_diis;i++){ diis_c[i] = (*x)[i]; } if(debug_flag){ cout<<"A*x = "<<(*A)*(*x)<<endl; cout<<"b = "<<*b<<endl; cout<<"x = "<<*x<<endl; } // Free memory delete A; delete b; delete x; // Compute effective size of the DIIS matrix N_diis_eff = (N_diis_curr - N_diis); // keep in mind that N_diis is a current one (updated) N_diis = N_diis_curr; }//void DIIS::update_diis_coefficients()
/* Return 1 if the matrix is singular, 0 if OK */ int solve_se( double **a, /* A[][] input matrix, returns LU decomposition of A */ double *b, /* B[] input array, returns solution X[] */ int n /* Dimensionality */ ) { double rip; /* Row interchange parity */ int *pivx, PIVX[10]; #if defined(DO_POLISH) || defined(DO_CHECK) double **sa; /* save input matrix values */ double *sb; /* save input vector values */ #endif if (n <= 10) pivx = PIVX; else pivx = ivector(0, n-1); #if defined(DO_POLISH) || defined(DO_CHECK) sa = dmatrix(0, n-1, 0, n-1); sb = dvector(0, n-1); /* Copy input matrix and vector values */ for (i = 0; i < n; i++) { sb[i] = b[i]; for (j = 0; j < n; j++) sa[i][j] = a[i][j]; } #endif if (lu_decomp(a, n, pivx, &rip)) { #if defined(DO_POLISH) || defined(DO_CHECK) free_dvector(sb, 0, n-1); free_dmatrix(sa, 0, n-1, 0, n-1); if (pivx != PIVX) free_ivector(pivx, 0, n-1); #endif return 1; } lu_backsub(a, n, pivx, b); #ifdef DO_POLISH lu_polish(n, sa, a, pivx, sb, b); /* Improve the solution */ #endif #ifdef DO_CHECK /* Check that the solution is correct */ for (i = 0; i < n; i++) { double sum, temp; sum = 0.0; for (j = 0; j < n; j++) sum += sa[i][j] * b[j]; temp = fabs(sum - sb[i]); if (temp > 1e-6) { free_dvector(sb, 0, n-1); free_dmatrix(sa, 0, n-1, 0, n-1); if (pivx != PIVX) free_ivector(pivx, 0, n-1); return 2; } } #endif #if defined(DO_POLISH) || defined(DO_CHECK) free_dvector(sb, 0, n-1); free_dmatrix(sa, 0, n-1, 0, n-1); #endif if (pivx != PIVX) free_ivector(pivx, 0, n-1); return 0; }
void marginalize( std::vector<aslam::backend::DesignVariable*>& inDesignVariables, std::vector<aslam::backend::ErrorTerm*>& inErrorTerms, int numberOfInputDesignVariablesToRemove, bool useMEstimator, boost::shared_ptr<aslam::backend::MarginalizationPriorErrorTerm>& outPriorErrorTermPtr, Eigen::MatrixXd& outCov, std::vector<aslam::backend::DesignVariable*>& outDesignVariablesInRTop, size_t numTopRowsInCov, size_t /* numThreads */) { SM_WARN_STREAM_COND(inDesignVariables.size() == 0, "Zero input design variables in the marginalizer!"); // check for duplicates! std::unordered_set<aslam::backend::DesignVariable*> inDvSetHT; for(auto it = inDesignVariables.begin(); it != inDesignVariables.end(); ++it) { auto ret = inDvSetHT.insert(*it); SM_ASSERT_TRUE(aslam::Exception, ret.second, "Error! Duplicate design variables in input list!"); } std::unordered_set<aslam::backend::ErrorTerm*> inEtSetHT; for(auto it = inErrorTerms.begin(); it != inErrorTerms.end(); ++it) { auto ret = inEtSetHT.insert(*it); SM_ASSERT_TRUE(aslam::Exception, ret.second, "Error! Duplicate error term in input list!"); } SM_DEBUG_STREAM("NO duplicates in input design variables or input error terms found."); // Partition the design varibles into removed/remaining. int dimOfDesignVariablesToRemove = 0; std::vector<aslam::backend::DesignVariable*> remainingDesignVariables; int k = 0; size_t dimOfDvsInTopBlock = 0; for(std::vector<aslam::backend::DesignVariable*>::const_iterator it = inDesignVariables.begin(); it != inDesignVariables.end(); ++it) { if (k < numberOfInputDesignVariablesToRemove) { dimOfDesignVariablesToRemove += (*it)->minimalDimensions(); } else { remainingDesignVariables.push_back(*it); } if(dimOfDvsInTopBlock < numTopRowsInCov) { outDesignVariablesInRTop.push_back(*it); } dimOfDvsInTopBlock += (*it)->minimalDimensions(); k++; } // store original block indices to prevent side effects std::vector<int> originalBlockIndices; std::vector<int> originalColumnBase; // assign block indices int columnBase = 0; for (size_t i = 0; i < inDesignVariables.size(); ++i) { originalBlockIndices.push_back(inDesignVariables[i]->blockIndex()); originalColumnBase.push_back(inDesignVariables[i]->columnBase()); inDesignVariables[i]->setBlockIndex(i); inDesignVariables[i]->setColumnBase(columnBase); columnBase += inDesignVariables[i]->minimalDimensions(); } int dim = 0; std::vector<size_t> originalRowBase; for(std::vector<aslam::backend::ErrorTerm*>::iterator it = inErrorTerms.begin(); it != inErrorTerms.end(); ++it) { originalRowBase.push_back((*it)->rowBase()); (*it)->setRowBase(dim); dim += (*it)->dimension(); } aslam::backend::DenseQrLinearSystemSolver qrSolver; qrSolver.initMatrixStructure(inDesignVariables, inErrorTerms, false); SM_INFO_STREAM("Marginalization optimization problem initialized with " << inDesignVariables.size() << " design variables and " << inErrorTerms.size() << " error terrms"); SM_INFO_STREAM("The Jacobian matrix is " << dim << " x " << columnBase); qrSolver.evaluateError(1, useMEstimator); qrSolver.buildSystem(1, useMEstimator); const Eigen::MatrixXd& jacobian = qrSolver.getJacobian(); const Eigen::VectorXd& b = qrSolver.e(); // check dimension of jacobian int jrows = jacobian.rows(); int jcols = jacobian.cols(); int dimOfRemainingDesignVariables = jcols - dimOfDesignVariablesToRemove; //int dimOfPriorErrorTerm = jrows; // check the rank Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(jacobian); //lu_decomp.setThreshold(1e-20); double threshold = lu_decomp.threshold(); int rank = lu_decomp.rank(); int fullRank = std::min(jacobian.rows(), jacobian.cols()); SM_DEBUG_STREAM("Rank of jacobian: " << rank << " (full rank: " << fullRank << ", threshold: " << threshold << ")"); bool rankDeficient = rank < fullRank; if(rankDeficient) { SM_WARN("Marginalization jacobian is rank deficient!"); } //SM_ASSERT_FALSE(aslam::Exception, rankDeficient, "Right now, we don't want the jacobian to be rank deficient - ever..."); Eigen::MatrixXd R_reduced; Eigen::VectorXd d_reduced; if (jrows < jcols) { SM_THROW(aslam::Exception, "underdetermined LSE!"); // underdetermined LSE, don't do QR R_reduced = jacobian.block(0, dimOfDesignVariablesToRemove, jrows, jcols - dimOfDesignVariablesToRemove); d_reduced = b; } else { // PTF: Do we know what will happen when the jacobian matrix is rank deficient? // MB: yes, bad things! // do QR decomposition sm::timing::Timer myTimer("QR Decomposition"); Eigen::HouseholderQR<Eigen::MatrixXd> qr(jacobian); Eigen::MatrixXd Q = qr.householderQ(); Eigen::MatrixXd R = qr.matrixQR().triangularView<Eigen::Upper>(); Eigen::VectorXd d = Q.transpose()*b; myTimer.stop(); if(numTopRowsInCov > 0) { sm::timing::Timer myTimer("Covariance computation"); Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(R); Eigen::MatrixXd Rinv = lu_decomp.inverse(); Eigen::MatrixXd covariance = Rinv * Rinv.transpose(); outCov = covariance.block(0, 0, numTopRowsInCov, numTopRowsInCov); myTimer.stop(); } // size_t numRowsToKeep = rank - dimOfDesignVariablesToRemove; // SM_ASSERT_TRUE_DBG(aslam::Exception, rankDeficient || (numRowsToKeep == dimOfRemainingDesignVariables), "must be the same if full rank!"); // get the top left block SM_ASSERT_GE(aslam::Exception, R.rows(), numTopRowsInCov, "Cannot extract " << numTopRowsInCov << " rows of R because it only has " << R.rows() << " rows."); SM_ASSERT_GE(aslam::Exception, R.cols(), numTopRowsInCov, "Cannot extract " << numTopRowsInCov << " cols of R because it only has " << R.cols() << " cols."); //outRtop = R.block(0, 0, numTopRowsInRtop, numTopRowsInRtop); // cut off the zero rows at the bottom R_reduced = R.block(dimOfDesignVariablesToRemove, dimOfDesignVariablesToRemove, dimOfRemainingDesignVariables, dimOfRemainingDesignVariables); //R_reduced = R.block(dimOfDesignVariablesToRemove, dimOfDesignVariablesToRemove, numRowsToKeep, dimOfRemainingDesignVariables); d_reduced = d.segment(dimOfDesignVariablesToRemove, dimOfRemainingDesignVariables); //d_reduced = d.segment(dimOfDesignVariablesToRemove, numRowsToKeep); //dimOfPriorErrorTerm = dimOfRemainingDesignVariables; } // now create the new error term boost::shared_ptr<aslam::backend::MarginalizationPriorErrorTerm> err(new aslam::backend::MarginalizationPriorErrorTerm(remainingDesignVariables, d_reduced, R_reduced)); outPriorErrorTermPtr.swap(err); // restore initial block indices to prevent side effects for (size_t i = 0; i < inDesignVariables.size(); ++i) { inDesignVariables[i]->setBlockIndex(originalBlockIndices[i]); inDesignVariables[i]->setColumnBase(originalColumnBase[i]); } int index = 0; for(std::vector<aslam::backend::ErrorTerm*>::iterator it = inErrorTerms.begin(); it != inErrorTerms.end(); ++it) { (*it)->setRowBase(originalRowBase[index++]); } }